@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithPrismaticChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<PrismaticJoint> joints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddPrismaticJoint_String_RigidBody_Vector3d_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); Vector3D jointOffset = RandomGeometry.nextVector3D(random, 5.0); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); PrismaticJoint joint = new PrismaticJoint(jointName, parentBody, jointOffset, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddPrismaticJoint_String_RigidBody_Transform3D_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); RigidBodyTransform transformToParent = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); PrismaticJoint joint = new PrismaticJoint(jointName, parentBody, transformToParent, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); }
int numberOfJoints = 100; List<PrismaticJoint> prismaticJoints = MultiBodySystemRandomTools.nextPrismaticJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, prismaticJoints.get(random.nextInt(numberOfJoints)).getPredecessor()); cumulatedLinearVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedLinearVelocity.scaleAdd(parentJoint.getQd(), jointAxis, cumulatedLinearVelocity); currentBody = parentJoint.getPredecessor();
@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); } } }
joint1.getPredecessor().updateFramesRecursively(); bodyManipulatorJacobian.compute(); spatialManipulatorJacobian.compute();