@Test
public void testOffsetAtCenterOfMassWithCantileveredBeam() throws UnreasonableAccelerationException
{
double massOne = 7.21;
Robot robot = new Robot("JointWrenchSensorTest");
PinJoint pinJointOne = createPinJointWithHangingMass("pinJointOne", massOne, Axis.Y, robot);
Vector3D comOffsetFromJointOne = new Vector3D();
pinJointOne.getLink().getComOffset(comOffsetFromJointOne);
JointWrenchSensor jointWrenchSensorOne = new JointWrenchSensor("jointWrenchSensorOne", comOffsetFromJointOne, robot);
pinJointOne.addJointWrenchSensor(jointWrenchSensorOne);
assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor());
robot.addRootJoint(pinJointOne);
pinJointOne.setQ(Math.PI/2.0);
pinJointOne.setTau(massOne * robot.getGravityZ() * comOffsetFromJointOne.getZ());
robot.doDynamicsAndIntegrate(0.0001);
double jointAcceleration = pinJointOne.getQDDYoVariable().getDoubleValue();
assertEquals(0.0, jointAcceleration, 1e-7);
Vector3D expectedJointForce = new Vector3D(-massOne * robot.getGravityZ(), 0.0, 0.0);
Vector3D expectedJointTorque = new Vector3D();
assertJointWrenchEquals(jointWrenchSensorOne, expectedJointForce, expectedJointTorque);
}