com.yobotics.simulationconstructionset
Class Robot

java.lang.Object
  com.yobotics.simulationconstructionset.YoVariableHolderImplementation
      com.yobotics.simulationconstructionset.Robot

All Implemented Interfaces:

YoVariableHolder, java.io.Serializable

public class Robot
extends YoVariableHolderImplementation
implements java.io.Serializable

Title: Robot

Description: A Robot is a forest of trees of Joints, each Joint having an associated Link. The Robot contains all the dynamic information, including Joint types, offsets between Joints, Link masses, center of mass locations, and moments of inertia. Each root joint has children

Copyright: Copyright (c) 2000-2005

Company: Yobotics, Inc.

Version:

1.0

Author:

Jerry Pratt

See Also:

Serialized Form

Constructor Summary

Robot(RobotDefinitionFixedFrame definition)

Robot(java.lang.String name)
Creates a Robot with the specified name.

Method Summary

void	addFunctionToIntegrate(FunctionToIntegrate functionToIntegrate) Adds a FunctionToIntegrate which will be integrated each simulation step.
void	addRootJoint(Joint root) Adds a root Joint to this robot.
void	addStaticBranchGroup(javax.media.j3d.BranchGroup staticBranchGroup) Adds the specified BranchGroup to the robot.
void	addStaticLink(Link staticLink) Adds the specified link to the robot.
void	addStaticLinkNode(javax.media.j3d.Link link3d) Adds the specified link3d to the robot.
void	addStaticSharedGroup(javax.media.j3d.SharedGroup staticSharedGroup) Adds the specified SharedGroup to the robot.
void	addYoVariableRegistry(YoVariableRegistry registry) Adds a YoVariableRegistry to the robot.
void	computeAngularMomentum(Joint rootJoint, javax.vecmath.Vector3d angularMomentum) Computes the total angular momentum about the center of mass for the subtree rooted at the specified root Joint.
void	computeAngularMomentum(javax.vecmath.Vector3d angularMomentum) Computes the total angular momentum about the center of mass for this Robot.
double	computeCenterOfMass(Joint rootJoint, javax.vecmath.Point3d comPoint) Computes the Center of Mass of the subtree rooted at the specified root Joint.
double	computeCenterOfMass(javax.vecmath.Point3d comPoint) Computes the center of mass of this Robot.
void	computeCenterOfPressure(javax.vecmath.Point3d copPoint, javax.vecmath.Vector3d copForce, javax.vecmath.Vector3d copMoment) Computes the Center of Pressure of the GroundContactPoints attached to this Robot.
double	computeCOMMomentum(Joint rootJoint, javax.vecmath.Point3d comPoint, javax.vecmath.Vector3d linearMomentum, javax.vecmath.Vector3d angularMomentum) Computes the Center of Mass location and total linear and angular momentum about the center of mass for the subtree rooted at the specified root Joint.
double	computeCOMMomentum(javax.vecmath.Point3d comPoint, javax.vecmath.Vector3d linearMomentum, javax.vecmath.Vector3d angularMomentum) Computes the Center of Mass location and total linear and angular momentum about the Center of Mass for this Robot.
double	computeGravitationalPotentialEnergy() Computes the total gravitational potential energy of this Robot.
double	computeGravitationalPotentialEnergy(Joint rootJoint) Computes the total gravitational potential energy of the subtree rooted at the specified root Joint.
double	computeLinearMomentum(Joint rootJoint, javax.vecmath.Vector3d linearMomentum) Computes the total linear momentum of the center of mass for the subtree rooted at the specified root Joint.
double	computeLinearMomentum(javax.vecmath.Vector3d linearMomentum) Computes the total linear momentum of the center of mass for this Robot.
double	computeRotationalKineticEnergy() Computes the total rotational kinetic energy of this Robot.
double	computeRotationalKineticEnergy(Joint rootJoint) Computes the total rotational kinetic energy of the subtree rooted at the specified root Joint.
double	computeTranslationalKineticEnergy() Computes the total translational kinetic energy of this Robot.
double	computeTranslationalKineticEnergy(Joint rootJoint) Computes the total translational kinetic energy of the subtree rooted at the specified root Joint.
java.util.ArrayList<VarList>	createAllVarLists()
void	decideGroundContactPointsInContact() Step through each joint to determine if any points are in contact with the ground.
void	doAfterControlBeforeDynamics()
void	doDynamicsAndIntegrate(double DT) Calculates the robot dynamics and integrates to the next step.
javax.media.j3d.BranchGroup	getBranchGroupCopy(Joint joint) Returns a copy of the BranchGroup for the specified joint with that joint as the root.
void	getCameraMountList(CameraMountList cameraMountList) Steps through every joint adding each camera mount to the provided list.
double	getGravityX() Gets this robot's component of gravitational acceleration in the x-direction.
double	getGravityY() Gets this robot's component of gravitational acceleration in the y-direction.
double	getGravityZ() Gets this robot's component of gravitational acceleration in the z-direction.
GroundContactModel	getGroundContactModel() Retrieves this robot's ground contact model.
java.util.ArrayList<GroundContactPoint>	getGroundContactPoints() Retrieves a list of all ground contact points associated with this robot.
java.lang.String	getName() Gets the name of this Robot.
java.util.ArrayList	getRangeSensors() Returns an ArrayList containing the range sensors of each joint.
YoVariableRegistry	getRobotsYoVariableRegistry()
javax.media.j3d.BranchGroup	getRootBranchGroup() Retrieves the BranchGroup for this robot.
java.util.ArrayList<Joint>	getRootJoints() Retrieves an ArrayList containing the rootJoints of this robot.
double	getTime() Gets this robot's time.
void	setController(RobotController controller) Adds a controller to use with this Robot.
void	setController(RobotController controller, int simulationTicksPerControlTick) Adds a controller to use with this Robot.
void	setGravity(double gZ) Sets the Z component of the gravity to the specified value.
void	setGravity(double gravityX, double gravityY, double gravityZ) Sets gravity to the specified values.
void	setGroundContactModel(GroundContactModel gcModel) Sets the ground contact model for this robot.
java.lang.String	toString() This method returns the display name of the robot followed by the names of each joint.
void	update() Updates all joint data without updating graphics.
void	updateAllGroundContactPointVelocities() Updates the velocities for all ground contact points.
void	updateGraphics() Updates the graphics for all of the robot's nodes.
void	updateVelocities() Updates the velocities and positions at each joint as well as the rotation matricies from and to the base coordinate system.

Methods inherited from class com.yobotics.simulationconstructionset.YoVariableHolderImplementation

addVariablesToHolder, addVariableToHolder, getAllVariables, getAllVariablesArray, getVariable, getVariable, getVariables, getVariables, getVariables, hasUniqueVariable, hasUniqueVariable

Methods inherited from class java.lang.Object

equals, getClass, hashCode, notify, notifyAll, wait, wait, wait

Constructor Detail

Robot

public Robot(RobotDefinitionFixedFrame definition)

Robot

public Robot(java.lang.String name)

Creates a Robot with the specified name. A Robot is a forest of trees of Joints, each Joint having an associated Link.

Parameters:

name - Name of the robot.

Method Detail

getTime

public double getTime()

Gets this robot's time.

Returns:

double

getGravityX

public double getGravityX()

Gets this robot's component of gravitational acceleration in the x-direction.

Returns:

double

getGravityY

public double getGravityY()

Gets this robot's component of gravitational acceleration in the y-direction.

Returns:

double

getGravityZ

public double getGravityZ()

Gets this robot's component of gravitational acceleration in the z-direction.

Returns:

double

addYoVariableRegistry

public void addYoVariableRegistry(YoVariableRegistry registry)

Adds a YoVariableRegistry to the robot. Will be added as a child registry to the robot's registry.

Parameters:

registry - YoVariableRegistry

addRootJoint

public void addRootJoint(Joint root)

Adds a root Joint to this robot. This joint may have multiple child joints which also can have children. A robot may have any number of root joints.

Parameters:

root - Joint to be added as the root Joint. A robot is comprised of a forest of trees, with one root Joint per tree.

getRootBranchGroup

public javax.media.j3d.BranchGroup getRootBranchGroup()

Retrieves the BranchGroup for this robot. This contains all of the information required for the graphical representation of this robot.

Returns:

BranchGroup The root BranchGroup of this robot.

getBranchGroupCopy

public javax.media.j3d.BranchGroup getBranchGroupCopy(Joint joint)

Returns a copy of the BranchGroup for the specified joint with that joint as the root. This copy also contains all of joint's children.

Parameters:

joint - Joint around which to base copied BranchGroup.

Returns:

BranchGroup New BranchGroup with joint as its root.

getGroundContactModel

public GroundContactModel getGroundContactModel()

Retrieves this robot's ground contact model.

Returns:

GroundContactModel The GroundContactModel of this robot.

See Also:

GroundContactModel

decideGroundContactPointsInContact

public void decideGroundContactPointsInContact()

Step through each joint to determine if any points are in contact with the ground.

getRootJoints

public java.util.ArrayList<Joint> getRootJoints()

Retrieves an ArrayList containing the rootJoints of this robot. These joints make up the entirety of the robot's visual component as all joints and links are at some level their children.

Returns:

ArrayList containing the root joints of the robot.

setGravity

public void setGravity(double gravityX,
                       double gravityY,
                       double gravityZ)

Sets gravity to the specified values. For example, if using meter-kilogram-seconds units of measure with Earth's gravity, then use setGravity(0.0, 0.0, -9.81);

Parameters:

gX - X component of the Gravity vector.

gY - Y component of the Gravity vector.

gZ - Z component of the Gravity vector.

setGravity

public void setGravity(double gZ)

Sets the Z component of the gravity to the specified value. For example, if using meter-kilogram-seconds units of measure with Earth's gravity, then use setGravity(-9.81);

Parameters:

gZ - Z component of the Gravity vector. X and Y components are set to 0.0.

addFunctionToIntegrate

public void addFunctionToIntegrate(FunctionToIntegrate functionToIntegrate)

Adds a FunctionToIntegrate which will be integrated each simulation step. If no functions are present that step is skipped.

Parameters:

functionToIntegrate - The function to be integrated.

See Also:

FunctionToIntegrate

setController

public void setController(RobotController controller)

Adds a controller to use with this Robot. A single robot can have multiple controllers, all of which execute their doControl methods when called. Controllers added with this function doControl every simulation tick.

Parameters:

controller - RobotController to use with this robot.

See Also:

RobotController

setController

public void setController(RobotController controller,
                          int simulationTicksPerControlTick)

Adds a controller to use with this Robot. This method provides a method for reducing the number of control ticks per simulation tick. In other words, the controller only executes once every x simulation ticks.

Parameters:

controller - RobotController to use with this robot.

simulationTicksPerControlTick - Number of simulation ticks per control tick.

setGroundContactModel

public void setGroundContactModel(GroundContactModel gcModel)

Sets the ground contact model for this robot. This allow the robot to interact with its surroundings based on their characteristics.

Parameters:

gcModel - GroundContactModel to be used with this robot.

See Also:

GroundContactModel

getName

public java.lang.String getName()

Gets the name of this Robot.

Returns:

Name of this Robot.

update

public void update()

Updates all joint data without updating graphics.

updateAllGroundContactPointVelocities

public void updateAllGroundContactPointVelocities()

Updates the velocities for all ground contact points. This is called once per simulation tick to ensure all ground contact point velocities are updated.

updateGraphics

public void updateGraphics()

Updates the graphics for all of the robot's nodes. This method is called once per frame assuming a ViewportPanel exists.

getCameraMountList

public void getCameraMountList(CameraMountList cameraMountList)

Steps through every joint adding each camera mount to the provided list. This function is called by the GUI on one of two occasions. When the GUI is created and when a robot is set. In both cases the method is not called in the null robot case.

Parameters:

cameraMountList - CameraMountList to which all mount points are added.

getRangeSensors

public java.util.ArrayList getRangeSensors()

Returns an ArrayList containing the range sensors of each joint. This allows StandardSimulationGraphics to set the local for each range sensor when the graphics are created.

Returns:

ArrayList containing the range sensors for each point.

getGroundContactPoints

public java.util.ArrayList<GroundContactPoint> getGroundContactPoints()

Retrieves a list of all ground contact points associated with this robot. This is currently used by several ground contact models as well as the computeCenterOfPressure method.

Returns:

ArrayList containing the GroundContactPoints, if none exist the list will be empty.

addStaticLink

public void addStaticLink(Link staticLink)

Adds the specified link to the robot. Static links have no effect on the simulation, they are purely cosmetic.

Parameters:

staticLink - Link to be added.

addStaticBranchGroup

public void addStaticBranchGroup(javax.media.j3d.BranchGroup staticBranchGroup)

Adds the specified BranchGroup to the robot. Static components have no effect on the simulation, they are purely cosmetic. A BranchGroup may contain several links.

Parameters:

staticBranchGroup - BranchGroup to be added.

addStaticSharedGroup

public void addStaticSharedGroup(javax.media.j3d.SharedGroup staticSharedGroup)

Adds the specified SharedGroup to the robot. Static components have no effect on the simulation, they are purely cosmetic.

Parameters:

staticSharedGroup - SharedGroup to be added.

addStaticLinkNode

public void addStaticLinkNode(javax.media.j3d.Link link3d)

Adds the specified link3d to the robot. Static components have no effect on the simulation, they are purely cosmetic.

Parameters:

link3d - Link to be added.

updateVelocities

public void updateVelocities()

Updates the velocities and positions at each joint as well as the rotation matricies from and to the base coordinate system. Each joint stores both its rotational and translational velocities. The updated velocities are based on those of the previous joint in the chain. This also updates all kinetic and ground contact points.

getRobotsYoVariableRegistry

public YoVariableRegistry getRobotsYoVariableRegistry()

doAfterControlBeforeDynamics

public void doAfterControlBeforeDynamics()

doDynamicsAndIntegrate

public void doDynamicsAndIntegrate(double DT)
                            throws UnreasonableAccelerationException

Calculates the robot dynamics and integrates to the next step. This integration is accomplished via the Runge-Kutta method which approximates the next point in the series by calculating four intermediary slopes. The robot dynamics are calculated four times using the following method to generate these Runge-Kutta slopes.

Robot dynamics are calculated via the Featherstone algorithm as discussed by Brian Mirtich in his Thesis, Impule-based Dynamic Simulation of Rigid Body Systems. This algorithm is implemented in four distinct passes as follows:

1. The first pass recurses down the tree, calculating the linear and angular velocities for each link. In the process the rotation matricies and radius vectors are recalculated. The coordinate system of each joint/link space is centered on the current link's center of mass. Each joint contains a rotation matrix and corresponding radius vector to translate information to and from the previous link's coordinate system. These functions must be updated before the velocities may be calculated. The kinematic and ground contact points as well as torque for each joint are also updated.
2. Pass two recurses down the tree again, calculating the isolated components of the spatial articulated inertia and spatial isolated zero-acceleration force for each link. Coriolis forces and forces from ground contact and kinetic points are included in these calculations.
3. In the third pass the completed articulated inertia and articulated z.a. forces are calculated for each link by recursing up the tree. These values are based on the positions, velocities and torques of each link.
4. The final pass recurses down the tree calculating the joint accelerations based on the information generated by the previous three passes. During this pass the joint positions, velocities and accelerations are stored for use in the Runge-Kutta calculations.

Between each run of the dynamics Euler Integration is used to shift the values in preparation for the next step. However, the values are returned to their original states before each integration. Each pass of the dynamics stores the Runge-Kutta slope for each parameter. Once the dynamics have been calculated the joints are checked for unreasonable accelerations. If none are present, Runge-Kutta is executed for each joint and the current time is updated.

Parameters:

DT - Step time for the integration.

Throws:

UnreasonableAccelerationException - This exception indicates that at least one joint underwent an unreasonable acceleration as defined by its joint type. This is often caused by overly large step times (DT).

computeTranslationalKineticEnergy

public double computeTranslationalKineticEnergy()

Computes the total translational kinetic energy of this Robot. This is the sum over i of 1/2 m_i v_i*v_i, where m_i is the mass of link i, and v_i is the translational velocity of the center of mass of link i.

Returns:

Total translational kinetic energy.

computeTranslationalKineticEnergy

public double computeTranslationalKineticEnergy(Joint rootJoint)

Computes the total translational kinetic energy of the subtree rooted at the specified root Joint. This is the sum over i of 1/2 m_i v_i*v_i, where m_i is the mass of link i, and v_i is the translational velocity of the center of mass of link i, and i includes all decendents of the specified root Joint.

Parameters:

rootJoint - Root Joint to compute energy from.

Returns:

Total translational kinetic energy of the subtree rooted at the specified root Joint.

computeRotationalKineticEnergy

public double computeRotationalKineticEnergy()

Computes the total rotational kinetic energy of this Robot. This is the sum over i of 1/2 w_i^T*J_i*w_i, where J_i is the ineria matrix of link i, and w_i is the rotational velocity vector of the center of mass of link i.

Returns:

Total rotational kinetic energy.

computeRotationalKineticEnergy

public double computeRotationalKineticEnergy(Joint rootJoint)

Computes the total rotational kinetic energy of the subtree rooted at the specified root Joint. This is the sum over i of 1/2 w_i^T*J_i*w_i, where J_i is the inertia matrix of link i, and w_i is the rotational velocity vector of the center of mass of link i, and i consisting of all the decendents of the specified root Joint.

Parameters:

rootJoint - Root Joint to compute energy from.

Returns:

Total rotational kinetic energy of the subtree rooted at the specified root Joint.

computeGravitationalPotentialEnergy

public double computeGravitationalPotentialEnergy()

Computes the total gravitational potential energy of this Robot. This is the sum over i of m_i*g*h_i, where m_i is the mass of link i, g is the gravitational constant, and h_i is the height above 0 of link i.

Returns:

Total gravitational potential energy.

computeGravitationalPotentialEnergy

public double computeGravitationalPotentialEnergy(Joint rootJoint)

Computes the total gravitational potential energy of the subtree rooted at the specified root Joint. This is the sum over i of m_i*g*h_i, where m_i is the mass of link i, g is the gravitational constant, and h_i is the height above 0 of link i, with i consisting of all the decendents of the specified root Joint.

Returns:

Total gravitational potential energy.

computeCenterOfMass

public double computeCenterOfMass(javax.vecmath.Point3d comPoint)

Computes the center of mass of this Robot. This center of mass position is returned by altering the provided Point3d. If the robot has no mass, it also has no center of mass point. This value is used in the calculation of center of momentum.

Parameters:

comPoint - Center of Mass point, in World Coordinates, that is computed.

Returns:

The total mass of the robot.

computeCenterOfMass

public double computeCenterOfMass(Joint rootJoint,
                                  javax.vecmath.Point3d comPoint)

Computes the Center of Mass of the subtree rooted at the specified root Joint. This center of mass position is returned by altering the provided Point3d. If the robot has no mass, it also has no center of mass point.

Parameters:

comPoint - Center of Mass point, in World Coordinates, that is computed.

Returns:

The total mass of the robot.

computeLinearMomentum

public double computeLinearMomentum(javax.vecmath.Vector3d linearMomentum)

Computes the total linear momentum of the center of mass for this Robot. The total mass of the robot is also computed and returned.

Parameters:

linearMomentum - Total linear momentum vector that is computed.

Returns:

The total mass of the robot.

computeLinearMomentum

public double computeLinearMomentum(Joint rootJoint,
                                    javax.vecmath.Vector3d linearMomentum)

Computes the total linear momentum of the center of mass for the subtree rooted at the specified root Joint. The total mass of the robot is also computed and returned.

Parameters:

rootJoint - Root Joint for which linear momentum is computed.

linearMomentum - Total linear momentum vector that is computed.

Returns:

The total mass of the robot.

computeAngularMomentum

public void computeAngularMomentum(javax.vecmath.Vector3d angularMomentum)

Computes the total angular momentum about the center of mass for this Robot.

Parameters:

angularMomentum - Total angular momentum vector that is computed.

computeAngularMomentum

public void computeAngularMomentum(Joint rootJoint,
                                   javax.vecmath.Vector3d angularMomentum)

Computes the total angular momentum about the center of mass for the subtree rooted at the specified root Joint.

Parameters:

rootJoint - Root Joint for which linear momentum is computed.

angularMomentum - Total angular momentum vector that is computed.

computeCOMMomentum

public double computeCOMMomentum(javax.vecmath.Point3d comPoint,
                                 javax.vecmath.Vector3d linearMomentum,
                                 javax.vecmath.Vector3d angularMomentum)

Computes the Center of Mass location and total linear and angular momentum about the Center of Mass for this Robot.

Parameters:

comPoint - Center of Mass point that is computed.

linearMomentum - Total linear momentum vector that is computed.

angularMomentum - Total angular momentum vector that is computed.

Returns:

Total mass of the robot.

computeCOMMomentum

public double computeCOMMomentum(Joint rootJoint,
                                 javax.vecmath.Point3d comPoint,
                                 javax.vecmath.Vector3d linearMomentum,
                                 javax.vecmath.Vector3d angularMomentum)

Computes the Center of Mass location and total linear and angular momentum about the center of mass for the subtree rooted at the specified root Joint.

Parameters:

rootJoint - Root Joint to computed momentum from.

comPoint - Center of Mass point that is computed.

linearMomentum - Total linear momentum vector that is computed.

angularMomentum - Total angular momentum vector that is computed.

Returns:

Total mass of the robot.

computeCenterOfPressure

public void computeCenterOfPressure(javax.vecmath.Point3d copPoint,
                                    javax.vecmath.Vector3d copForce,
                                    javax.vecmath.Vector3d copMoment)

Computes the Center of Pressure of the GroundContactPoints attached to this Robot.

Parameters:

copPoint - Center of Pressure point that is computed.

copForce - Center of Pressure total force vector that is computed.

copMoment - Total moment generated about the Center of Pressure from all the GroundContactPoints.

toString

public java.lang.String toString()

This method returns the display name of the robot followed by the names of each joint.

Overrides:

toString in class java.lang.Object

Returns:

String, display name of the robot

createAllVarLists

public java.util.ArrayList<VarList> createAllVarLists()