public static RigidBody getRootBody(RigidBody body) { RigidBody ret = body; while (ret.getParentJoint() != null) { ret = ret.getParentJoint().getPredecessor(); } return ret; }
private static int determineSize(RigidBody[] rigidBodiesInOrder) { int ret = 0; for (RigidBody rigidBody : rigidBodiesInOrder) { ret += rigidBody.getParentJoint().getDegreesOfFreedom(); } return ret; }
private static int[] createMassMatrixIndices(RigidBody[] rigidBodiesInOrder) { int[] ret = new int[rigidBodiesInOrder.length]; int currentIndex = 0; for (int i = 0; i < rigidBodiesInOrder.length; i++) { RigidBody rigidBody = rigidBodiesInOrder[i]; ret[i] = currentIndex; currentIndex += rigidBody.getParentJoint().getDegreesOfFreedom(); } return ret; }
@Override public ReferenceFrame getFrameAfterParentJoint() { return rigidBody.getParentJoint().getFrameAfterJoint(); }
@Override public ReferenceFrame getFrameAfterParentJoint() { return rigidBody.getParentJoint().getFrameAfterJoint(); }
public String getParentJointName() { return contactingRigidBody.getParentJoint().getName(); }
public ReferenceFrame getEstimationFrame() { return estimationLink.getParentJoint().getFrameAfterJoint(); }
public YoPointVelocityDataObject(String namePrefix, RigidBody body, YoVariableRegistry registry) { this(namePrefix, body.getParentJoint().getFrameAfterJoint(), registry); rigidBodyName = body.getName(); }
public static boolean isAncestor(RigidBody candidateDescendant, RigidBody ancestor) { RigidBody currentBody = candidateDescendant; while (!currentBody.isRootBody()) { if (currentBody == ancestor) { return true; } currentBody = currentBody.getParentJoint().getPredecessor(); } return currentBody == ancestor; }
public static int computeDistanceToAncestor(RigidBody descendant, RigidBody ancestor) { int ret = 0; RigidBody currentBody = descendant; while (!currentBody.isRootBody() && (currentBody != ancestor)) { ret++; currentBody = currentBody.getParentJoint().getPredecessor(); } if (currentBody != ancestor) ret = -1; return ret; }
public IMUDefinition(String name, RigidBody rigidBody, RigidBodyTransform transformFromIMUToJoint) { this.name = name; this.rigidBody = rigidBody; this.transformFromIMUToJoint = new RigidBodyTransform(transformFromIMUToJoint); ReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint(); imuFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name, frameAfterJoint, transformFromIMUToJoint); }
private final static ListOfPointsContactablePlaneBody createListOfPointsContactablePlaneBody(String name, RigidBody body, RigidBodyTransform contactPointsTransform, List<Point2d> contactPoints) { ReferenceFrame parentFrame = body.getParentJoint().getFrameAfterJoint(); ReferenceFrame contactPointsFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name, parentFrame, contactPointsTransform); ListOfPointsContactablePlaneBody ret = new ListOfPointsContactablePlaneBody(body, contactPointsFrame, contactPoints); return ret; } }
public ForceSensorDefinition(String sensorName, RigidBody rigidBody, RigidBodyTransform transformFromSensorToParentJoint) { this.sensorName = sensorName; this.rigidBody = rigidBody; InverseDynamicsJoint parentJoint = rigidBody.getParentJoint(); this.parentJointName = parentJoint.getName(); this.transformFromSensorToParentJoint = new RigidBodyTransform(transformFromSensorToParentJoint); ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); sensorFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(sensorName + "Frame", frameAfterJoint, transformFromSensorToParentJoint); }
public static Footstep generateStandingFootstep(RobotSide side, RigidBody foot, ReferenceFrame soleFrame) { FramePose footFramePose = new FramePose(foot.getParentJoint().getFrameAfterJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(foot, side, soleFrame, footstepPoseFrame, trustHeight); return footstep; }
private void setOffDiagonalTerms(int i) { int parentIndex; int j = i; while (isValidParentIndex(parentIndex = parentMap[j])) { ReferenceFrame parentFrame = allRigidBodiesInOrder[parentIndex].getInertia().getExpressedInFrame(); changeFrameOfMomenta(parentFrame); j = parentIndex; GeometricJacobian motionSubspace = allRigidBodiesInOrder[j].getParentJoint().getMotionSubspace(); setMassMatrixPart(i, j, motionSubspace); } }
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis, boolean isPartOfClosedKinematicLoop) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; return new PassiveRevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis), isPartOfClosedKinematicLoop); }
public static PrismaticJoint addPrismaticJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; PrismaticJoint joint = new PrismaticJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis)); return joint; }
public static RevoluteJoint addRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; RevoluteJoint joint = new RevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis)); return joint; }
private Footstep createFootstepAtCurrentLocation(RobotSide robotSide) { ContactablePlaneBody foot = feet.get(robotSide); ReferenceFrame footReferenceFrame = foot.getRigidBody().getParentJoint().getFrameAfterJoint(); FramePose framePose = new FramePose(footReferenceFrame); framePose.changeFrame(worldFrame); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", framePose); boolean trustHeight = true; Footstep footstep = new Footstep(foot.getRigidBody(), robotSide, foot.getSoleFrame(), poseReferenceFrame, trustHeight); return footstep; }
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; }