/** * 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; }
/** * 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 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<RevoluteJoint> nextRevoluteJointChain(Random random, String prefix, RigidBodyBasics rootBody, Vector3DReadOnly[] jointAxes) { RigidBodyBasics predecessor = rootBody; List<RevoluteJoint> revoluteJoints = new ArrayList<>(); for (int i = 0; i < jointAxes.length; i++) { RevoluteJoint joint = nextRevoluteJoint(random, prefix + "Joint" + i, jointAxes[i], predecessor); revoluteJoints.add(joint); predecessor = nextRigidBody(random, prefix + "Body" + i, joint); } return revoluteJoints; }
predecessor = nextRigidBody(random, prefix + "Body" + i, joint);
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); }
private void addSpine(Random random) { RevoluteJoint spineRoll = MultiBodySystemRandomTools.nextRevoluteJoint(random, "spineRoll", roll, pelvis); RigidBodyBasics trunnion1 = MultiBodySystemRandomTools.nextRigidBody(random, "spineTrunnion1", spineRoll); RevoluteJoint spinePitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, "spinePitch", pitch, trunnion1); RigidBodyBasics trunnion2 = MultiBodySystemRandomTools.nextRigidBody(random, "spineTrunnion2", spinePitch); RevoluteJoint spineYaw = MultiBodySystemRandomTools.nextRevoluteJoint(random, "spineYaw", yaw, trunnion2); spineJoints.put(SpineJointName.SPINE_ROLL, spineRoll); spineJoints.put(SpineJointName.SPINE_PITCH, spinePitch); spineJoints.put(SpineJointName.SPINE_YAW, spineYaw); oneDoFJoints.put(spineRoll.getName(), spineRoll); oneDoFJoints.put(spinePitch.getName(), spinePitch); oneDoFJoints.put(spineYaw.getName(), spineYaw); }
/** * Generates a random kinematic tree 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 tree, i.e. every rigid-body can have one or more * child joint(s). * </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 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<PrismaticJoint> nextPrismaticJointTree(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<PrismaticJoint> prismaticJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { PrismaticJoint joint = nextPrismaticJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); prismaticJoints.add(joint); predecessor = prismaticJoints.get(random.nextInt(prismaticJoints.size())).getSuccessor(); } return SubtreeStreams.from(PrismaticJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
/** * 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 prefix provides a common prefix used for all the joint and rigid-body names. * @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, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<RevoluteJoint> revoluteJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { RevoluteJoint joint = nextRevoluteJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); revoluteJoints.add(joint); predecessor = revoluteJoints.get(random.nextInt(revoluteJoints.size())).getSuccessor(); } return SubtreeStreams.from(RevoluteJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
/** * Generates a random kinematic tree 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 tree, i.e. every rigid-body can have one or more * child joint(s). * </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 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<OneDoFJoint> nextOneDoFJointTree(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<OneDoFJoint> oneDoFJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint joint = nextOneDoFJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); oneDoFJoints.add(joint); predecessor = oneDoFJoints.get(random.nextInt(oneDoFJoints.size())).getSuccessor(); } return SubtreeStreams.from(OneDoFJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
nextRigidBody(random, prefix + "Body" + i, joint); joints.add(joint); predecessor = joints.get(random.nextInt(joints.size())).getSuccessor();
/** * 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.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); }
RigidBodyBasics hipTrunnion = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_hipTrunnion", hipYaw); RigidBodyBasics hip = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_hip", hipRoll); RigidBodyBasics upperLeg = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_upperLeg", hipPitch); RigidBodyBasics lowerLeg = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_lowerLeg", kneePitch); RigidBodyBasics ankleTrunnion = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_ankleTrunnion", ankleRoll); RigidBodyBasics foot = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_foot", anklePitch);
RigidBodyBasics shoulderTrunnion = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_shoulderTrunnion", shoulderYaw); RigidBodyBasics shoulder = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_shoulder", shoulderRoll); RigidBodyBasics upperArm = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_upperArm", shoulderPitch); RigidBodyBasics lowerArm = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_lowerArm", elbowPitch); RigidBodyBasics forearm = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_forearm", elbowYaw); RigidBodyBasics wristTrunnion1 = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_wristTrunnion1", firstWristPitch); RigidBodyBasics hand = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_hand", wristRoll);
pelvis = MultiBodySystemRandomTools.nextRigidBody(random, "pelvis", rootJoint); chest = MultiBodySystemRandomTools.nextRigidBody(random, "chest", spineJoints.get(SpineJointName.SPINE_YAW)); head = MultiBodySystemRandomTools.nextRigidBody(random, "head", neckJoints.get(NeckJointName.DISTAL_NECK_PITCH));