How to use

getOrientation

method

in

us.ihmc.euclid.geometry.interfaces.Pose3DBasics

/**
* 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 &in; [-<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());
 };
}