/** * Generates a random kinematic chain composed of rigid-bodies and prismatic joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param rootBody the root to which the kinematic chain is to be attached. * @param numberOfJoints how many joints the kinematic chain should be composed of. * @return the list of all the joints composing the kinematic chain. */ public static List<PrismaticJoint> nextPrismaticJointChain(Random random, RigidBodyBasics rootBody, int numberOfJoints) { return nextPrismaticJointChain(random, "", rootBody, numberOfJoints); }
/** * Generates a random kinematic chain composed of rigid-bodies and prismatic joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param numberOfJoints how many joints the kinematic chain should be composed of. * @return the list of all the joints composing the kinematic chain. */ public static List<PrismaticJoint> nextPrismaticJointChain(Random random, int numberOfJoints) { return nextPrismaticJointChain(random, "", numberOfJoints); }
/** * Generates a random kinematic chain composed of rigid-bodies and prismatic joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param numberOfJoints how many joints the kinematic chain should be composed of. * @return the list of all the joints composing the kinematic chain. */ public static List<PrismaticJoint> nextPrismaticJointChain(Random random, String prefix, int numberOfJoints) { return nextPrismaticJointChain(random, prefix, MecanoRandomTools.nextVector3DArray(random, numberOfJoints, 1.0)); }
/** * Generates a random kinematic chain composed of rigid-bodies and prismatic joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param rootBody the root to which the kinematic chain is to be attached. * @param numberOfJoints how many joints the kinematic chain should be composed of. * @return the list of all the joints composing the kinematic chain. */ public static List<PrismaticJoint> nextPrismaticJointChain(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { return nextPrismaticJointChain(random, prefix, rootBody, MecanoRandomTools.nextVector3DArray(random, numberOfJoints, 1.0)); }
/** * Generates a random kinematic chain composed of rigid-bodies and prismatic joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param jointAxes array containing in order the axis for each joint. The length of the array also * defines the number of joints for the generated kinematic chain. * @return the list of all the joints composing the kinematic chain. */ public static List<PrismaticJoint> nextPrismaticJointChain(Random random, String prefix, Vector3DReadOnly[] jointAxes) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody(prefix + "RootBody", worldFrame); return nextPrismaticJointChain(random, prefix, rootBody, jointAxes); }
@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 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); } } }