simplecyesim.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

/*
 * SimpleCyeSim.java
 */

package EDU.cmu.cs.coral.abstractrobot;

import java.awt.*;
import java.lang.Math;
import java.util.Enumeration;
import java.util.*;
import EDU.gatech.cc.is.util.*;
import EDU.gatech.cc.is.simulation.*;
import EDU.gatech.cc.is.communication.*;
import EDU.cmu.cs.coral.simulation.*;
import EDU.cmu.cs.coral.abstractrobot.*;
import EDU.gatech.cc.is.abstractrobot.*;
import EDU.cmu.cs.coral.util.Polygon2;
import EDU.cmu.cs.coral.util.Circle2;
import EDU.cmu.cs.coral.localize.GaussianSampler;

/**
 * SimpleCyeSim implements SimpleCye for simulation.
 * Also includes code implementing communication and
 * vision.
 * <P>
 * <A HREF="../COPYRIGHT.html">Copyright</A>
 * (c)1999,2000 CMU
 *
 * @author Rosemary Emery
 * @version $Revision: 1.10 $
 */

public class SimpleCyeSim extends Simple
    implements SimpleCye, SimulatedObject
	{
	private	CircularBuffer	trail;	// robot's trail
	private KinSensorSim	kin_sensor;	// senses our kin
	private TransceiverSim	transceiver;	// comm to other robots
	protected Vec2	position;
	protected Vec2	steer;
	private	double	speed;

	protected Vec2 trailer_steer;
	protected Vec2 trailer_position;

	  protected Vec2 mvstep; //the amount the front moves in 1 timestep

	protected Color	foreground, background;
	private long	time;
	private double	timed;
	protected double left, right, top, bottom;
	protected	SimulatedObject[] all_objects = new SimulatedObject[0];
	private	int	visionclass;
	public	static final boolean DEBUG = false;
	protected Polygon2 cyeBody;
	protected Polygon2 trailer;
	protected boolean CanTurn = true;

	  protected GaussianSampler visionNoiseGen; //this generates noise for us...
	  protected double visionNoiseStddev;// standard deviation of the noise....
	  protected double visionNoiseMean; //mean of thenoise (usually 0)

	/**
	 * Instantiate a <B>SimpleCyeSim</B> object.  Be sure
	 * to also call init with proper values.
	 * @see SimpleCyeSim#init
	 */
        public SimpleCyeSim()
		{
		/*--- set parameters ---*/
                super(1);

		position = new Vec2(0,0);
		steer = new Vec2(1,0);
		trailer_steer = new Vec2(1,0);
		trailer_position = new Vec2(1,0);
		updatePolys(position, trailer_position);
		foreground = Color.black;
		background = Color.black;
		if (DEBUG) System.out.println("SimpleCyeSim: instantiated.");

		//set the noise maker to initially be no noise
		visionNoiseStddev = 0.0;
		visionNoiseMean = 0.0;
		visionNoiseGen = new GaussianSampler(1,31337); //one variable and seed value
		

		/*--- set default bounds ---*/
		top = 1000;
		bottom = -1000;
		left = -1000;
		right = 1000;
		}	


        /**
         * Initialize a <B>SimpleCySim</B> object.
         */
	public void init(double xp, double yp, double tp, double ignore,
		Color f, Color b, int v, int i, long s)
		{
		trail = new CircularBuffer(1000);
		setID(i);
                kin_sensor = new KinSensorSim(this);
		transceiver = new TransceiverSim(this, this);
		position = new Vec2(xp,yp);
		steer = new Vec2(1,0);
		steer.sett(tp);
		trailer_steer = new Vec2(1,0);
		trailer_steer.sett(tp); // initial same heading as front of cye robot
		trailer_position = new Vec2(xp,yp); // initial same position as robot
		mvstep = new Vec2(0,0);
		
		updatePolys(position, trailer_position);
		foreground = f;
		background = b;
		time = 0;
		timed = 0;
		visionclass = v;
		if (DEBUG) System.out.println("SimpleCyeSim: initialized"
			+" at "+xp+","+yp);
		}


	/**
	 * Take a simulated step;
	 */
	private double last_traversability = 1.0;
	public void takeStep(long time_increment, SimulatedObject[] all_objs)
		{
		if (DEBUG) System.out.println("SimpleCyeSim.TakeStep()");

		/*--- keep pointer to the other objects ---*/
		all_objects = all_objs;

		/*--- update the time ---*/
		time += time_increment;
		double time_incd = ((double)time_increment)/1000;
		timed += time_incd;

		/*--- compute left and right wheel speeds ---*/

		double right_wheel_vel = 0.0;
		double left_wheel_vel = 0.0;		

		/*--- figuring out position relative to where what to be heading ---*/
		/*--- i.e. controller ---*/

		double temp_heading = desired_heading;
		double temp_steer_t = steer.t;

		double heading_difference = temp_heading - temp_steer_t;

		/*--- fix heading difference so that lies between +/- PI ---*/

		if (heading_difference > Math.PI)
			{
			heading_difference -= 2*Math.PI;
			}
		else if (heading_difference < -1*Math.PI)
			{
			heading_difference += 2*Math.PI;
			}

		double angle_between_trailer_and_front = steer.t - trailer_steer.t;

		if (angle_between_trailer_and_front > Math.PI)
			{
			angle_between_trailer_and_front -= 2*Math.PI;
			}
		else if (angle_between_trailer_and_front < -1*Math.PI)
			{
			angle_between_trailer_and_front += 2*Math.PI;
			}

		// now for controller

		// proportional control
		double minNonWrap = 0.0*desired_speed; // minimum for non cord wrapping case
		double minWrap = 0.15*desired_speed; // minimum for cord wrapping case
		double proportion = 0.0; // what will use to determine speed
		
		if (desired_speed < 0.0)
		  {
                        if (canTurn(angle_between_trailer_and_front, heading_difference))
                            {
                                right_wheel_vel = base_speed*desired_speed;
				left_wheel_vel = base_speed*desired_speed;
                            }
                        else
			  {
			    right_wheel_vel = 0.0;
			    left_wheel_vel = 0.0;
			  }

                    }
		
		else if(steer.t == desired_heading) 
			{
			// if heading in direction want to be in keep on going
			right_wheel_vel = desired_speed*base_speed;
			left_wheel_vel = desired_speed*base_speed;
			}
		else if(desired_speed == 0.0) 
			{
			
			// call after above so will stop turning if headed in the right direction
			if (canTurn(angle_between_trailer_and_front, heading_difference))
				{
				right_wheel_vel = dsignum(heading_difference)*0.25*base_speed;
				left_wheel_vel = -1.0*right_wheel_vel;
				}
			else 
				{
				right_wheel_vel = 0.0;
				left_wheel_vel = 0.0;
				}
			}
		else if(canTurn(angle_between_trailer_and_front, heading_difference)) 
			{
			if (Math.abs(heading_difference) >= (Math.PI)/2 )
			{
				if (Math.abs(heading_difference) < Math.PI)
					{
					right_wheel_vel = dsignum(heading_difference)*desired_speed*base_speed;
					}
				else
					{
					right_wheel_vel = dsignum(heading_difference)*desired_speed*base_speed*-1;	
					}
				left_wheel_vel = -1*right_wheel_vel;
				}
			else 
				{
//				proportion = ((minNonWrap-desired_speed)/(Math.PI/2))*Math.abs(heading_difference) + desired_speed;
				proportion = (-4*(minNonWrap-desired_speed)/Math.pow(Math.PI,2))*Math.pow(Math.abs(heading_difference),2) + (4*(minNonWrap-desired_speed)/Math.PI)*Math.abs(heading_difference)+ desired_speed;
//				proportion = 0.0;
				if (dsignum(heading_difference) < 0.0)
					{
					right_wheel_vel = base_speed*proportion;
					left_wheel_vel = base_speed*desired_speed;
					}
				else 
					{
					right_wheel_vel = base_speed*desired_speed;
					left_wheel_vel = base_speed*proportion;
					}
				}
			}
		else // cannot turn in direction i want to go or i will wrap cord
			{
//			proportion = ((minWrap-desired_speed)/(Math.PI/2))*Math.abs(heading_difference) + desired_speed;
			proportion = (-4*(minWrap-desired_speed)/Math.pow(Math.PI,2))*Math.pow(Math.abs(heading_difference),2) + (4*(minWrap-desired_speed)/Math.PI)*Math.abs(heading_difference)+ desired_speed;
//			proportion = 0.175*desired_speed;
			if (dsignum(heading_difference) < 0.0)
				{
				right_wheel_vel = proportion*base_speed;
				left_wheel_vel = base_speed*desired_speed;
				}
			else 
				{
				right_wheel_vel = base_speed*desired_speed;
				left_wheel_vel = proportion*base_speed;
				}
			}


		/*--- set distance each wheel traveled in this time step ---*/

		double delta_distance_right = (right_wheel_vel*time_incd);
		double delta_distance_left = (left_wheel_vel*time_incd);

		/*--- set heading and velocity based on right and left wheel velocities ---*/
		double delta_phi = (delta_distance_right-delta_distance_left)/LENGTH; // sign taken into account
		steer.sett(steer.t + delta_phi);

		Vec2 delta_displacement = new Vec2(steer.x, steer.y);
		delta_displacement.setx(((delta_distance_right+delta_distance_left)/2)*Math.cos(steer.t));
		delta_displacement.sety(((delta_distance_right+delta_distance_left)/2)*Math.sin(steer.t));
		
		/*--- now compute the location of the trailer ---*/
		/*--- first the new heading of the trailer ---*/

		double trailer_delta_phi = -1*sgn(desired_speed)*delta_displacement.r*Math.sin(-steer.t + trailer_steer.t)/HITCH_TO_TRAILER_WHEEL;
		trailer_steer.sett(trailer_steer.t + trailer_delta_phi);

		/*--- now the new position of the trailer ---*/

		Vec2 delta_trailer_displacement = new Vec2(trailer_steer.x, trailer_steer.y);
		delta_trailer_displacement.setx(sgn(desired_speed)*delta_displacement.r*Math.cos(steer.t));
		delta_trailer_displacement.sety(sgn(desired_speed)*delta_displacement.r*Math.sin(steer.t));

		// note: position of trailer is relative to its hitch		

		Vec2 trailer_mvstep = new Vec2(delta_trailer_displacement.x, delta_trailer_displacement.y);

		/*--- compute a movement step ---*/
		mvstep = new Vec2(delta_displacement.x, delta_displacement.y);
		Vec2 velocity = new Vec2(steer.x, steer.y);

		if (time_incd == 0.0) 
			{
			velocity.setr(0);
			} 
		else 
			{
			velocity.setr(delta_displacement.r/time_incd);
			}

		/*--- test the new position to see if in bounds ---*/
		// note: this test must be completely rewritten in order to account for
		// configuration space of cye robot - it is _not_ a sphere!!!!
		// note:  add in check to see if trailer in bounds

		Vec2 pp = new Vec2(position.x, position.y);
		Vec2 trailer_pp = new Vec2(trailer_position.x, trailer_position.y);

// ensure not colliding with edge of boundary

		pp.add(mvstep);

//		System.out.println("position " + pp.x + " " + pp.y + " " + time);
		if ((pp.x+RADIUS > right)||(pp.x-RADIUS < left))
			{
			velocity.setr(0);
			mvstep.setr(0);
			trailer_mvstep.setr(0);
			}
		if ((pp.y+RADIUS > top)||(pp.y-RADIUS < bottom))
			{
			velocity.setr(0);
			mvstep.setr(0);
			trailer_mvstep.setr(0);
			}

		/*--- test the new position to see if on top of obstacle ---*/
		// first check main body

		boolean moveok = true;
		last_traversability = 1.0;
		trailer_pp.add(trailer_mvstep);


		updatePolys(pp, trailer_pp);

		for (int i=0; i<all_objects.length; i++)
			{
			if (all_objects[i].isObstacle() && 
				(all_objects[i].getID() != unique_id))
				{
				Vec2 tmp = all_objects[i].getClosestPoint(pp);
				if (tmp.r < RADIUS)
					// only perform more time consuming check is object within RADIUS of centre of robot
					{
					if(all_objects[i].checkCollision(cyeBody))
						{
						moveok = false;
						break;
						}
					else if(all_objects[i].checkCollision(trailer))
						{
						moveok = false;
						break;
						}
					}
				}
			else if (all_objects[i] instanceof 
				SimulatedTerrainObject)
				{
				Vec2 tmp = all_objects[i].getClosestPoint(pp);
				if (tmp.r == 0) // on/in object
					last_traversability =
				((SimulatedTerrainObject)all_objects[i]).getTraversability();
				}
			}

		if (moveok) 
			{
			position.add(mvstep);
			trailer_position.add(trailer_mvstep);
			}

		else  // move is not okay - take back orientation change
			{
			steer.sett(steer.t - delta_phi);
			trailer_steer.sett(trailer_steer.t - trailer_delta_phi);
			updatePolys(position, trailer_position);
			}
	
		/*--- test the new position to see if on top of pushable ---*/
		for (int i=0; i<all_objects.length; i++)
			{
			if (all_objects[i].isPushable() && 
				(all_objects[i].getID() != unique_id))
				{
				Vec2 tmp = all_objects[i].getClosestPoint(position);
				if (tmp.r < RADIUS)
					// only perform more time consuming check is object within RADIUS of centre of robot
					{
					if(all_objects[i].checkCollision(cyeBody))
						{
						  // push based on closest point
						  all_objects[i].push(tmp, velocity);
						}
					else if(all_objects[i].checkCollision(trailer))
						{
						  tmp = new Vec2(all_objects[i].getClosestPoint(trailer_position));
						  all_objects[i].push(tmp, velocity);
						}
					}
				}
			}
	}

	boolean canTurn(double angle_between_trailer_and_front, double heading_difference)
		{
		if ((Math.abs(angle_between_trailer_and_front ) <= (Math.PI)/2) || 
			(angle_between_trailer_and_front <= -1*Math.PI/2 && heading_difference > 0.0)
			|| (angle_between_trailer_and_front >= Math.PI/2 && heading_difference < 0.0))
			{
			  CanTurn = true;
				return true;
			}
		else 
			{
			  CanTurn = false;
			return false;
			}
		}			

	public boolean getCommandError(long timestamp)
           {
	     if (CanTurn)
	       return false;
	     else
	       return true;
	   }

        public double getVoltage(long timestamp)
           {
	     return 12.0;
	   }

	void updatePolys(Vec2 pp, Vec2 trailer_pp)
		{		
		Vec2[] body = new Vec2[4];     // outline of body
		Vec2 vertex;
		double[] bodyx = new double[4];
		double[] bodyy = new double[4];
		double newAng;
		double xpix = pp.x;
		double ypix = pp.y;
		body[0] = new Vec2(WIDTH/2, LENGTH/2); 
		body[3] = new Vec2(-1*WIDTH/2, LENGTH/2);
		body[2] = new Vec2(-1*WIDTH/2, -1*LENGTH/2);
		body[1] = new Vec2(WIDTH/2,  -1*LENGTH/2);
		
		int j,k;
		for(j = 0; j<4; j++) // scale and rotate
			{
			newAng = body[j].t + steer.t;
			if (newAng >= 2*Math.PI)
				{
				newAng-= 2*Math.PI;
				}
			body[j].sett(newAng);
			bodyx[j] = body[j].x + xpix;