/** * 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); }
@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); }
/** * Generates a revolute 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 RevoluteJoint nextRevoluteJoint(Random random, String name, RigidBodyBasics predecessor) { Vector3D jointAxis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); return nextRevoluteJoint(random, name, jointAxis, predecessor); }
/** * 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 jointAxis used to define the joint axis. * @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, Vector3DReadOnly jointAxis, RigidBodyBasics predecessor) { if (random.nextBoolean()) return nextPrismaticJoint(random, name, jointAxis, predecessor); else return nextRevoluteJoint(random, name, jointAxis, predecessor); }
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 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; }
/** * 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()); }
RevoluteJoint hipYaw = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_hipYaw", yaw, pelvis); RigidBodyBasics hipTrunnion = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_hipTrunnion", hipYaw); RevoluteJoint hipRoll = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_hipRoll", roll, hipTrunnion); RigidBodyBasics hip = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_hip", hipRoll); RevoluteJoint hipPitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_hipPitch", pitch, hip); RigidBodyBasics upperLeg = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_upperLeg", hipPitch); RevoluteJoint kneePitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_kneePitch", pitch, upperLeg); RigidBodyBasics lowerLeg = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_lowerLeg", kneePitch); RevoluteJoint ankleRoll = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_ankleRoll", roll, lowerLeg); RigidBodyBasics ankleTrunnion = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_ankleTrunnion", ankleRoll); RevoluteJoint anklePitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_anklePitch", pitch, ankleTrunnion); RigidBodyBasics foot = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_foot", anklePitch);
RevoluteJoint shoulderYaw = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_shoulderYaw", yaw, chest); RigidBodyBasics shoulderTrunnion = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_shoulderTrunnion", shoulderYaw); RevoluteJoint shoulderRoll = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_shoulderRoll", roll, shoulderTrunnion); RigidBodyBasics shoulder = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_shoulder", shoulderRoll); RevoluteJoint shoulderPitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_shoulderPitch", pitch, shoulder); RigidBodyBasics upperArm = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_upperArm", shoulderPitch); RevoluteJoint elbowPitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_elbowPitch", pitch, upperArm); RigidBodyBasics lowerArm = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_lowerArm", elbowPitch); RevoluteJoint elbowYaw = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_elbowYaw", yaw, lowerArm); RigidBodyBasics forearm = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_forearm", elbowYaw); RevoluteJoint firstWristPitch = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_firstWristPitch", pitch, forearm); RigidBodyBasics wristTrunnion1 = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_wristTrunnion1", firstWristPitch); RevoluteJoint wristRoll = MultiBodySystemRandomTools.nextRevoluteJoint(random, prefix + "_wristRoll", roll, wristTrunnion1); RigidBodyBasics hand = MultiBodySystemRandomTools.nextRigidBody(random, prefix + "_hand", wristRoll);
/** * 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); } }