@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); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSingleRigidBodyTranslation() { Random random = new Random(1766L); RigidBodyBasics elevator = new RigidBody("elevator", world); Vector3D jointAxis = RandomGeometry.nextVector3D(random); jointAxis.normalize(); PrismaticJoint joint = new PrismaticJoint("joint", elevator, new Vector3D(), jointAxis); RigidBodyBasics body = new RigidBody("body", joint, RandomGeometry.nextDiagonalMatrix3D(random), random.nextDouble(), new Vector3D()); joint.setQ(random.nextDouble()); joint.setQd(random.nextDouble()); Momentum momentum = computeMomentum(elevator, world); momentum.changeFrame(world); FrameVector3D linearMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getLinearPart())); FrameVector3D angularMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getAngularPart())); FrameVector3D linearMomentumCheck = new FrameVector3D(joint.getFrameBeforeJoint(), jointAxis); linearMomentumCheck.scale(body.getInertia().getMass() * joint.getQd()); FrameVector3D angularMomentumCheck = new FrameVector3D(world); double epsilon = 1e-9; EuclidCoreTestTools.assertTuple3DEquals(linearMomentumCheck, linearMomentum, epsilon); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumCheck, angularMomentum, epsilon); assertTrue(linearMomentum.length() > epsilon); }
FrameVector3D jointAxis = new FrameVector3D(parentJoint.getJointAxis()); cumulatedLinearVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedLinearVelocity.scaleAdd(parentJoint.getQd(), jointAxis, cumulatedLinearVelocity); currentBody = parentJoint.getPredecessor();