public TwoLinkRobotForTesting()
{
super("TwoLink");
this.gravity = 9.81;
this.setGravity(0.0, 0.0, -gravity);
upperJoint = new PinJoint("upper", new Vector3D(0.0, 0.0, 4.0 * linkLength), this, Axis.Y);
Link upperArmLink = upperArm();
upperJoint.setLink(upperArmLink);
upperJoint.setLimitStops(-2.0, 5.0, 2.0, 1000.0);
upperJoint.setVelocityLimits(7.0, 1000.0);
upperJoint.setDynamic(true);
this.addRootJoint(upperJoint);
elbowJoint = new PinJoint("elbow", new Vector3D(0.0, 0.0, -linkLength), this, Axis.Y);
upperJoint.addJoint(elbowJoint);
Link lowerArmLink = lowerArm();
elbowJoint.setLink(lowerArmLink);
elbowJoint.setLimitStops(-4.0, 3.0, 2.0, 1.0);
elbowJoint.setVelocityLimits(4.0, 1.0);
}