harrassblue.java

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

/*
 * harrassblue.java
 */

package Domains.CTF;

import	java.io.*;
import	EDU.gatech.cc.is.util.Vec2;
import	EDU.gatech.cc.is.abstractrobot.*;
import	EDU.gatech.cc.is.clay.*;
import	EDU.gatech.cc.is.learning.*;

/**
 * <B>Introduction</B><BR>
 * <P>
 * For detailed information on how to configure behaviors, see the
 * <A HREF="../clay/docs/index.html">Clay page</A>.
 * <P>
 * <A HREF="../COPYRIGHT.html">Copyright</A>
 * (c)1997 Georgia Tech Research Corporation
 *
 * @author Tucker Balch
 * @version $Revision: 1.2 $
 */

public class harrassblue  extends ControlSystemCapture
	{
	final int WANDER = 0;
	final int ACQUIRE_RED = 1;
	final int ACQUIRE_BLUE = 2;
	final int DELIVER_RED = 3;
	final int DELIVER_BLUE = 4;
	final int DELIVER = 5;
	final int DROP = 6;
	final int WANDER_IN_HOME_ZONE = 7; // not used

	final double HOME_ZONE = 2.0;

	public final static boolean DEBUG = false;
	private NodeVec2	turret_configuration;
	private NodeVec2	steering_configuration;
	private NodeDouble	gripper_fingers_configuration;
	private NodeDouble	gripper_height_configuration;
	private NodeDouble 	reward_monitor;
	private NodeInt 	state_monitor;
	private NodeInt 	action_monitor;

	private	String		filename_prefix;
	private	String		policy_filename;
	private	String		log_filename;
	private	String		profile_filename;
	private	int		num_delivered=0;



	/**
	 * Configure the control system using Clay.
	 */
	public void configure()
		{
		//================
		// Set some initial hardware configurations.
		//================
		if (true) System.out.println("harrassblue .Configure()");
		abstract_robot.setObstacleMaxRange(3.0); // don't consider 
	 						 // things further away
		abstract_robot.setBaseSpeed(0.4*abstract_robot.MAX_TRANSLATION);
		abstract_robot.setGripperHeight(-1,0);   // put gripper down
		abstract_robot.setGripperFingers(-1,1);  // open gripper

		int tmp = abstract_robot.getPlayerNumber(-1);
		if (tmp<10) filename_prefix = "agent0";
		else filename_prefix = "agent";
		filename_prefix = filename_prefix.concat(
			String.valueOf(tmp));
		policy_filename = filename_prefix.concat(".policy");
		log_filename = filename_prefix.concat(".log");
		profile_filename = filename_prefix.concat(".profile");


		//================
		// perceptual schemas
		//================
		//--- robot's global position
		NodeVec2
		PS_GLOBAL_POS = new v_GlobalPosition_r(abstract_robot);

		//--- obstacles
		NodeVec2Array // the sonar readings
		PS_OBS = new va_Obstacles_r(abstract_robot);

		//--- HOMEBASE 
		NodeVec2      // the center of our little world
		PS_HOMEBASE_GLOBAL = new v_FixedPoint_(10.0,0.0);
		NodeVec2      // make it egocentric
		PS_HOMEBASE = new v_GlobalToEgo_rv(abstract_robot,
				PS_HOMEBASE_GLOBAL);

		//--- WEST 
		NodeVec2      // the western front
		PS_WEST_GLOBAL = new v_FixedPoint_(-5,0.0);
		NodeVec2      // make it egocentric
		PS_WEST = new v_GlobalToEgo_rv(abstract_robot,
				PS_WEST_GLOBAL);

		//--- EAST 
		NodeVec2      // the eastern front
		PS_EAST_GLOBAL = new v_FixedPoint_(5,0.0);
		NodeVec2      // make it egocentric
		PS_EAST = new v_GlobalToEgo_rv(abstract_robot,
				PS_EAST_GLOBAL);

		//--- RED_BIN 
		NodeVec2      // the place to deliver red things
		PS_RED_BIN_GLOBAL = new v_FixedPoint_(-10,0.0);
		NodeVec2      // make it egocentric
		PS_RED_BIN = new v_GlobalToEgo_rv(abstract_robot,
				PS_RED_BIN_GLOBAL);

		//--- BLUE_BIN 
		NodeVec2      // the place to deliver blue things
		PS_BLUE_BIN_GLOBAL = new v_FixedPoint_(10,0.0);
		NodeVec2      // make it egocentric
		PS_BLUE_BIN = new v_GlobalToEgo_rv(abstract_robot,
				PS_BLUE_BIN_GLOBAL);

		//=== RED TARGETS (visual class 0)
		NodeVec2Array 
		PS_RED_TARGETS_EGO = 
			new va_VisualObjects_r(0,abstract_robot);
		NodeVec2Array 
		PS_RED_TARGETS_GLOBAL = 
			new va_Add_vav(PS_RED_TARGETS_EGO, PS_GLOBAL_POS);

		//--- filter out red targs in bins
		NodeVec2Array 
		PS_RED_TARGETS_GLOBAL_FILT = new va_FilterOutClose_vva(1.0,
			PS_HOMEBASE_GLOBAL, PS_RED_TARGETS_GLOBAL);

		//--- make them egocentric
		NodeVec2Array 
		PS_RED_TARGETS_EGO_FILT = new va_Subtract_vav(
			PS_RED_TARGETS_GLOBAL_FILT, PS_GLOBAL_POS);

		//--- get the closest one
		NodeVec2
		PS_CLOSEST_RED = new v_Closest_va(PS_RED_TARGETS_EGO_FILT);

		//--- filter out red targs in home zone
		NodeVec2Array 
		PS_RED_TARGETS_OUT_HZ_GLOBAL_FILT = new va_FilterOutClose_vva(
			HOME_ZONE+.5,
			PS_HOMEBASE_GLOBAL, 
			PS_RED_TARGETS_GLOBAL);

		//--- make them egocentric
		NodeVec2Array 
		PS_RED_TARGETS_OUT_HZ_EGO_FILT = new va_Subtract_vav(
			PS_RED_TARGETS_OUT_HZ_GLOBAL_FILT, PS_GLOBAL_POS);

		//--- get the closest one
		NodeVec2
		PS_CLOSEST_RED_OUT_HZ = new v_Closest_va(
			PS_RED_TARGETS_OUT_HZ_EGO_FILT);


		//=== BLUE TARGETS (visual class 1)
		NodeVec2Array 
		PS_BLUE_TARGETS_EGO = 
			new va_VisualObjects_r(2,abstract_robot);
		NodeVec2Array 
		PS_BLUE_TARGETS_GLOBAL = 
			new va_Add_vav(PS_BLUE_TARGETS_EGO, PS_GLOBAL_POS);

		//--- filter out targets in bins
		NodeVec2Array 
		PS_BLUE_TARGETS_GLOBAL_FILT = new va_FilterOutClose_vva(1.0,
			PS_HOMEBASE_GLOBAL, PS_BLUE_TARGETS_GLOBAL);

		//--- make them egocentric
		NodeVec2Array 
		PS_BLUE_TARGETS_EGO_FILT = new va_Subtract_vav(
			PS_BLUE_TARGETS_GLOBAL_FILT, PS_GLOBAL_POS);

		//--- get the closest one
		NodeVec2
		PS_CLOSEST_BLUE = new v_Closest_va(PS_BLUE_TARGETS_EGO_FILT);

		//--- filter out blue targs in home zone
		NodeVec2Array 
		PS_BLUE_TARGETS_OUT_HZ_GLOBAL_FILT = new va_FilterOutClose_vva(
			HOME_ZONE+.5,
			PS_HOMEBASE_GLOBAL, 
			PS_BLUE_TARGETS_GLOBAL);

		//--- make them egocentric
		NodeVec2Array 
		PS_BLUE_TARGETS_OUT_HZ_EGO_FILT = new va_Subtract_vav(
			PS_BLUE_TARGETS_OUT_HZ_GLOBAL_FILT, PS_GLOBAL_POS);

		//--- get the closest one
		NodeVec2
		PS_CLOSEST_BLUE_OUT_HZ = new v_Closest_va(
			PS_BLUE_TARGETS_OUT_HZ_EGO_FILT);

		//=== type of object in the gripper
		NodeInt
		PS_IN_GRIPPER = new i_InGripper_r(abstract_robot);


		//================
		// Perceptual Features
		//================
		// is a red visible?
		NodeBoolean
		PF_RED_VISIBLE = new b_NonZero_v(PS_CLOSEST_RED);

		// is a red not visible?
		NodeBoolean
		PF_NOT_RED_VISIBLE = new b_Not_s(PF_RED_VISIBLE);

		// is a blue visible?
		NodeBoolean
		PF_BLUE_VISIBLE = new b_NonZero_v(PS_CLOSEST_BLUE);

		// is a blue not visible?
		NodeBoolean
		PF_NOT_BLUE_VISIBLE = new b_Not_s(PF_BLUE_VISIBLE);

		// is a red visible outside HZ?
		NodeBoolean
		PF_RED_VISIBLE_OUT_HZ = new b_NonZero_v(PS_CLOSEST_RED_OUT_HZ);

		// is a red not visible outside HZ?
		NodeBoolean
		PF_NOT_RED_VISIBLE_OUT_HZ = new b_Not_s(PF_RED_VISIBLE_OUT_HZ);

		// is a blue visible outside HZ?
		NodeBoolean
		PF_BLUE_VISIBLE_OUT_HZ = new b_NonZero_v(
			PS_CLOSEST_BLUE_OUT_HZ);

		// is a blue not visible outside HZ?
		NodeBoolean
		PF_NOT_BLUE_VISIBLE_OUT_HZ = new b_Not_s(
			PF_BLUE_VISIBLE_OUT_HZ);

		// is a red thing in the gripper?
		NodeBoolean
		PF_RED_IN_GRIPPER = new b_Equal_i(0,PS_IN_GRIPPER);

		// is a blue thing in the gripper?
		NodeBoolean
		PF_BLUE_IN_GRIPPER = new b_Equal_i(1,PS_IN_GRIPPER);

		// close to homebase 
		NodeBoolean
		PF_CLOSE_TO_HOMEBASE = new b_Close_vv(HOME_ZONE, PS_GLOBAL_POS,
			PS_HOMEBASE_GLOBAL);

		// not close to homebase
		NodeBoolean
		PF_NOT_CLOSE_TO_HOMEBASE = new b_Not_s(PF_CLOSE_TO_HOMEBASE);

		// for drop criteria
		NodeBoolean
		PF_REAL_CLOSE_TO_HOMEBASE = new b_Close_vv(1.0, PS_GLOBAL_POS,
			PS_HOMEBASE_GLOBAL);

		// drop criteria
		NodeBoolean
		PF_DROP_CRITERIA = new b_Not_s(PF_CLOSE_TO_HOMEBASE);

		// close to red bin 
		NodeBoolean
		PF_CLOSE_TO_RED_BIN = new b_Close_vv(0.4, PS_GLOBAL_POS,
			PS_RED_BIN_GLOBAL);

		// close to blue bin 
		NodeBoolean
		PF_CLOSE_TO_BLUE_BIN = new b_Close_vv(0.4, PS_GLOBAL_POS,
			PS_BLUE_BIN_GLOBAL);


		//================
		// motor schemas
		//================
		// avoid obstacles
		NodeVec2
		MS_AVOID_OBSTACLES = new v_Avoid_va(1.5,
			abstract_robot.RADIUS + 0.1,
			PS_OBS);

		// noise vector
		NodeVec2
		MS_NOISE_VECTOR = new v_Noise_(5,seed);

		// avoid the homebase
		NodeVec2
		MS_AVOID_HOMEBASE = new v_Avoid_v(1.0,
			abstract_robot.RADIUS + 0.1,
			PS_HOMEBASE);

		// avoid the HOME_ZONE
		NodeVec2
		MS_AVOID_HOME_ZONE = new v_Avoid_v(4.0,
			abstract_robot.RADIUS + 0.1,
			PS_HOMEBASE);

		// stay in the HOME_ZONE
		NodeVec2
		MS_STAY_IN_HOME_ZONE = new v_LinearAttraction_v(
			HOME_ZONE,
			HOME_ZONE-1, 
			PS_HOMEBASE);

		// stay in the east territory
		NodeVec2
		MS_STAY_IN_EAST_TERRITORY = new v_LinearAttraction_v(5.0,
			4.0, PS_EAST);

		// stay in the west territory
		NodeVec2
		MS_STAY_IN_WEST_TERRITORY = new v_LinearAttraction_v(5.0,
			4.0, 
			PS_WEST);

		// avoid the red bin
		NodeVec2
		MS_AVOID_RED_BIN = new v_Avoid_v(
			2.0,
			abstract_robot.RADIUS + 0.1,
			PS_RED_BIN);

		// avoid the blue bin
		NodeVec2
		MS_AVOID_BLUE_BIN = new v_Avoid_v(
			2.0,
			abstract_robot.RADIUS + 0.1,
			PS_BLUE_BIN);

		// swirl obstacles wrt red targets
		NodeVec2
		MS_SWIRL_OBSTACLES_RED = new v_Swirl_vav(
			2.0,
			abstract_robot.RADIUS + 0.22,
			PS_OBS,
			PS_CLOSEST_RED);

		// swirl obstacles wrt blue targets
		NodeVec2
		MS_SWIRL_OBSTACLES_BLUE = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.22,
			PS_OBS,
			PS_CLOSEST_BLUE);

		// swirl obstacles wrt homebase
		NodeVec2
		MS_SWIRL_OBSTACLES_HOMEBASE = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.22,
			PS_OBS,
			PS_HOMEBASE);

		// go to red bin
		NodeVec2
		MS_MOVE_TO_RED_BIN = new v_LinearAttraction_v(1.0,1.0,
			PS_RED_BIN);

		// go to home
		NodeVec2
		MS_MOVE_TO_HOMEBASE = new v_LinearAttraction_v(0.4,0.0,
			PS_HOMEBASE);

		// go to blue bin
		NodeVec2
		MS_MOVE_TO_BLUE_BIN = new v_LinearAttraction_v(0.4,0.0,
			PS_BLUE_BIN);

		// go to red target
		NodeVec2
		MS_MOVE_TO_RED = new v_LinearAttraction_v(0.4,0.0,
			PS_CLOSEST_RED);

		// go to blue target
		NodeVec2
		MS_MOVE_TO_BLUE = new v_LinearAttraction_v(0.0,0.0,
			PS_CLOSEST_BLUE);

		// swirl obstacles wrt noise
		NodeVec2
		MS_SWIRL_OBSTACLES_NOISE = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.1,
			PS_OBS,
			MS_NOISE_VECTOR);


		//================
		// AS_WANDER