/** * Sets the orientation part of this pose 3D with the given orientation. * * @param orientation the orientation used to set this pose's orientation. Not modified. */ default void setOrientation(Orientation3DReadOnly orientation) { getOrientation().set(orientation); }
/** * Normalizes the quaternion part of this pose and then limits the angle of the rotation it * represents to be ∈ [-<i>pi</i>;<i>pi</i>]. * <p> * Edge cases: * <ul> * <li>if the quaternion contains {@link Double#NaN}, this method is ineffective. * </ul> * </p> */ default void normalizeQuaternionAndLimitToPi() { getOrientation().normalizeAndLimitToPi(); }
/** * Sets the orientation part of this pose 3D with the given orientation 2D. * * @param orientation the orientation 2D used to set this pose's orientation. Not modified. */ default void setOrientation(Orientation2DReadOnly orientation) { getOrientation().setToYawQuaternion(orientation.getYaw()); }
/** * 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()); }
/** * 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()); }
/** {@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(); }
/** * Transforms the position and orientation parts of this pose 3D by the given {@code transform}. * * @param transform the geometric transform to apply on this pose 3D. Not modified. */ @Override default void applyTransform(Transform transform) { transform.transform(getPosition()); transform.transform(getOrientation()); }
/** * 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); }
public static RobotConfigurationData extractRobotConfigurationData(FullHumanoidRobotModel fullRobotModel) { OneDoFJointBasics[] joints = FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(joints, new ForceSensorDefinition[0], new IMUDefinition[0]); RobotConfigurationDataFactory.packJointState(robotConfigurationData, Arrays.stream(joints).collect(Collectors.toList())); robotConfigurationData.getRootTranslation().set(fullRobotModel.getRootJoint().getJointPose().getPosition()); robotConfigurationData.getRootOrientation().set(fullRobotModel.getRootJoint().getJointPose().getOrientation()); return robotConfigurationData; }
/** {@inheritDoc} */ @Override default int setJointConfiguration(int rowStart, DenseMatrix64F matrix) { getJointPose().getOrientation().set(rowStart, matrix); getJointPose().getPosition().set(rowStart + 4, matrix); return rowStart + getConfigurationMatrixSize(); }
public void recordCurrentState() { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); rootJointTranslation.set(rootJoint.getJointPose().getPosition()); rootJointRotation.set(rootJoint.getJointPose().getOrientation()); yoRootJointTranslation.set(rootJointTranslation); yoRootJointRotation.set(rootJointRotation); }
/** {@inheritDoc} */ @Override default int setJointConfiguration(int rowStart, DenseMatrix64F matrix) { int index = rowStart; double qRot = matrix.get(index++, 0); double x = matrix.get(index++, 0); double z = matrix.get(index++, 0); getJointPose().getOrientation().setToPitchQuaternion(qRot); getJointPose().getPosition().set(x, 0.0, z); return rowStart + getConfigurationMatrixSize(); }
@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()); }; }