Physics Tutorial 3: Robots

In this tutorial, we set up a robots simulation in which two circular gray robots attempt to collect several blue cans and move them to the top of the screen. The robots simulation uses all the main features of the system - there are friction forces, constraints, and collisions.

The robots are simulated with a circular body and an effector. The effector is two small circles that are constrained to the body using pin joints. In order to pick up a can, the robot needs to approach the can so that the can contacts the robot's body between the effectors. The Effector class is already defined for you because it doesn't do anything interesting on its own.

This tutorial teaches:

Create the Can class

In the sim/app/physicsTutorial3 directory, create a file called Can.java. In this file add:

package sim.app.physicsTutorial3;

import java.awt.*;
import sim.engine.*;
import sim.physics2D.physicalObject.*;
import sim.physics2D.util.*;

public class Can extends MobileObject2D implements Steppable
{
	public boolean visible;
	public Can(Double2D pos, Double2D vel)
	{
		// vary the mass with the size
		this.setPose(pos, new Angle(0));
		this.setVelocity(vel);
		this.setShape(new sim.physics2D.shape.Circle(2, Color.blue), 80);
		this.setCoefficientOfFriction(.5);
		this.setCoefficientOfRestitution(1);
		visible = true;
	}
 
	public void step(SimState state)
	{
		Double2D position = this.getPosition();
		PhysicsTutorial3 simPhysicsTutorial3 = (PhysicsTutorial3)state;
		simPhysicsTutorial3.fieldEnvironment.setObjectLocation(this, new sim.util.Double2D(position.x, position.y));
	}
}

The only thing interesting to note here is that the can has a coefficient of friction of .5. This is to simulate the fact that the can is heavy and is relatively hard to push.

Create the shell robot class

In the sim/app/physicsTutorial3 directory, create a file called Bot.java. In this file add:


package sim.app.physicsTutorial3;

import java.awt.*;
import sim.physics2D.constraint.*;
import sim.engine.*;
import sim.physics2D.physicalObject.*;
import sim.physics2D.forceGenerator.ForceGenerator;
import sim.physics2D.util.*;
import sim.util.Bag;

public class Bot extends sim.robot2D.Robot implements Steppable, ForceGenerator
{
	Can currentCan;
	
	private ConstraintEngine objCE;
	private PinJoint pj;
	
	private Double2D canHome;
	private double canHomeRadius = 20;
	private Double2D botHome;
	private int botState;

	private final int HAVECAN = 1;
	private final int APPROACHINGCAN = 2;
	private final int RELEASINGCAN = 3;
	private final int RETURNINGHOME = 4;
	private final int SEARCHING = 5;
	
	public Effector e1;
	public Effector e2;
	
	public Bot(Double2D pos, Double2D vel)
	{
		// vary the mass with the size
		this.setPose(pos, new Angle(0));
		this.setVelocity(vel);
		this.setShape(new sim.physics2D.shape.Circle(10, Color.gray), 300);
		this.setCoefficientOfFriction(.2);
		this.setCoefficientOfRestitution(1);	
		
		this.currentCan = null;
		this.canHome = new Double2D(50, 50);
		this.botHome = pos;
		this.botState = SEARCHING;
		
		this.objCE = ConstraintEngine.getInstance();
	}
}

This code sets up the constructor for the Bot class and defines some variables. Note the following:

Add the Bot's step method

Add the following method to the Bot class:

	public void step(SimState state)
	{
		Double2D position = this.getPosition();
		PhysicsTutorial3 simPhysicsTutorial3 = (PhysicsTutorial3)state;
		simPhysicsTutorial3.fieldEnvironment.setObjectLocation(this, new sim.util.Double2D(position.x, position.y));
	
		// Find a can
		if (botState == SEARCHING)
		{
			Bag objs = simPhysicsTutorial3.fieldEnvironment.allObjects;
			objs.shuffle(state.random);
			for (int i = 0; i < objs.numObjs; i++)
			{
				if (objs.objs[i] instanceof Can)
				{
					currentCan = (Can)objs.objs[i];
					if (currentCan.getPosition().y > 50 && currentCan.visible)
					{ 
						botState = APPROACHINGCAN;	
						break;
					}
					else
						currentCan = null; // can is already home or has been picked
											// up by another bot
				}
			}
			
			if (currentCan == null)
				botState = RETURNINGHOME;
		}
	}

At each time step, the robot tries to find a can to collect if its current state is SEARCHING. If it can't find an eligible can (which means they have all been collected), it returns to its starting position.

Add the Bot's handleCollisions method

Add the following method to the Bot class:

	public int handleCollision(PhysicalObject2D other, Double2D colPoint)
	{
		Double2D globalPointPos = this.getPosition().add(colPoint);
		Double2D localPointPos = this.localFromGlobal(globalPointPos);
		Angle colAngle = this.getAngle(localPointPos);
		
		// Make sure the object is a can and that it is (roughly) between
		// the effectors
		if (other instanceof Can && botState == APPROACHINGCAN
				&& (colAngle.radians < Math.PI / 8 || colAngle.radians > (Math.PI * 2 - Math.PI / 8)))
		{
			// Create a fixed joint directly at the center of the can
			pj = new PinJoint(other.getPosition(), this, other);
			objCE.registerForceConstraint(pj);
			
			botState = HAVECAN;
			
			objCE.setNoCollisions(this, other);
			objCE.setNoCollisions(e1, other);
			objCE.setNoCollisions(e2, other);
			
			currentCan.visible = false;
			return 2; // sticky collision
		}
		else
			return 1; // regular collision
	}

The physics engine notifies an object if it is involved in a collision by calling its handleCollision method. This method is defined by the base class sim.physics2D.physicalObject.PhysicalObject2D. An object can choose to override this method to perform actions when a collision occurs. After being notified of a collision, the object can choose to turn off collision response for the collision by returning 0, respond to the collision as normal by returning 1 (this is the default behavior), or set the collision as "sticky" by returning 2. A "sticky" collision is a perfectly inelastic one (like a flat basketball) in which all energy is lost along the collision normal.

The physics engine passes the object the collision point for the current collision. The passed in variable represents a vector from the object's position (center of mass) to the collision point. The vector is defined in the global coordinate frame, so the first thing the object does in this method is get the collision point in its local coordinate frame using the localFromGlobal method provided by Robot.

After getting the local coordinate of the collision point, the bot checks to see if this collision represents an attempt to pick up a can. For this to happen, the other object must be a Can and the collision must occur between the two circles representing the robot's effector.

If the collision is an attempt to pick up a can, the bot "picks up" the can by dynamically constraining the can to itself with a pin joint. It then turns off collisions between the can and itself and its effector using sim.physics2D.constraint.ConstraintEngine's setNoCollisions method. This tells the collision detector to no longer consider these pairs of objects. This method can be used for any pair of objects for which collision detection and response is not desired. Finally, the method sets this collision as "sticky" to prevent the collision response from attempting to bounce the can off the bot (since we are trying to simulate the bot grabbing the can).

If this collision is not an attempt to pick up a can, the method returns 1 for normal collision response.

Add the Bot's addForce method

Add the following method to the Bot class:

	public void addForce()
	{
		switch (botState)
		{
			case HAVECAN:
				if (this.getPosition().y <= 40)
				{
					if (this.getVelocity().length() > 0.01 || this.getAngularVelocity() > 0.01)
						this.stop();
					else
					{
						objCE.unRegisterForceConstraint(pj);				
						botState = RELEASINGCAN;
						objCE.removeNoCollisions(this, currentCan);
						objCE.removeNoCollisions(e1, currentCan);
						objCE.removeNoCollisions(e2, currentCan);
						currentCan.visible = true;
					}
				}
				else
					this.goTo(new Double2D(this.getPosition().x, 40));
				break;
			case RELEASINGCAN:
				// back out of can home
				if (this.getPosition().subtract(currentCan.getPosition()).length() <= 30)
					backup();
				else
					botState = SEARCHING;
				break;
			case APPROACHINGCAN:
				if (currentCan.visible)
					this.goTo(currentCan.getPosition());
				else
					botState = SEARCHING;
				break;
			case RETURNINGHOME:
				if (this.getPosition().subtract(botHome).length() <= 30)
				{
					if (this.getOrientation().radians != 0)
						this.faceTowards(new Angle(0));
					else
						stop();
				}
				else
					this.goTo(botHome);
				break;	
		}

	}

Recall from physics tutorial 2 that any forces applied to objects should be applied from within a ForceGenerator's addForce method. This method is called at least once per time step by the numerical integrator.

This method uses the controller functionality provided by sim.app.robot2D.Robot to control the robot based on its current state. If the bot currently has a can and is at the top of the screen, it first stops and then releases the can by dynamically unregistering the pin joint. If it has just released the can, it backs up and finds a new can to collect. If the bot has found a can (state APPROACHINGCAN), it moves toward it. Finally, if there are no more cans to collect (state RETURNINGHOME), the bot returns to its starting position and stops.

Set up the physics simulation in the MASON SimState's start method

The file sim/app/physicsTutorial3/PhysicsTutorial3.java already has much of the non-physics related MASON code needed to set up the simulation. The only code to add is in the start method. Add the following code to the start method after the "Add physics specific code here" line.

		// Create and schedule the physics engine
		PhysicsEngine2D objPE = new PhysicsEngine2D();
		schedule.scheduleRepeating(objPE);
		
		Double2D pos;
		Double2D vel;
		
		Bot bot;
		Can can;
		Wall wall;

		// WALLS
		// HORIZ
		pos = new Double2D(100,0);
		wall = new Wall(pos, 193, 6);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
 
		pos = new Double2D(100,200);
		wall = new Wall(pos, 193, 6);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
		
		// VERT
		pos = new Double2D(0,100);
		wall = new Wall(pos, 6, 200);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
	
		pos = new Double2D(200,100);
		wall = new Wall(pos, 6, 200);
		fieldEnvironment.setObjectLocation(wall, new sim.util.Double2D(pos.x, pos.y));
		objPE.register(wall);
		
		// Set up the cans
		for (int i = 0; i < 4; i++)
		{
			double x = Math.max(Math.min(random.nextDouble() * 200, 190), 10);
			double y = Math.max(Math.min(random.nextDouble() * 200, 190), 60);
			
			pos = new Double2D(x, y);
			
			can = new Can(pos, new Double2D(0, 0));
			fieldEnvironment.setObjectLocation(can, new sim.util.Double2D(pos.x, pos.y));
			objPE.register(can);
			schedule.scheduleRepeating(can);
		}
		
		// Set up the bots
		for (int i = 0; i < 2; i++)
		{
			double x = Math.max(Math.min(random.nextDouble() * 200, 180), 20);
			double y = Math.max(Math.min(random.nextDouble() * 200, 180), 50);
			
			pos = new Double2D(x, y);
			bot = new Bot(pos, new Double2D(0, 0));
			objPE.register(bot);
			fieldEnvironment.setObjectLocation(bot, new sim.util.Double2D(pos.x, pos.y));
			schedule.scheduleRepeating(bot);
			
			Effector effector;
			
			pos = new Double2D(x + 12, y + 6);
			effector = new Effector(pos, new Double2D(0, 0), 1, Color.gray);
			objPE.register(effector);
			fieldEnvironment.setObjectLocation(effector, new sim.util.Double2D(pos.x, pos.y));
			schedule.scheduleRepeating(effector);
			bot.e1 = effector;
			
			objPE.setNoCollisions(bot, effector);

			PinJoint pj = new PinJoint(pos, effector, bot);	
			objPE.register(pj);
			
			pos = new Double2D(x + 12, y - 6);
			effector = new Effector(pos, new Double2D(0, 0), 1, Color.gray);
			objPE.register(effector);
			fieldEnvironment.setObjectLocation(effector, new sim.util.Double2D(pos.x, pos.y));
			schedule.scheduleRepeating(effector);
			bot.e2 = effector;
			
			objPE.setNoCollisions(bot, effector);
	
			pj = new PinJoint(pos, effector, bot);
			objPE.register(pj);		
		}

This code is similar to the start methods in tutorials 1 and 2. It creates and schedules the physics engine, creates and places the walls, and creates the cans and bots.

Run the simulation

Compile Can.java, Bot.java, Wall.java, Effector.java, PhysicsTutorial3.java, and PhysicsTutorial3WithUI.java. Then run the program as java sim.app.physicsTutorial3.PhysicsTutorial3WithUI.

After pressing play, you should see two gray robots attempting to move four blue cans to the top of the screen.