/** * Builds a new implementation of {@code RevoluteJointBasics}. * * @param name the joint name. * @param predecessor the predecessor of the joint. * @param transformToParent the transform to the frame after the parent joint. * @param jointAxis the joint axis. * @return the new revolute joint. */ default RevoluteJointBasics buildRevoluteJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, Vector3DReadOnly jointAxis) { return new RevoluteJoint(name, predecessor, transformToParent, jointAxis); }
/** * 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 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 RevoluteJoint nextRevoluteJoint(Random random, String name, Vector3DReadOnly jointAxis, RigidBodyBasics predecessor) { RigidBodyTransform transformToParent = predecessor.isRootBody() ? null : EuclidCoreRandomTools.nextRigidBodyTransform(random); return new RevoluteJoint(name, predecessor, transformToParent, jointAxis); }
private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(jointName, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(name, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createHand(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand", joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createHand(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y", parentBody, new Vector3D(0.0, 0.0, SHIN_LENGTH), Y); joint.setQ(Math.toRadians(30)); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand", joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createHand(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand" + suffix, joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createHand(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, SHIN_LENGTH), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand" + suffix, joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createLowerArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y", parentBody, new Vector3D(0.0, 0.0, THIGH_LENGTH), Y); joint.setQ(Math.toRadians(40)); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm", joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createLowerArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y", parentBody, new Vector3D(0.0, 0.0, THIGH_LENGTH), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm", joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createLowerArm(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, THIGH_LENGTH), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm" + suffix, joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(Math.toRadians(20)); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createLowerArm(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm" + suffix, joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private void initializeFourBar(RigidBodyTransform jointAtoElevator, RigidBodyTransform jointBtoA, RigidBodyTransform jointCtoB, RigidBodyTransform jointDtoC, Vector3D jointAxisA, Vector3D jointAxisB, Vector3D jointAxisC, Vector3D jointAxisD) { masterJointA = new RevoluteJoint("jointA", elevator, jointAtoElevator, jointAxisA); rigidBodyAB = createAndAttachCylinderRB("rigidBodyAB", masterJointA); passiveJointB = ScrewTools.addPassiveRevoluteJoint("jointB", rigidBodyAB, jointBtoA, jointAxisB, true); rigidBodyBC = createAndAttachCylinderRB("rigidBodyBC", passiveJointB); passiveJointC = ScrewTools.addPassiveRevoluteJoint("jointC", rigidBodyBC, jointCtoB, jointAxisC, true); rigidBodyCD = createAndAttachCylinderRB("rigidBodyCD", passiveJointC); passiveJointD = ScrewTools.addPassiveRevoluteJoint("jointD", rigidBodyCD, jointDtoC, jointAxisD, true); rigidBodyDA = createAndAttachCylinderRB("rigidBodyCD", passiveJointD); masterJointA.setQ(random.nextDouble()); passiveJointB.setQ(random.nextDouble()); passiveJointC.setQ(random.nextDouble()); passiveJointD.setQ(random.nextDouble()); elevator.updateFramesRecursively(); }
@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())); }
public FootSpoof(String name, double xToAnkle, double yToAnkle, double zToAnkle, List<Point2D> contactPoints2dInSoleFrame, double coefficientOfFriction) { RigidBodyTransform transformToAnkle = new RigidBodyTransform(); transformToAnkle.setTranslation(new Vector3D(-xToAnkle, -yToAnkle, -zToAnkle)); // if(FootstepUtilsTest.DEBUG_TESTS) // System.out.println("FootSpoof: making transform from plane to ankle equal to "+transformToAnkle); shinFrame = new PoseReferenceFrame(name + "ShinFrame", ReferenceFrame.getWorldFrame()); this.shin = new RigidBody(name, shinFrame); this.ankle = new RevoluteJoint(name + "Ankle", shin, new RigidBodyTransform(), new Vector3D(0.0, 1.0, 0.0)); this.foot = new RigidBody(name, ankle, new Matrix3D(), 1.0, new RigidBodyTransform()); soleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(name + "soleFrame", ankle.getFrameAfterJoint(), transformToAnkle); for (Point2D contactPointInSoleFrame : contactPoints2dInSoleFrame) { FramePoint3D point = new FramePoint3D(soleFrame, contactPointInSoleFrame.getX(), contactPointInSoleFrame.getY(), 0.0); contactPoints.add(point); contactPoints2d.add(new FramePoint2D(point)); } totalNumberOfContactPoints = contactPoints.size(); this.coefficientOfFriction = coefficientOfFriction; }
private void buildMechanismAndJacobians() { RigidBodyBasics base = new RigidBody("base", ReferenceFrame.getWorldFrame()); joint1 = new PrismaticJoint("joint1", base, new Vector3D(), new Vector3D(0.0, -1.0, 0.0)); RigidBodyBasics body1 = new RigidBody("body1", joint1, new Matrix3D(), 0.0, new Vector3D()); joint2 = new RevoluteJoint("joint2", body1, new Vector3D(0.0, 0.0, 3.0), new Vector3D(-1.0, 0.0, 0.0)); RigidBodyBasics body2 = new RigidBody("body2", joint2, new Matrix3D(), 0.0, new Vector3D()); joint3 = new PrismaticJoint("joint4", body2, new Vector3D(), new Vector3D(0.0, 0.0, 1.0)); RigidBodyBasics body3 = new RigidBody("body3", joint3, new Matrix3D(), 0.0, new Vector3D()); // create Jacobians bodyManipulatorJacobian = new GeometricJacobian(base, body3, joint3.getFrameAfterJoint()); spatialManipulatorJacobian = new GeometricJacobian(base, body3, joint1.getFrameBeforeJoint()); } }