harrassblue.java

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

		//================
		// wander anywhere
		v_StaticWeightedSum_va 
		AS_WANDER = new v_StaticWeightedSum_va();

		AS_WANDER.weights[0]  = 1.0;
		AS_WANDER.embedded[0] = MS_AVOID_OBSTACLES;

		AS_WANDER.weights[1]  = 1.0;
		AS_WANDER.embedded[1] = MS_NOISE_VECTOR;

		AS_WANDER.weights[2]  = 1.0;
		AS_WANDER.embedded[2] = MS_SWIRL_OBSTACLES_NOISE;

		AS_WANDER.weights[3]  = 2.0;
		AS_WANDER.embedded[3] = MS_AVOID_HOME_ZONE;

		AS_WANDER.weights[4]  = 2.0;
		AS_WANDER.embedded[4] = MS_MOVE_TO_RED_BIN;


		//================
		// AS_WANDER_IN_HOME_ZONE
		//================
		// wander only in the home zone
		v_StaticWeightedSum_va 
		AS_WANDER_IN_HOME_ZONE = new v_StaticWeightedSum_va();

		AS_WANDER_IN_HOME_ZONE.weights[0]  = 1.0;
		AS_WANDER_IN_HOME_ZONE.embedded[0] = MS_AVOID_OBSTACLES;

		AS_WANDER_IN_HOME_ZONE.weights[1]  = 1.0;
		AS_WANDER_IN_HOME_ZONE.embedded[1] = MS_NOISE_VECTOR;

		AS_WANDER_IN_HOME_ZONE.weights[2]  = 1.0;
		AS_WANDER_IN_HOME_ZONE.embedded[2] = MS_SWIRL_OBSTACLES_NOISE;

		AS_WANDER_IN_HOME_ZONE.weights[3]  = 1.0;
		AS_WANDER_IN_HOME_ZONE.embedded[3] = MS_AVOID_HOMEBASE;

		AS_WANDER_IN_HOME_ZONE.weights[4]  = 1.0;
		AS_WANDER_IN_HOME_ZONE.embedded[4] = 
					MS_STAY_IN_HOME_ZONE;


		//================
		// AS_WANDER_WEST
		//================
		// wander only in the west
		v_StaticWeightedSum_va 
		AS_WANDER_WEST = new v_StaticWeightedSum_va();

		AS_WANDER_WEST.weights[0]  = 1.0;
		AS_WANDER_WEST.embedded[0] = MS_AVOID_OBSTACLES;

		AS_WANDER_WEST.weights[1]  = 1.0;
		AS_WANDER_WEST.embedded[1] = MS_NOISE_VECTOR;

		AS_WANDER_WEST.weights[2]  = 1.0;
		AS_WANDER_WEST.embedded[2] = MS_SWIRL_OBSTACLES_NOISE;

		AS_WANDER_WEST.weights[3]  = 1.0;
		AS_WANDER_WEST.embedded[3] = MS_AVOID_HOME_ZONE;

		AS_WANDER_WEST.weights[4]  = 1.0;
		AS_WANDER_WEST.embedded[4] = 
					MS_STAY_IN_WEST_TERRITORY;


		//================
		// AS_WANDER_EAST
		//================
		// wander only in the east
		v_StaticWeightedSum_va 
		AS_WANDER_EAST = new v_StaticWeightedSum_va();

		AS_WANDER_EAST.weights[0]  = 1.0;
		AS_WANDER_EAST.embedded[0] = MS_AVOID_OBSTACLES;

		AS_WANDER_EAST.weights[1]  = 1.0;
		AS_WANDER_EAST.embedded[1] = MS_NOISE_VECTOR;

		AS_WANDER_EAST.weights[2]  = 1.0;
		AS_WANDER_EAST.embedded[2] = MS_SWIRL_OBSTACLES_NOISE;

		AS_WANDER_EAST.weights[3]  = 1.0;
		AS_WANDER_EAST.embedded[3] = MS_AVOID_HOME_ZONE;

		AS_WANDER_EAST.weights[4]  = 1.0;
		AS_WANDER_EAST.embedded[4] = 
					MS_STAY_IN_EAST_TERRITORY;


		//================
		// AS_ACQUIRE_RED
		//================
		// go to a red thing
		v_StaticWeightedSum_va 
		AS_ACQUIRE_RED = new v_StaticWeightedSum_va();

		AS_ACQUIRE_RED.weights[0]  = 1.0;
		AS_ACQUIRE_RED.embedded[0] = MS_AVOID_OBSTACLES;

		AS_ACQUIRE_RED.weights[1]  = 1.0;
		AS_ACQUIRE_RED.embedded[1] = MS_MOVE_TO_RED;

		AS_ACQUIRE_RED.weights[2]  = 1.0;
		AS_ACQUIRE_RED.embedded[2] = MS_SWIRL_OBSTACLES_RED;

		AS_ACQUIRE_RED.weights[3]  = 0.2;
		AS_ACQUIRE_RED.embedded[3] = MS_NOISE_VECTOR;

		AS_ACQUIRE_RED.weights[4]  = 1.0;
		AS_ACQUIRE_RED.embedded[4] = MS_AVOID_HOMEBASE;


		//================
		// AS_ACQUIRE_BLUE
		//================
		// go to a blue thing
		v_StaticWeightedSum_va 
		AS_ACQUIRE_BLUE = new v_StaticWeightedSum_va();

		AS_ACQUIRE_BLUE.weights[0]  = 0.7;
		AS_ACQUIRE_BLUE.embedded[0] = MS_AVOID_OBSTACLES;

		AS_ACQUIRE_BLUE.weights[1]  = 1.7;
		AS_ACQUIRE_BLUE.embedded[1] = MS_MOVE_TO_BLUE;

		AS_ACQUIRE_BLUE.weights[2]  = 1.5;
		AS_ACQUIRE_BLUE.embedded[2] = MS_MOVE_TO_RED_BIN;


		//================
		// AS_DELIVER_RED
		//================
		// head for the red bin
		v_StaticWeightedSum_va 
		AS_DELIVER_RED = new v_StaticWeightedSum_va();

		AS_DELIVER_RED.weights[0]  = 1.0;
		AS_DELIVER_RED.embedded[0] = MS_AVOID_OBSTACLES;

		AS_DELIVER_RED.weights[1]  = 1.0;
		AS_DELIVER_RED.embedded[1] = MS_MOVE_TO_RED_BIN;

		AS_DELIVER_RED.weights[2]  = 1.0;
		AS_DELIVER_RED.embedded[2] = MS_SWIRL_OBSTACLES_HOMEBASE;

		AS_DELIVER_RED.weights[3]  = 0.2;
		AS_DELIVER_RED.embedded[3] = MS_NOISE_VECTOR;

		AS_DELIVER_RED.weights[4]  = 1.0;
		AS_DELIVER_RED.embedded[4] = MS_AVOID_BLUE_BIN;

		AS_DELIVER_RED.weights[5]  = 1.0;
		AS_DELIVER_RED.embedded[5] = MS_AVOID_HOMEBASE;


		//================
		// AS_DELIVER_BLUE
		//================
		// head for the blue bin
		v_StaticWeightedSum_va 
		AS_DELIVER_BLUE = new v_StaticWeightedSum_va();

		AS_DELIVER_BLUE.weights[0]  = 1.0;
		AS_DELIVER_BLUE.embedded[0] = MS_AVOID_OBSTACLES;

		AS_DELIVER_BLUE.weights[1]  = 1.0;
		AS_DELIVER_BLUE.embedded[1] = MS_MOVE_TO_BLUE_BIN;

		AS_DELIVER_BLUE.weights[2]  = 1.0;
		AS_DELIVER_BLUE.embedded[2] = MS_SWIRL_OBSTACLES_HOMEBASE;

		AS_DELIVER_BLUE.weights[3]  = 0.2;
		AS_DELIVER_BLUE.embedded[3] = MS_NOISE_VECTOR;

		AS_DELIVER_BLUE.weights[4]  = 1.0;
		AS_DELIVER_BLUE.embedded[4] = MS_AVOID_RED_BIN;

		AS_DELIVER_BLUE.weights[5]  = 1.0;
		AS_DELIVER_BLUE.embedded[5] = MS_AVOID_HOMEBASE;


                //======
                // STATE AND ACTION
                //======
                i_FSA_ba
                STATE_MACHINE = new i_FSA_ba();
                state_monitor = STATE_MACHINE;

                STATE_MACHINE.state = 0;

                // WANDER
                //STATE_MACHINE.triggers[WANDER][0]     = PF_RED_VISIBLE;
                STATE_MACHINE.triggers[WANDER][0]       = PF_BLUE_VISIBLE;
                STATE_MACHINE.follow_on[WANDER][0]      = ACQUIRE_BLUE;
                //STATE_MACHINE.triggers[WANDER][1]       = PF_BLUE_VISIBLE;
                //STATE_MACHINE.triggers[WANDER][1]     = PF_BLUE_VISIBLE_OUT_HZ;
                //STATE_MACHINE.follow_on[WANDER][1]      = ACQUIRE_BLUE;

                // ACQUIRE_RED
                STATE_MACHINE.triggers[ACQUIRE_RED][0]  = PF_RED_IN_GRIPPER;
                STATE_MACHINE.follow_on[ACQUIRE_RED][0] = WANDER;
                STATE_MACHINE.triggers[ACQUIRE_RED][1]  = PF_NOT_RED_VISIBLE;
                STATE_MACHINE.follow_on[ACQUIRE_RED][1] = WANDER;
                STATE_MACHINE.triggers[ACQUIRE_RED][2]  = PF_BLUE_IN_GRIPPER;
                STATE_MACHINE.follow_on[ACQUIRE_RED][2] = DELIVER_BLUE;

                // DELIVER_RED
                //STATE_MACHINE.triggers[DELIVER_RED][0] = PF_CLOSE_TO_RED_BIN;
                STATE_MACHINE.triggers[DELIVER_RED][0] = PF_CLOSE_TO_RED_BIN;
                STATE_MACHINE.follow_on[DELIVER_RED][0] = WANDER;

                // ACQUIRE_BLUE
                STATE_MACHINE.triggers[ACQUIRE_BLUE][0] = PF_BLUE_IN_GRIPPER;
                STATE_MACHINE.follow_on[ACQUIRE_BLUE][0]= DELIVER_BLUE;
                STATE_MACHINE.triggers[ACQUIRE_BLUE][1] = PF_NOT_BLUE_VISIBLE;
                STATE_MACHINE.follow_on[ACQUIRE_BLUE][1]= WANDER;
                STATE_MACHINE.triggers[ACQUIRE_BLUE][2] = PF_RED_IN_GRIPPER;
                STATE_MACHINE.follow_on[ACQUIRE_BLUE][2]= DELIVER_RED;

                // DELIVER_BLUE
                STATE_MACHINE.triggers[DELIVER_BLUE][0] = PF_CLOSE_TO_BLUE_BIN;
                STATE_MACHINE.follow_on[DELIVER_BLUE][0]= WANDER;

                // DROP
                //STATE_MACHINE.triggers[DROP][0]
                //                       = PF_DROP_CRITERIA;
                //STATE_MACHINE.follow_on[DROP][0]        = WANDER;

		action_monitor = STATE_MACHINE;

                //reward
                d_ForageReward_bbbb
                REWARD = new d_ForageReward_bbbb(
                        PF_CLOSE_TO_RED_BIN,
                        PF_RED_IN_GRIPPER,
                        PF_CLOSE_TO_BLUE_BIN,
                        PF_BLUE_IN_GRIPPER);
                reward_monitor = REWARD;

		//================
		// STEERING
		//================
		v_Select_vai
		//STEERING = new v_Select_vai((NodeInt)LEARNED_ACTION);
		STEERING = new v_Select_vai((NodeInt)STATE_MACHINE);

		STEERING.embedded[0] 	= AS_WANDER;
		STEERING.embedded[1]	= AS_ACQUIRE_RED;
		STEERING.embedded[2]	= AS_ACQUIRE_BLUE;
		STEERING.embedded[3]	= AS_DELIVER_RED;
		STEERING.embedded[4]	= AS_DELIVER_BLUE;
		STEERING.embedded[5]	= AS_WANDER_IN_HOME_ZONE;
		//STEERING.embedded[2]	= AS_WANDER_WEST;
		//STEERING.embedded[3]	= AS_WANDER_EAST;
		//STEERING.embedded[8]	= MS_AVOID_HOME_ZONE;


		//================
		// TURRET
		//================
		v_Select_vai
		//TURRET = new v_Select_vai((NodeInt)LEARNED_ACTION);
                TURRET = new v_Select_vai((NodeInt)STATE_MACHINE);

		TURRET.embedded[0] 	= AS_WANDER;
		TURRET.embedded[1]	= MS_MOVE_TO_RED;
		TURRET.embedded[2]	= MS_MOVE_TO_BLUE;
		TURRET.embedded[3]	= PS_HOMEBASE; // always face home
		TURRET.embedded[4]	= PS_HOMEBASE; // always face home
		TURRET.embedded[5]	= AS_WANDER_IN_HOME_ZONE;
		//TURRET.embedded[8]	= MS_AVOID_HOME_ZONE;
		//TURRET.embedded[1]	= AS_WANDER_IN_HOME_ZONE;
		//TURRET.embedded[2]	= AS_WANDER_WEST;
		//TURRET.embedded[3]	= AS_WANDER_EAST;



		//================
		// GRIPPER_FINGERS
		//================
		d_Select_i
		//GRIPPER_FINGERS = new d_Select_i(LEARNED_ACTION);
                GRIPPER_FINGERS = new d_Select_i(STATE_MACHINE);

		GRIPPER_FINGERS.embedded[0] 	= 1;  // open 
		GRIPPER_FINGERS.embedded[1]	= -1; // trigger for acq 
		GRIPPER_FINGERS.embedded[2]	= -1; // trigger for acq
		GRIPPER_FINGERS.embedded[3]	= 0;  // closed for deliver 
		GRIPPER_FINGERS.embedded[4]	= 0;  // closed for deliver
		//GRIPPER_FINGERS.embedded[5]	= -1; // trigger for acquire
		//GRIPPER_FINGERS.embedded[6]	= 0;  // closed 
		//GRIPPER_FINGERS.embedded[7]	= 0;  // closed 
		//GRIPPER_FINGERS.embedded[8]	= 1;  // open 


		steering_configuration = STEERING;
		turret_configuration = TURRET;
		gripper_fingers_configuration = GRIPPER_FINGERS;
		}
		
	/**
	 * Called every timestep to allow the control system to
	 * run.
	 */
	public int takeStep()
		{
		Vec2	result;
		double	dresult;
		long	curr_time = abstract_robot.getTime();
		Vec2	p;

		// STEER
		result = steering_configuration.Value(curr_time);
		abstract_robot.setSteerHeading(curr_time, result.t);
		abstract_robot.setSpeed(curr_time, result.r);

		// TURRET
		result = turret_configuration.Value(curr_time);
		abstract_robot.setTurretHeading(curr_time, result.t);

		// FINGERS
		dresult = gripper_fingers_configuration.Value(curr_time);
		abstract_robot.setGripperFingers(curr_time, dresult);

		// DISPLAY the state
		int state = state_monitor.Value(curr_time);
		int action = action_monitor.Value(curr_time);
		String msg = "blank";
		if (action == WANDER)
			msg = "AS_WANDER";
		else if (action == ACQUIRE_RED)
			msg = "AS_ACQUIRE_RED";
		else if (action == ACQUIRE_BLUE)
			msg = "HARRASS";
		else if (action == DELIVER_RED)
			msg = "AS_DELIVER_RED";
		else if (action == DELIVER_BLUE)
			msg = "AS_DELIVER_BLUE";
		else if (action == WANDER_IN_HOME_ZONE)
			msg = "AS_WANDER_IN_HOME_ZONE";
		else if (action == DROP)
			msg = "DROP";
		msg = msg.concat(String.valueOf(state));
		abstract_robot.setDisplayString(msg);

		// Check for reward
		double reward = reward_monitor.Value(curr_time);
		//System.out.println(abstract_robot.getPlayerNumber(curr_time));
		if (reward > 0) 
			{
			System.out.println("Reward for robot "+
				abstract_robot.getPlayerNumber(curr_time));
			num_delivered++;
			}

		return(CSSTAT_OK);
		}

	public void quit()
		{
		System.out.println("quit()");
		}
	public void trialInit()
		{
		System.out.println("trialInit()");
		}
	public void trialEnd()
		{
		System.out.println("trialEnd()");

		/*--- compose the log string ---*/
		String logmsg = num_delivered + " "
				+ 0 + " "
				+ 0 + " "
				+ 0 + "\n";
		System.out.print(logmsg);
		
		/*--- write the log string to a file ---*/
		RandomAccessFile rf;
		try
			{
			rf = new RandomAccessFile(log_filename, "rw");
			rf.seek(rf.length()); // go to end
			rf.writeBytes(logmsg);
			rf.close();
			}
		catch (Exception e)
			{
			System.out.println(e + " can't log to "+log_filename);
			}
		}
	}