public static RigidBody addRandomRigidBody(String name, Random random, InverseDynamicsJoint parentJoint) { Matrix3d momentOfInertia = RandomTools.generateRandomDiagonalMatrix3d(random); double mass = random.nextDouble(); Vector3d comOffset = RandomTools.generateRandomVector(random); RigidBody ret = ScrewTools.addRigidBody(name, parentJoint, momentOfInertia, mass, comOffset); return ret; }
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); FramePoint point1 = new FramePoint(soleFrame, new Point3d(footForward, footHalfWidth, 0.0));
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; }
private static RigidBody cloneRigidBody(RigidBody original, String cloneSuffix, InverseDynamicsJoint parentJointOfClone) { FramePoint comOffset = new FramePoint(); original.getCoMOffset(comOffset); comOffset.changeFrame(original.getParentJoint().getFrameAfterJoint()); String nameOriginal = original.getName(); Matrix3d massMomentOfInertiaPartCopy = original.getInertia().getMassMomentOfInertiaPartCopy(); double mass = original.getInertia().getMass(); Vector3d comOffsetCopy = comOffset.getVectorCopy(); RigidBody clone = ScrewTools.addRigidBody(nameOriginal + cloneSuffix, parentJointOfClone, massMomentOfInertiaPartCopy, mass, comOffsetCopy); return clone; }
parentBody = ScrewTools.addRigidBody(attachedLink.getLinkName(), parentJoint, new Matrix3d(), 1.0, new Vector3d()); robotArmRigidBodies.put(attachedLink, parentBody);
ScrewTools.addRigidBody(currentJoint.getName(), currentIDJoint, momentOfInertia, mass, comOffset); ScrewTools.addRigidBody(currentJoint.getName(), currentIDJoint, momentOfInertia, mass, comOffset);
momentOfInertia = InertiaTools.rotate(transform, momentOfInertia); System.out.println(momentOfInertia); RigidBody aBody = ScrewTools.addRigidBody("test", rootJoint, momentOfInertia, mass, inertiaPose); rootJoint.updateFramesRecursively();