public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FramePose3DReadOnly rawPose) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); }
/** * Tests if this pose is exactly equal to {@code other}. * <p> * If the two poses have different frames, this method returns {@code false}. * </p> * * @param other the other pose to compare against this. Not modified. * @return {@code true} if the two poses are exactly equal and are expressed in the same reference * frame, {@code false} otherwise. */ default boolean equals(FramePose3DReadOnly other) { if (other == null || getReferenceFrame() != other.getReferenceFrame()) return false; return Pose3DReadOnly.super.equals(other); } }
/** * Tests if this pose is equal to the given {@code other} to an {@code epsilon}. * <p> * If the two poses have different frames, this method returns {@code false}. * </p> * * @param other the other pose to compare against this. Not modified. * @param epsilon the tolerance to use when comparing.. * @return {@code true} if the two poses are equal and are expressed in the same reference frame, * {@code false} otherwise. */ default boolean epsilonEquals(FramePose3DReadOnly other, double epsilon) { if (getReferenceFrame() != other.getReferenceFrame()) return false; return Pose3DReadOnly.super.epsilonEquals(other, epsilon); }
public PoseReferenceFrame(String frameName, FramePose3DReadOnly pose) { this(frameName, pose.getReferenceFrame()); setPoseAndUpdate(pose); }
/** * Sets this frame pose 3D to the {@code other} frame pose 3D. * * @param other the other frame pose 3D. Not modified. */ default void setIncludingFrame(FramePose3DReadOnly other) { setIncludingFrame(other.getReferenceFrame(), other); } }
/** * Sets this frame pose 2D to the x, y, yaw components and reference frame of the given * {@code framePose3DReadOnly}. * * @param framePose3DReadOnly the frame pose 3D. Not modified. */ default void setIncludingFrame(FramePose3DReadOnly framePose3DReadOnly) { setIncludingFrame(framePose3DReadOnly.getReferenceFrame(), framePose3DReadOnly); } }
public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FramePose3DReadOnly rawPose) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPose, rawPose.getReferenceFrame()); }
/** * Sets this pose 3D to be the same as the given one expressed in the reference frame of this. * <p> * If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to * {@link #set(FramePose3DReadOnly)}. * </p> * <p> * If {@code other} is expressed in a different frame than {@code this}, then {@code this} is set to * {@code other} and then transformed to be expressed in {@code this.getReferenceFrame()}. * </p> * * @param other the other frame pose 3D to set this to. Not modified. */ default void setMatchingFrame(FramePose3DReadOnly other) { Pose3DBasics.super.set(other); other.getReferenceFrame().transformFromThisToDesiredFrame(getReferenceFrame(), this); }
public void setFromAnklePose(FramePose3DReadOnly anklePose, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(anklePose.getOrientation()); tempTransform.setTranslation(anklePose.getPosition()); tempTransform.multiplyInvertOther(transformFromAnkleToSole); footstepPose.setIncludingFrame(anklePose.getReferenceFrame(), tempTransform); }