/** {@inheritDoc} */ @Override public void update() { centerOfMassJacobian.reset(); super.update(); }
public AfterJointReferenceFrameNameMap(RigidBodyBasics base) { for(JointBasics joint : base.childrenSubtreeIterable()) { afterJointReferenceFrames.put(joint.getFrameAfterJoint().getName(), joint.getFrameAfterJoint()); } }
public void updateJointPositions_ID_to_SCS() { if (scsFloatingJoint != null) { idFloatingJoint.getFrameAfterJoint().getTransformToDesiredFrame(transformToWorld, ReferenceFrame.getWorldFrame()); scsFloatingJoint.setRotationAndTranslation(transformToWorld); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setQ(idJoint.getQ()); } }
public MovingMidFrame(String name, MovingReferenceFrame frameOne, MovingReferenceFrame frameTwo) { super(name, frameOne.getRootFrame()); if (frameOne == frameTwo) throw new IllegalArgumentException("The frames have to be different."); frameOne.verifySameRoots(frameTwo); this.frameOne = frameOne; this.frameTwo = frameTwo; }
private static void assertFrameTwistEquals(MovingReferenceFrame expected, MovingReferenceFrame actual, double epsilon) { EuclidCoreTestTools.assertRigidBodyTransformEquals(expected.getTransformToRoot(), actual.getTransformToRoot(), 1.0e-12); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); expected.getTwistOfFrame(expectedTwist); actual.getTwistOfFrame(actualTwist); expectedTwist.setBodyFrame(actual); expectedTwist.changeFrame(actual); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, epsilon); }
verifySameRoots(base); getTwistOfFrame(relativeTwistToPack); relativeTwistToPack.setBaseFrame(base); ((MovingReferenceFrame) base).getTwistOfFrame(relativeTwistToPack); relativeTwistToPack.changeFrame(this); relativeTwistToPack.sub(getTwistOfFrame()); relativeTwistToPack.invert(); throw unhandledReferenceFrameTypeException(base);
public TwistReadOnly getBodyTwist() { return getBodyFixedFrame().getTwistOfFrame(); }
public void holdConfiguration(RigidBodyBasics rigidBody) { if (hasTrajectoryCommand) { initialGuessHolder.set(initialGuessCandidate); } else { initialGuessHolder.set(rigidBody.getBodyFixedFrame().getTransformToWorldFrame()); } }
/** * The first pass consists in calculating the bias wrench resulting from external and Coriolis * forces, and the bias acceleration resulting from the Coriolis acceleration. */ public void passOne() { if (!isRoot()) { MovingReferenceFrame frameAfterJoint = getFrameAfterJoint(); MovingReferenceFrame frameBeforeJoint = getJoint().getFrameBeforeJoint(); if (parent.isRoot()) frameAfterJoint.getTransformToDesiredFrame(transformToParentJointFrame, parent.getBodyFixedFrame()); else frameAfterJoint.getTransformToDesiredFrame(transformToParentJointFrame, parent.getFrameAfterJoint()); bodyInertia.computeDynamicWrench(null, getBodyTwist(), biasWrench); biasWrench.sub(externalWrench); biasWrench.changeFrame(frameAfterJoint); biasAcceleration.setToZero(frameAfterJoint, input.getInertialFrame(), frameBeforeJoint); biasAcceleration.changeFrame(frameAfterJoint, getJoint().getJointTwist(), frameAfterJoint.getTwistOfFrame()); biasAcceleration.get(c); } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).passOne(); }
RigidBodyTransform handTransform = handControlFrame.getTransformToWorldFrame(); handControlFrame.getTransformToDesiredFrame(handControlFrameTransformToBodyFixedFrame, hand.getBodyFixedFrame()); trajectory.getControlFramePositionInEndEffector().set(handControlFrameTransformToBodyFixedFrame.getTranslationVector()); trajectory.getControlFrameOrientationInEndEffector().set(handControlFrameTransformToBodyFixedFrame.getRotationMatrix());
ZUpFrame zUpFrameAfterJoint = new ZUpFrame(frameAfterJoint.getRootFrame(), frameAfterJoint, frameAfterJoint.getName() + "ZUp"); MovingZUpFrame movingZUpFrameAfterJoint = new MovingZUpFrame(frameAfterJoint, frameAfterJoint.getName() + "ZUp");
@Override public SpatialAccelerationReadOnly getRelativeAcceleration(RigidBodyReadOnly base, RigidBodyReadOnly body) { MovingReferenceFrame baseFrame = base.getBodyFixedFrame(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); baseAcceleration.setIncludingFrame(getAccelerationOfBody(base)); acceleration.setIncludingFrame(getAccelerationOfBody(body)); if (areVelocitiesConsidered()) { baseFrame.getTwistRelativeToOther(bodyFrame, deltaTwist); baseAcceleration.changeFrame(bodyFrame, deltaTwist, baseFrame.getTwistOfFrame()); } else { baseAcceleration.changeFrame(bodyFrame); } acceleration.sub(baseAcceleration); return acceleration; }
private void updateTwistOfFrame() { if (isTwistOfFrameUpToDateRecursive()) return; if (parentMovingFrame == null) { if (isFixedInParent) twistOfFrame.setToZero(this, getParent(), this); else twistOfFrame.setIncludingFrame(twistRelativeToParent); } else { twistOfFrame.setIncludingFrame(parentMovingFrame.getTwistOfFrame()); twistOfFrame.changeFrame(this); if (isFixedInParent) twistOfFrame.setBodyFrame(this); else twistOfFrame.add(twistRelativeToParent); } isTwistOfFrameUpToDate = true; for (int i = 0; i < childrenMovingFrames.size(); i++) childrenMovingFrames.get(i).isTwistOfFrameUpToDate = false; }
/** * 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 MovingZUpFrame(MovingReferenceFrame nonZUpFrame, String name) { super(name, nonZUpFrame.getRootFrame(), true); this.rootFrame = nonZUpFrame.getRootFrame(); this.nonZUpFrame = nonZUpFrame; }
predecessorBodyFrame.getTwistRelativeToOther(baseFrame, bodyTwistRelativeToBase);
public TwistReadOnly getBodyTwist() { return getBodyFixedFrame().getTwistOfFrame(); }
@Override protected void setBehaviorInput() { RobotSide initialStanceSide = RobotSide.LEFT; RigidBodyTransform soleToWorld = referenceFrames.getSoleFrame(initialStanceSide).getTransformToWorldFrame(); FramePose3D stanceFootPose = new FramePose3D(ReferenceFrame.getWorldFrame(), soleToWorld); if (goalPose == null) System.err.println("WalkToLocationPlannedBehavior: goal pose NULL"); planPathToLocationBehavior.setInputs(goalPose, stanceFootPose, initialStanceSide); publishTextToSpeack("Planning Path"); } };
public MovingMidFrameZUpFrame(String name, MovingReferenceFrame frameOne, MovingReferenceFrame frameTwo) { super(name, frameOne.getRootFrame(), true); if (frameOne == frameTwo) throw new IllegalArgumentException("The frames have to be different."); frameOne.verifySameRoots(frameTwo); this.frameOne = frameOne; this.frameTwo = frameTwo; }
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); } }