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); }
endEffectors.get(robotSide).put(LimbName.LEG, foot); oneDoFJoints.put(hipYaw.getName(), hipYaw); oneDoFJoints.put(hipRoll.getName(), hipRoll); oneDoFJoints.put(hipPitch.getName(), hipPitch); oneDoFJoints.put(kneePitch.getName(), kneePitch); oneDoFJoints.put(ankleRoll.getName(), ankleRoll); oneDoFJoints.put(anklePitch.getName(), anklePitch);
endEffectors.get(robotSide).put(LimbName.ARM, hand); oneDoFJoints.put(shoulderYaw.getName(), shoulderYaw); oneDoFJoints.put(shoulderRoll.getName(), shoulderRoll); oneDoFJoints.put(shoulderPitch.getName(), shoulderPitch); oneDoFJoints.put(elbowPitch.getName(), elbowPitch); oneDoFJoints.put(elbowYaw.getName(), elbowYaw); oneDoFJoints.put(firstWristPitch.getName(), firstWristPitch); oneDoFJoints.put(wristRoll.getName(), wristRoll);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testExtractRevoluteJoints() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); RevoluteJoint[] revoluteJoints = MultiBodySystemTools.filterJoints(jointsArr, RevoluteJoint.class); assertEquals(jointsArr.length - 1, revoluteJoints.length); for(int i = 0; i < revoluteJoints.length; i++) { assertEquals("testJoint" + i, revoluteJoints[i].getName()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRevoluteJoint_String_RigidBody_Transform3D_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); RigidBodyTransform transformToParent = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, transformToParent, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); assertTrue(jointAxis.equals(joint.getJointAxis())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRevoluteJoint_String_RigidBody_Vector3d_Vector3d() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); RigidBodyBasics[] rootBodies = {chain.getElevator()}; JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(rootBodies); RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator()); RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1]; bodiesArray[0] = chain.getElevator(); for(int i = 0; i < partialBodiesArray.length; i++) { bodiesArray[i+1] = partialBodiesArray[i]; } String jointName = "joint"; RigidBodyBasics parentBody = bodiesArray[bodiesArray.length - 1]; Vector3D jointOffset = RandomGeometry.nextVector3D(random, 5.0); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); assertTrue(jointAxis.equals(joint.getJointAxis())); }
masterJointA.getName() + " is set outside of its bounds [" + masterJointA.getJointLimitLower() + ", " + masterJointA.getJointLimitUpper() + "]. The current value is: " + masterJointA.getQ());
System.out.println("\nOutput joint: " + fourBarOutputJoint.getName() + "\n"); double qA = masterJointA.getQ(); double qB = passiveJointB.getQ();