/** * Creates a new root rigid-body to which the first joint of a robot kinematic chain can be * added. * <p> * Note that there is only one root body per robot. * </p> * * @param bodyName the name for this rigid-body. * @param transformToParent provides the pose of this rigid-body's body-fixed-frame with respect * to the {@code parentStationaryFrame}. Not modified. * @param parentStationaryFrame the parent stationary, i.e. non-moving with respect to world * frame, frame to which this rigid-body will create and attach its body fixed frame. * Most of the time {@code parentStationaryFrame == ReferenceFrame.getWorldFrame()}. */ public RigidBody(String bodyName, RigidBodyTransform transformToParent, ReferenceFrame parentStationaryFrame) { if (bodyName == null) throw new IllegalArgumentException("Name can not be null"); name = bodyName; inertia = null; bodyFixedFrame = MovingReferenceFrame.constructFrameFixedInParent(bodyName + "Frame", parentStationaryFrame, transformToParent); parentJoint = null; nameId = bodyName; }
public FullQuadrupedRobotModelFromDescription(RobotQuadrant[] robotQuadrants, RobotDescription description, QuadrupedJointNameMap sdfJointNameMap, String[] sensorLinksToTrack) { super(description, sdfJointNameMap, sensorLinksToTrack); this.robotQuadrants = robotQuadrants; for (OneDoFJointBasics oneDoFJoint : getOneDoFJoints()) { QuadrupedJointName quadrupedJointName = sdfJointNameMap.getJointNameForSDFName(oneDoFJoint.getName()); jointNameOneDoFJointBiMap.put(quadrupedJointName, oneDoFJoint); // Assign default joint limits JointLimit jointLimit = new JointLimit(oneDoFJoint); jointLimits.put(quadrupedJointName, jointLimit); jointLimitData.put(oneDoFJoint, new JointLimitData(oneDoFJoint)); } for (RobotQuadrant robotQuadrant : robotQuadrants) { RigidBodyTransform soleToParentTransform = sdfJointNameMap.getSoleToParentFrameTransform(robotQuadrant); MovingReferenceFrame soleFrame = MovingReferenceFrame .constructFrameFixedInParent(robotQuadrant.toString() + "SoleFrame", getEndEffectorFrame(robotQuadrant, LimbName.LEG), soleToParentTransform); soleFrames.put(robotQuadrant, soleFrame); } }
MovingReferenceFrame soleFrame = MovingReferenceFrame.constructFrameFixedInParent(sidePrefix + "Sole", getEndEffectorFrame(robotSide, LimbName.LEG), soleToAnkleTransform); soleFrames.put(robotSide, soleFrame);
private RigidBody(String bodyName, JointBasics parentJoint, RigidBodyTransform inertiaPose) { if (bodyName == null) throw new IllegalArgumentException("Name can not be null"); name = bodyName; this.parentJoint = parentJoint; ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); bodyFixedFrame = MovingReferenceFrame.constructFrameFixedInParent(bodyName + "CoM", frameAfterJoint, inertiaPose); inertia = new SpatialInertia(bodyFixedFrame, bodyFixedFrame); // inertia should be expressed in body frame, otherwise it will change inertia.getBodyFrame().checkReferenceFrameMatch(inertia.getReferenceFrame()); parentJoint.setSuccessor(this); nameId = parentJoint.getPredecessor().getNameId() + NAME_ID_SEPARATOR + bodyName; }
/** * Creates the reference frame that is at the joint and rigidly attached to the joint's predecessor. * * @param joint the joint to which the new frame is for. Not modified. * @param transformToParent the transform from the new frame to the frame after the parent joint. * Not modified. * @param beforeJointName the name of the new frame. * @return the new frame before joint. */ public static MovingReferenceFrame newJointFrame(JointReadOnly joint, RigidBodyTransform transformToParent, String beforeJointName) { MovingReferenceFrame parentFrame; RigidBodyReadOnly parentBody = joint.getPredecessor(); if (parentBody.isRootBody()) { parentFrame = parentBody.getBodyFixedFrame(); /* * TODO Special case to keep the beforeJointFrame of the SixDoFJoint to be the elevatorFrame. This * should probably removed, might cause reference frame exceptions though. */ if (transformToParent == null) return parentFrame; } else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); return MovingReferenceFrame.constructFrameFixedInParent(beforeJointName, parentFrame, transformToParent); }
MovingReferenceFrame soleFrame = MovingReferenceFrame.constructFrameFixedInParent(sidePrefix + "Sole", getEndEffectorFrame(robotSide, LimbName.LEG), soleToAnkleTransform); soleFrames.put(robotSide, soleFrame); MovingReferenceFrame attachmentPlateFrame = MovingReferenceFrame.constructFrameFixedInParent(sidePrefix + "HandControlFrame", getEndEffectorFrame(robotSide, LimbName.ARM), handControlFrameToWristTransform); handControlFrames.put(robotSide, attachmentPlateFrame);
RigidBodyTransform rightSoleToAnkleFrame = TransformTools.createTranslationTransform(footLength / 2.0 - footBack + toFootCenterX, -toFootCenterY, -ankleHeight); MovingReferenceFrame leftSoleFrame = MovingReferenceFrame.constructFrameFixedInParent("Left_Sole", leftFootBody.getBodyFixedFrame(), leftSoleToAnkleFrame); MovingReferenceFrame rightSoleFrame = MovingReferenceFrame.constructFrameFixedInParent("Right_Sole", rightFootBody.getBodyFixedFrame(), rightSoleToAnkleFrame);