@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testWithChainComposedOfPrismaticJoints() throws Exception
{
Random random = new Random(234234L);
int numberOfJoints = 20;
List<PrismaticJoint> prismaticJoints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints);
TwistCalculator twistCalculator = new TwistCalculator(worldFrame, prismaticJoints.get(random.nextInt(numberOfJoints)).getPredecessor());
for (int i = 0; i < 100; i++)
{
MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -10.0, 10.0, prismaticJoints);
MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, prismaticJoints);
twistCalculator.compute();
FrameVector3D cumulatedLinearVelocity = new FrameVector3D(worldFrame);
for (PrismaticJoint joint : prismaticJoints)
{
RigidBodyBasics body = joint.getSuccessor();
Twist actualTwist = new Twist();
twistCalculator.getTwistOfBody(body, actualTwist);
ReferenceFrame bodyFrame = body.getBodyFixedFrame();
Twist expectedTwist = new Twist(bodyFrame, worldFrame, bodyFrame);
FrameVector3D jointAxis = new FrameVector3D(joint.getJointAxis());
cumulatedLinearVelocity.changeFrame(jointAxis.getReferenceFrame());
cumulatedLinearVelocity.scaleAdd(joint.getQd(), jointAxis, cumulatedLinearVelocity);
cumulatedLinearVelocity.changeFrame(bodyFrame);
expectedTwist.getLinearPart().set(cumulatedLinearVelocity);
MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12);
}
}
}