public static RevoluteJoint addRandomRevoluteJoint(String name, Vector3d jointAxis, Random random, RigidBody predecessor) { Vector3d jointOffset = RandomTools.generateRandomVector(random); jointAxis.normalize(); RevoluteJoint ret = ScrewTools.addRevoluteJoint(name, predecessor, jointOffset, jointAxis); return ret; }
public static RevoluteJoint addRevoluteJoint(String jointName, RigidBody parentBody, Vector3d jointOffset, Vector3d jointAxis) { RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setTranslationAndIdentityRotation(jointOffset); return addRevoluteJoint(jointName, parentBody, transformToParent, jointAxis); }
private static OneDoFJoint cloneOneDoFJoint(OneDoFJoint original, String cloneSuffix, RigidBody clonePredecessor) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = original.getOffsetTransform3D(); Vector3d jointAxisCopy = original.getJointAxis().getVectorCopy(); OneDoFJoint clone; if (original instanceof RevoluteJoint) clone = ScrewTools.addRevoluteJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else if (original instanceof PrismaticJoint) clone = ScrewTools.addPrismaticJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else throw new RuntimeException("Unhandled type of " + OneDoFJoint.class.getSimpleName() + ": " + original.getClass().getSimpleName()); clone.setJointLimitLower(original.getJointLimitLower()); clone.setJointLimitUpper(original.getJointLimitUpper()); return clone; }
this.ankle = ScrewTools.addRevoluteJoint(name + "Ankle", shin, new RigidBodyTransform(), new Vector3d(0.0, 1.0, 0.0)); this.foot = ScrewTools.addRigidBody(name, ankle, new Matrix3d(), 1.0, new RigidBodyTransform()); soleFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name + "soleFrame", ankle.getFrameAfterJoint(), transformToAnkle);
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 = ScrewTools.addRevoluteJoint(name + "Ankle", shin, new RigidBodyTransform(), new Vector3d(0.0, 1.0, 0.0)); this.foot = ScrewTools.addRigidBody(name, ankle, new Matrix3d(), 1.0, new RigidBodyTransform()); soleFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name + "soleFrame", ankle.getFrameAfterJoint(), transformToAnkle); for (Point2d contactPointInSoleFrame : contactPoints2dInSoleFrame) { FramePoint point = new FramePoint(soleFrame, contactPointInSoleFrame.getX(), contactPointInSoleFrame.getY(), 0.0); contactPoints.add(point); contactPoints2d.add(point.toFramePoint2d()); } totalNumberOfContactPoints = contactPoints.size(); this.coefficientOfFriction = coefficientOfFriction; }
parentJoint = ScrewTools.addRevoluteJoint(armJoint.getJointName(false), parentBody, armJoint.getJointOffset(), armJoint.getJointAxis()); robotArmRevoluteJoints.put(armJoint, parentJoint);
currentJoint.getOffset(jointOffset); RevoluteJoint currentIDJoint = ScrewTools.addRevoluteJoint(currentJoint.getName(), parentIDBody, jointOffset, jointAxis);