/** * Asserts on a per component basis that the two twists are equal to an {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the automated message. * @param expected the expected twist. Not modified. * @param actual the actual spatial twist. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two twists are not equal. If only one of the arguments is equal * to {@code null}. */ public static void assertTwistEquals(String messagePrefix, TwistReadOnly expected, TwistReadOnly actual, double epsilon) { assertTwistEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
/** * Asserts on a per component basis that the two twists are equal to an {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param expected the expected twist. Not modified. * @param actual the actual spatial twist. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two twists are not equal. If only one of the arguments is equal * to {@code null}. */ public static void assertTwistEquals(TwistReadOnly expected, TwistReadOnly actual, double epsilon) { assertTwistEquals(null, expected, actual, epsilon); }
expectedTwist.changeFrame(entry.getValue()); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
expectedTwist.changeFrame(entry.getValue()); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
expectedTwist.changeFrame(entry.getValue()); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
expectedTwist.changeFrame(entry.getValue()); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
expectedTwist.changeFrame(entry.getValue()); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
Twist expectedTwist = computeExpectedTwistByFiniteDifference(dt, bodyFrame, bodyFrameInFuture); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
Twist expectedTwist = computeExpectedTwistByFiniteDifference(dt, bodyFrame, bodyFrameInFuture); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
@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.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(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); } } }
private static void assertFrameTwistEquals(MovingReferenceFrame expected, MovingReferenceFrame actual, double epsilon) { EuclidCoreTestTools.assertRigidBodyTransformEquals(expected.getTransformToRoot(), actual.getTransformToRoot(), 1.0e-12); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); expected.getTwistOfFrame(expectedTwist); actual.getTwistOfFrame(actualTwist); expectedTwist.setBodyFrame(actual); expectedTwist.changeFrame(actual); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, epsilon); }
rootJacobian.getTwist(jointVelocitiesMatrix, actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12); jacobian.getTwist(jointVelocitiesMatrix, actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12);
MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); expectedTwist.changeFrame(joint.getFrameAfterJoint()); joint.getFrameAfterJoint().getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); expectedTwist.changeFrame(child.getFrameBeforeJoint()); child.getFrameBeforeJoint().getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
rootJacobian.getTwist(jointVelocitiesMatrix, actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12); jacobian.getTwist(jointVelocitiesMatrix, actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12);
@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); } } }
Twist expectedTwist = computeExpectedTwistByFiniteDifference(dt, bodyFrame, bodyFrameInFuture); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); Twist expectedRelativeTwist = computeExpectedRelativeTwistByFiniteDifference(dt, bodyFrame, bodyFrameInFuture, baseFrame, baseFrameInFuture); MecanoTestTools.assertTwistEquals(expectedRelativeTwist, actualRelativeTwist, 1.0e-5);
expectedTwist.getLinearPart().set(cumulatedLinearVelocity); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12);
Twist expectedTwist = computeExpectedTwistByFiniteDifference(dt, bodyFrame, bodyFrameInFuture); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);