/** * Generates a random kinematic chain composed of rigid-bodies and revolute 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<RevoluteJoint> nextRevoluteJointChain(Random random, int numberOfJoints) { return nextRevoluteJointChain(random, "", numberOfJoints); }
/** * Generates a random state and update the given {@code joints} with it. * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. * @param joints the joints to set the state of. Modified. */ public static void nextState(Random random, JointStateType stateToRandomize, JointBasics[] joints) { for (JointBasics joint : joints) nextState(random, stateToRandomize, joint); }
/** * Generates a random state and update the given {@code joints} with it. * <p> * The random state is guaranteed to be within the joint limits. For instance, a random * configuration is constrained to be in: [{@code joint.getJointLimitLower()}, * {@code joint.getJointLimitUpper()}]. * </p> * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. As no limits are imposed on the * joint accelerations, the state to randomize cannot be the acceleration. For generating * random acceleration, please see * {@link #nextState(Random, JointStateType, double, double, OneDoFJointBasics)}. * @param joints the joints to set the state of. Modified. */ public static void nextStateWithinJointLimits(Random random, JointStateType stateToRandomize, OneDoFJointBasics[] joints) { for (OneDoFJointBasics joint : joints) nextStateWithinJointLimits(random, stateToRandomize, joint); }
/** * Generates a 1-DoF joint with random physical parameters and attaches it to the given * {@code predecessor}. * * @param random the random generator to use. * @param name the joint name. * @param predecessor the rigid-body to which the joint is added as a child. * @return the random joint. */ public static OneDoFJoint nextOneDoFJoint(Random random, String name, RigidBodyBasics predecessor) { if (random.nextBoolean()) return nextPrismaticJoint(random, name, predecessor); else return nextRevoluteJoint(random, name, predecessor); }
/** * Generates a joint with random type and physical parameters and attaches it to the given * {@code predecessor}. * * @param random the random generator to use. * @param name the joint name. * @param predecessor the rigid-body to which the joint is added as a child. * @return the random joint. */ public static JointBasics nextJoint(Random random, String name, RigidBodyBasics predecessor) { switch (random.nextInt(6)) { case 0: return nextSixDoFJoint(random, name, predecessor); case 1: return nextPlanarJoint(random, name, predecessor); case 2: return nextSphericalJoint(random, name, predecessor); case 3: return nextPrismaticJoint(random, name, predecessor); case 4: return nextRevoluteJoint(random, name, predecessor); default: return nextFixedJoint(random, name, predecessor); } }
protected void setUpRandomChainRobot() { Random random = new Random(); joints = new ArrayList<RevoluteJoint>(); Vector3D[] jointAxes = {X, Y, Z, X, Z, Z, X, Y, Z, X}; joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "", elevator, jointAxes)); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); elevator.updateFramesRecursively(); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); }
MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(updateDT); List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints); Twist expectedTwist = new Twist(); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); joints.get(0).getPredecessor().updateFramesRecursively();
private void addNeck(Random random) { RevoluteJoint lowerNeckPitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, "lowerNeckPitch", pitch, chest); RigidBodyBasics trunnion1 = MultiBodySystemRandomTools.nextRigidBody(random, "neckTrunnion1", lowerNeckPitch); RevoluteJoint neckYaw = MultiBodySystemRandomTools.nextRevoluteJoint(random, "neckYaw", yaw, trunnion1); RigidBodyBasics trunnion2 = MultiBodySystemRandomTools.nextRigidBody(random, "neckTrunnion2", neckYaw); RevoluteJoint upperNeckPitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, "upperNeckPitch", pitch, trunnion2); neckJoints.put(NeckJointName.PROXIMAL_NECK_PITCH, lowerNeckPitch); neckJoints.put(NeckJointName.DISTAL_NECK_YAW, neckYaw); neckJoints.put(NeckJointName.DISTAL_NECK_PITCH, upperNeckPitch); oneDoFJoints.put(lowerNeckPitch.getName(), lowerNeckPitch); oneDoFJoints.put(neckYaw.getName(), neckYaw); oneDoFJoints.put(upperNeckPitch.getName(), upperNeckPitch); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void treeTest() throws UnreasonableAccelerationException { Random random = new Random(12651L); ArrayList<RevoluteJoint> joints = new ArrayList<>(); RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); RevoluteJoint rootJoint = MultiBodySystemRandomTools.nextRevoluteJoint(random, "rootJoint", elevator); // Just to make sure there is only one root joint for the SCS robot RigidBodyBasics rootBody = MultiBodySystemRandomTools.nextRigidBody(random, "rootBody", rootJoint); int numberOfJoints = 10; joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointTree(random, rootBody, numberOfJoints - 1)); joints.add(0, rootJoint); SCSRobotFromInverseDynamicsRobotModel robot = new SCSRobotFromInverseDynamicsRobotModel("robot", rootJoint); assertAAndADotV(random, joints, elevator, robot, numberOfJoints); }
/** * Creates a new random multi-body system. * * @param random the random generator to use. * @param jointAxes array containing in order the axis for each revolute joint. The length of the * array also defines the number of revolute joints for the generated kinematic chain. */ public RandomFloatingRevoluteJointChain(Random random, Vector3D[] jointAxes) { elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame()); rootJoint = new SixDoFJoint("rootJoint", elevator); RigidBody rootBody = nextRigidBody(random, "rootBody", rootJoint); revoluteJoints = nextRevoluteJointChain(random, "test", rootBody, jointAxes); joints.add(rootJoint); joints.addAll(revoluteJoints); }
@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); } } }
List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointTree(random, numberOfJoints); RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getPredecessor()); RigidBodyBasics rootBodyInFuture = MultiBodySystemFactories.cloneMultiBodySystem(rootBody, worldFrame, "Test"); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints);
List<RevoluteJoint> revoluteJoints = MultiBodySystemRandomTools.nextRevoluteJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, revoluteJoints.get(random.nextInt(numberOfJoints)).getPredecessor()); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -10.0, 10.0, revoluteJoints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, revoluteJoints); twistCalculator.compute();
List<PrismaticJoint> prismaticJoints = MultiBodySystemRandomTools.nextPrismaticJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, prismaticJoints.get(random.nextInt(numberOfJoints)).getPredecessor()); MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -10.0, 10.0, prismaticJoints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, prismaticJoints); twistCalculator.compute();
/** * 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 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, RigidBodyBasics rootBody, Vector3DReadOnly[] jointAxes) { RigidBodyBasics predecessor = rootBody; List<PrismaticJoint> prismaticJoints = new ArrayList<>(); for (int i = 0; i < jointAxes.length; i++) { PrismaticJoint joint = nextPrismaticJoint(random, prefix + "Joint" + i, jointAxes[i], predecessor); prismaticJoints.add(joint); predecessor = nextRigidBody(random, prefix + "Body" + i, joint); } return prismaticJoints; }
/** * Generates a random kinematic chain composed of rigid-bodies and 1-DoF 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 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<OneDoFJoint> nextOneDoFJointChain(Random random, String prefix, RigidBodyBasics rootBody, Vector3DReadOnly[] jointAxes) { RigidBodyBasics predecessor = rootBody; List<OneDoFJoint> oneDoFJoints = new ArrayList<>(); for (int i = 0; i < jointAxes.length; i++) { OneDoFJoint joint = nextOneDoFJoint(random, prefix + "Joint" + i, jointAxes[i], predecessor); oneDoFJoints.add(joint); predecessor = nextRigidBody(random, prefix + "Body" + i, joint); } return oneDoFJoints; }
JointBasics joint = nextJoint(random, prefix + "Joint" + i, predecessor); joints.add(joint); predecessor = nextRigidBody(random, prefix + "Body" + i, joint);
/** * Generates a random kinematic tree composed of rigid-bodies and revolute joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic tree, i.e. every rigid-body can have one or more * child joint(s). * </p> * * @param random the random generator to use. * @param rootBody the root to which the kinematic tree is to be attached. * @param numberOfJoints how many joints the kinematic tree should be composed of. * @return the list of all the joints composing the kinematic tree. */ public static List<RevoluteJoint> nextRevoluteJointTree(Random random, RigidBodyBasics rootBody, int numberOfJoints) { return nextRevoluteJointTree(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 1-DoF 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<OneDoFJoint> nextOneDoFJointChain(Random random, int numberOfJoints) { return nextOneDoFJointChain(random, "", numberOfJoints); }