/** * Sets the x-coordinate of the position. * * @param x the x-coordinate of the position. */ default void setX(double x) { getPosition().setX(x); }
/** * Sets the y-coordinate of the position. * * @param y the y-coordinate of the position. */ default void setY(double y) { getPosition().setY(y); }
/** * Sets the position coordinates. * * @param x the x-coordinate of the position. * @param y the y-coordinate of the position. * @param z the z-coordinate of the position. */ default void setPosition(double x, double y, double z) { getPosition().set(x, y, z); }
/** * Sets the x and y coordinates from the given tuple 2D, the z coordinate remains unchanged. * <p> * The z component remains unchanged. * </p> * * @param position2D the tuple with the new x and y coordinates. Not modified. */ default void setPosition(Tuple2DReadOnly position2D) { getPosition().set(position2D); }
/** * Sets the position from the given tuple 2D and z coordinate. * * @param position2D the tuple with the new x and y coordinates. Not modified. * @param z the new z value for this pose's position z-coordinate. */ default void setPosition(Tuple2DReadOnly position2D, double z) { getPosition().set(position2D, z); }
/** * Sets the z-coordinate of the position. * * @param z the z-coordinate of the position. */ default void setZ(double z) { getPosition().setZ(z); }
/** * Prepends a rotation about the z-axis to this pose 3D: Rotates the position part and prepends the * rotation to the orientation part. * * @param yaw the angle to rotate about the z-axis. */ default void prependYawRotation(double yaw) { RotationMatrixTools.applyYawRotation(yaw, getPosition(), getPosition()); getOrientation().prependYawRotation(yaw); }
/** * Prepends a rotation about the y-axis to this pose 3D: Rotates the position part and prepends the * rotation to the orientation part. * * @param pitch the angle to rotate about the y-axis. */ default void prependPitchRotation(double pitch) { RotationMatrixTools.applyPitchRotation(pitch, getPosition(), getPosition()); getOrientation().prependPitchRotation(pitch); }
/** * Prepends a rotation about the x-axis to this pose 3D: Rotates the position part and prepends the * rotation to the orientation part. * * @param roll the angle to rotate about the x-axis. */ default void prependRollRotation(double roll) { RotationMatrixTools.applyRollRotation(roll, getPosition(), getPosition()); getOrientation().prependRollRotation(roll); }
/** {@inheritDoc} */ @Override default boolean containsNaN() { return getOrientation().containsNaN() || getPosition().containsNaN(); }
/** {@inheritDoc} */ @Override default void setToNaN() { getOrientation().setToNaN(); getPosition().setToNaN(); }
/** * Rotates the position part of this pose 3D by the given {@code rotation} and prepends it to the * orientation part. * * @param rotation the rotation to prepend to this pose 3D. Not modified. */ default void prependRotation(Orientation3DReadOnly rotation) { rotation.transform(getPosition()); rotation.transform(getOrientation()); }
/** * Transforms the position and orientation parts of this pose 3D by the inverse of the given * {@code transform}. * * @param transform the geometric transform to apply on this pose 3D. Not modified. */ @Override default void applyInverseTransform(Transform transform) { transform.inverseTransform(getPosition()); transform.inverseTransform(getOrientation()); } }
/** * Sets the position to (0, 0) and the orientation to the neutral quaternion, i.e. zero rotation. */ @Override default void setToZero() { getOrientation().setToZero(); getPosition().setToZero(); }
/** * Appends the given {@code transform} to this pose 3D. * * @param transform the quaternion-based transform to append to this pose 3D. Not modified. */ default void appendTransform(QuaternionBasedTransform transform) { QuaternionTools.addTransform(getOrientation(), transform.getTranslationVector(), getPosition()); getOrientation().multiply(transform.getQuaternion()); }
/** {@inheritDoc} */ @Override default int setJointConfiguration(int rowStart, DenseMatrix64F matrix) { getJointPose().getOrientation().set(rowStart, matrix); getJointPose().getPosition().set(rowStart + 4, matrix); return rowStart + getConfigurationMatrixSize(); }
/** * Appends the given {@code transform} to this pose 3D. * * @param transform the rigid-body transform to append to this pose 3D. Not modified. */ default void appendTransform(RigidBodyTransform transform) { QuaternionTools.addTransform(getOrientation(), transform.getTranslationVector(), getPosition()); getOrientation().append(transform.getRotationMatrix()); }
public void recordCurrentState() { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); rootJointTranslation.set(rootJoint.getJointPose().getPosition()); rootJointRotation.set(rootJoint.getJointPose().getOrientation()); yoRootJointTranslation.set(rootJointTranslation); yoRootJointRotation.set(rootJointRotation); }
@Override public void doControl() { refFrame.update(); sixDofPelvisJoint.setJointConfiguration(robotPose); pelvisPoseHistoryCorrection.doControl(Conversions.secondsToNanoseconds(scs.getTime())); floatingJoint.setQuaternion(sixDofPelvisJoint.getJointPose().getOrientation()); floatingJoint.setPosition(sixDofPelvisJoint.getJointPose().getPosition()); } }
public static ChecksumUpdater newJointChecksumUpdater(GenericCRC32 checksum, SixDoFJoint joint) { return () -> { checksum.update(joint.getJointPose().getOrientation()); checksum.update(joint.getJointPose().getPosition()); checksum.update(joint.getJointTwist().getAngularPart()); checksum.update(joint.getJointTwist().getLinearPart()); checksum.update(joint.getJointAcceleration().getAngularPart()); checksum.update(joint.getJointAcceleration().getLinearPart()); }; }