public MovingZUpFrame(MovingReferenceFrame nonZUpFrame, String name) { super(name, nonZUpFrame.getRootFrame(), true); this.rootFrame = nonZUpFrame.getRootFrame(); this.nonZUpFrame = nonZUpFrame; }
private boolean isTwistOfFrameUpToDateRecursive() { return isTwistOfFrameUpToDate && (parentMovingFrame == null || parentMovingFrame.isTwistOfFrameUpToDateRecursive()); }
/** * Gets the twist of this frame with respect the closest stationary frame, i.e. not moving with * respect to its root frame. * <p> * The returned twist the twist of {@code this} with respect to (usually) * {@link ReferenceFrame#getWorldFrame()} and expressed in {@code this}. * </p> * * @return the absolute velocity of this frame. The returned object should not be modified. */ public TwistReadOnly getTwistOfFrame() { updateTwistOfFrame(); return twistOfFrame; }
public TwistReadOnly getBodyTwist() { return getBodyFixedFrame().getTwistOfFrame(); }
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; }
/** {@inheritDoc} */ @Override public void update() { centerOfMassJacobian.reset(); super.update(); }
public void holdConfiguration(RigidBodyBasics rigidBody) { if (hasTrajectoryCommand) { initialGuessHolder.set(initialGuessCandidate); } else { initialGuessHolder.set(rigidBody.getBodyFixedFrame().getTransformToWorldFrame()); } }
/** * Packs the offset from the frame before this joint to the frame after this parent joint. * * @param jointOffsetTransformToPack the transform in which this joint's offset is stored. * Modified. */ default void getJointOffset(RigidBodyTransform jointOffsetTransformToPack) { getFrameBeforeJoint().getTransformToParent(jointOffsetTransformToPack); }
public TwistReadOnly getBodyTwist() { return getBodyFixedFrame().getTwistOfFrame(); }
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; }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { // revoluteJoint.setQ(revoluteJoint.getQ() + RandomTools.generateRandomDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.setQ(RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }
@Override public RigidBodyTransform getOffsetTransform3D() { return new RigidBodyTransform(jointToWrap.getFrameBeforeJoint().getTransformToParent()); }
/** * Packs the twist of this frame with respect the closest stationary frame, i.e. not moving with * respect to its root frame. * <p> * The returned twist the twist of {@code this} with respect to (usually) * {@link ReferenceFrame#getWorldFrame()} and expressed in {@code this}. * </p> * * @param twistToPack the twist in which the absolute velocity of this frame is stored. Modified. */ public void getTwistOfFrame(Twist twistToPack) { twistToPack.setIncludingFrame(getTwistOfFrame()); }
public MovingWalkingReferenceFrame(String name, MovingReferenceFrame pelvisFrame, MovingMidFootZUpGroundFrame midFootZUpGroundFrame) { super(name, pelvisFrame.getRootFrame(), true); if (pelvisFrame == midFootZUpGroundFrame) throw new IllegalArgumentException("The frames have to be different."); pelvisFrame.verifySameRoots(midFootZUpGroundFrame); this.pelvisFrame = pelvisFrame; this.midFootZUpGroundFrame = midFootZUpGroundFrame; }
@Override public RigidBodyTransform getJointTransform3D() { return new RigidBodyTransform(jointToWrap.getFrameAfterJoint().getTransformToParent()); } };
public void get(FrameVector3D frameVectorToPack) { referenceFrame.getTwistOfFrame(twist); frameVectorToPack.setIncludingFrame(twist.getAngularPart()); } }
private static RigidBodyTransform getCloneJointTransformToParent(JointReadOnly original) { if (original.getFrameBeforeJoint() == original.getPredecessor().getBodyFixedFrame()) return null; else return original.getFrameBeforeJoint().getTransformToParent(); }
public void get(FrameVector3D frameVectorToPack) { referenceFrame.getTwistOfFrame(twist); frameVectorToPack.setIncludingFrame(twist.getLinearPart()); } }
public void getLinearVelocity(FrameVector3D linearVelocityToPack) { frameOfInterest.getTwistOfFrame(twist); linearVelocityToPack.setIncludingFrame(twist.getLinearPart()); }
public void getAngularVelocity(FrameVector3D angularVelocityToPack) { frameOfInterest.getTwistOfFrame(twist); angularVelocityToPack.setIncludingFrame(twist.getAngularPart()); } }