Best Java code snippets using us.ihmc.euclid.transform.RigidBodyTransform.getRotation (Showing top 20 results out of 315)

/**
* Packs the orientation of this shape into an axis-angle.
*
* @param orientationToPack used to pack the orientation of this shape. Modified.
*/
public final void getOrientation(Orientation3DBasics orientationToPack)
{
 shapePose.getRotation(orientationToPack);
}

public static void convertTransformToClosestYawPitchRoll(RigidBodyTransform transform, double[] yawPitchRollRef, double[] yawPitchRollToPack)
{
 RotationMatrix rotationMatrix = rotationMatrixForYawPitchRollConvertor.get();
 transform.getRotation(rotationMatrix);
 convertMatrixToClosestYawPitchRoll(rotationMatrix, yawPitchRollRef, yawPitchRollToPack);
}

public static double getMagnitudeOfAngleOfRotation(RigidBodyTransform rigidBodyTransform)
{
 AxisAngle axisAngle = new AxisAngle();
 rigidBodyTransform.getRotation(axisAngle);
 return Math.abs(axisAngle.getAngle());
}

private void generateExpectedOrientation()
{
 RotationMatrix randomTransformBodyToWorldMatrix = new RotationMatrix();
 RotationMatrix transformIMUToJointMatrix = new RotationMatrix();
 randomTransformBodyToWorld.getRotation(randomTransformBodyToWorldMatrix);
 transformIMUToJoint.getRotation(transformIMUToJointMatrix);
 expectedIMUOrientation.set(randomTransformBodyToWorldMatrix);
 expectedIMUOrientation.multiply(transformIMUToJointMatrix);
}

private void getYawPitchRoll(double[] yawPitchRoll, RigidBodyTransform transform3D)
{
 // This seems to work much better than going to quaternions first, especially when yaw is large...
 RotationMatrix rotationMatrix = new RotationMatrix();
 transform3D.getRotation(rotationMatrix);
 yawPitchRoll[0] = Math.atan2(rotationMatrix.getM10(), rotationMatrix.getM00());
 yawPitchRoll[1] = Math.asin(-rotationMatrix.getM20());
 yawPitchRoll[2] = Math.atan2(rotationMatrix.getM21(), rotationMatrix.getM22());
 if (Double.isNaN(yawPitchRoll[0]) || Double.isNaN(yawPitchRoll[1]) || Double.isNaN(yawPitchRoll[2]))
 {
   throw new RuntimeException("yaw, pitch, or roll are NaN! transform3D = " + transform3D);
 }
}

  private void verifyRelationship(RigidBodyTransform transform1, RigidBodyTransform transform2)
  {
   Quaternion quaternion1 = new Quaternion();
   Quaternion quaternion2 = new Quaternion();

   transform1.getRotation(quaternion1);
   transform2.getRotation(quaternion2);
   
   // Going through the quaternion multiplication method:
   Quaternion quaternion1TimesQuaternion2TimesQuaternion1Inverse = computeQuat1Quat2Quat1Conjugate(quaternion1, quaternion2);
   
   // Now going through the rotation vector (axis-angle as a single vector) method:
   Quaternion rotatedAxisAngle2Quaternion = computeRotatedRotationVector(transform1, quaternion1, quaternion2);
   
   EuclidCoreTestTools.assertQuaternionEquals(quaternion1TimesQuaternion2TimesQuaternion1Inverse, rotatedAxisAngle2Quaternion, 1e-7);
//      System.out.println("quaternion1TimesQuaternion2TimesQuaternion1Inverse = " + quaternion1TimesQuaternion2TimesQuaternion1Inverse);
//      System.out.println("rotatedAxisAngle2Quaternion = " + rotatedAxisAngle2Quaternion);
  }

public static void packEuclidRigidBodyTransformToGeometry_msgsPose(RigidBodyTransform pelvisTransform, Pose pose)
{
 Vector3D point = new Vector3D();
 pelvisTransform.getTranslation(point);
 us.ihmc.euclid.tuple4D.Quaternion rotation = new us.ihmc.euclid.tuple4D.Quaternion();
 pelvisTransform.getRotation(rotation);
 
 packEuclidTuple3DAndQuaternionToGeometry_msgsPose(point, rotation, pose);
}

  public void drawBox(BufferedImage image, RigidBodyTransform transform, double scale)
  {
   DenseMatrix64F rotation =new DenseMatrix64F(3,3);
   Vector3D translation = new Vector3D();
   transform.getRotation(rotation);
   transform.getTranslation(translation);
   Se3_F64 targetToSensor = new Se3_F64(rotation,new Vector3D_F64(translation.getX(), translation.getY(), translation.getZ()));

   Graphics2D g2 = image.createGraphics();
   g2.setStroke(new BasicStroke(4));
   g2.setColor(java.awt.Color.RED);
   VisualizeFiducial.drawCube(targetToSensor, this.intrinsicParameters, scale, 2, g2);
   g2.finalize();

   
  }
}

private void extractTandR(RigidBodyTransform tran, Vector3D T, Vector3D R)
{
 tran.getTranslation(T);
 tran.getRotation(rotationMatrix);
 rotationMatrix.get(m);
 ConvertRotation3D_F64.matrixToEuler(m, EulerType.XYZ, euler);
 R.setX(euler[0]);
 R.setY(euler[1]);
 R.setZ(euler[2]);
}

@Test
public void testTransformWithRotationMatrix() throws Exception
{
 Random random = new Random(2342L);
 RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random);
 RotationMatrix matrixOriginal = EuclidCoreRandomTools.nextRotationMatrix(random);
 RotationMatrix matrixExpected = new RotationMatrix();
 RotationMatrix matrixActual = new RotationMatrix();
 transform.getRotation(matrixExpected);
 matrixExpected.multiply(matrixOriginal);
 matrixActual.set(matrixOriginal);
 transform.transform(matrixActual);
 EuclidCoreTestTools.assertMatrix3DEquals(matrixExpected, matrixActual, EPS);
}

@Test
public void testTransformWithQuaternion() throws Exception
{
 Random random = new Random(34534L);
 RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random);
 Quaternion quaternionOriginal = EuclidCoreRandomTools.nextQuaternion(random);
 Quaternion quaternionExpected = new Quaternion();
 Quaternion quaternionActual = new Quaternion();
 transform.getRotation(quaternionExpected);
 quaternionExpected.multiply(quaternionOriginal);
 transform.transform(quaternionOriginal, quaternionActual);
 EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPS);
 quaternionActual.set(quaternionOriginal);
 transform.transform(quaternionActual);
 EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPS);
}

private DenseMatrix64F getSpatialErrorEndEffectorDesired()
{
 transformEndEffectorToDesired.getTranslation(errorTranslationVector);
 transformEndEffectorToDesired.getRotation(errorQuat);
 errorAxisAngle.set(errorQuat);
 errorAngularVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorAngularVector.scale(errorAxisAngle.getAngle());
 errorAngularVector.get(0, 0, spatialError);
 errorTranslationVector.get(3, 0, spatialError);
 return spatialError;
}

  @Override
  protected void updateTransformToParent(RigidBodyTransform transformToParent)
  {
   //TODO: Combine with RotationTools.removePitchAndRollFromTransform(). 
   origin.getReferenceFrame().getTransformToDesiredFrame(nonZUpToWorld, worldFrame);
   nonZUpToWorld.getRotation(nonZUpToWorldRotation);

   double yaw = nonZUpToWorldRotation.getYaw();
   euler.set(0.0, 0.0, yaw);
   transformToParent.setRotationEulerAndZeroTranslation(euler);

   originPoint3d.set(origin);
   nonZUpToWorld.transform(originPoint3d);
   translation.set(originPoint3d);
   transformToParent.setTranslation(translation);
  }
}

protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side)
{
 ReferenceFrame soleFrame = soleFrames.get(side);
 Vector3D translation = new Vector3D();
 Quaternion rotation = new Quaternion();
 soleFrame.getTransformToWorldFrame().getTranslation(translation);
 soleFrame.getTransformToWorldFrame().getRotation(rotation);
 FramePose2D solePose2d = new FramePose2D(soleFrame, new Point2D(translation.getX(), translation.getY()), rotation.getYaw());
 Footstep foot = createFootstep(side, solePose2d);
 return foot;
}

private void setPelvisYoVariables(RigidBodyTransform pelvisTransform)
{
 Vector3D pelvisTranslation = new Vector3D();
 double[] yawPitchRoll = new double[3];
 pelvisTransform.getTranslation(pelvisTranslation);
 Quaternion pelvisRotation = new Quaternion();
 pelvisTransform.getRotation(pelvisRotation);
 YawPitchRollConversion.convertQuaternionToYawPitchRoll(pelvisRotation, yawPitchRoll);
 computedPelvisPositionX.set(pelvisTranslation.getX());
 computedPelvisPositionY.set(pelvisTranslation.getY());
 computedPelvisPositionZ.set(pelvisTranslation.getZ());
 computedPelvisYaw.set(yawPitchRoll[0]);
 computedPelvisPitch.set(yawPitchRoll[1]);
 computedPelvisRoll.set(yawPitchRoll[2]);
}

protected void updatePerfectOrientation()
{
 imuFrame.getTransformToDesiredFrame(worldFrame).getRotation(orientation);
 perfM00.set(orientation.getM00());
 perfM01.set(orientation.getM01());
 perfM02.set(orientation.getM02());
 perfM10.set(orientation.getM10());
 perfM11.set(orientation.getM11());
 perfM12.set(orientation.getM12());
 perfM20.set(orientation.getM20());
 perfM21.set(orientation.getM21());
 perfM22.set(orientation.getM22());
}

private void computeError(RigidBodyTransform desiredTransform)
{
 jacobian.compute();
 jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame());
 errorTransform.setAndInvert(desiredTransform);
 errorTransform.multiply(actualTransform);
 errorTransform.getRotation(errorRotationMatrix);
 errorAxisAngle.set(errorRotationMatrix);
 axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotationVector.set(axis);
 errorRotationVector.scale(errorAxisAngle.getAngle());
 errorRotationVector.scale(0.2);
 
 errorTransform.getTranslation(errorTranslationVector);
 
 errorRotationVector.get(0, 0, spatialError);
 errorTranslationVector.get(3, 0, spatialError);
}

private void computeDriftTransform()
{
 RigidBodyTransform driftTransform = new RigidBodyTransform();
 pelvisFrameFromMocap.update();
 pelvisFrameFromRobotConfigurationDataPacket.update();
 pelvisFrameFromMocap.getTransformToDesiredFrame(driftTransform, pelvisFrameFromRobotConfigurationDataPacket);
 
 Vector3D driftTranslation = new Vector3D();
 driftTransform.getTranslation(driftTranslation);
 
 Quaternion driftRotation = new Quaternion();
 driftTransform.getRotation(driftRotation);
 double[] driftRotationYPR = new double[3];
 YawPitchRollConversion.convertQuaternionToYawPitchRoll(driftRotation, driftRotationYPR);
 
 mocapWorldToRobotWorldTransformX.set(driftTranslation.getX());
 mocapWorldToRobotWorldTransformY.set(driftTranslation.getY());
 mocapWorldToRobotWorldTransformZ.set(driftTranslation.getZ());
 
 mocapWorldToRobotWorldTransformYaw.set(driftRotationYPR[0]);
 mocapWorldToRobotWorldTransformPitch.set(driftRotationYPR[1]);
 mocapWorldToRobotWorldTransformRoll.set(driftRotationYPR[2]);      
}

public static void rigidBodyTransformToOpenCVTR(RigidBodyTransform transform, Mat tvec, Mat rvec)
{
 Point3D translation = new Point3D();
 transform.getTranslation(translation);
 AxisAngle axisAngle = new AxisAngle();
 transform.getRotation(axisAngle);
 Vector3D rotVector = new Vector3D(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ());
 rotVector.normalize();
 rotVector.scale(axisAngle.getAngle());
 tvec.put(0, 0, translation.getX());
 tvec.put(1, 0, translation.getY());
 tvec.put(2, 0, translation.getZ());
 rvec.put(0, 0, rotVector.getX());
 rvec.put(1, 0, rotVector.getY());
 rvec.put(2, 0, rotVector.getZ());
}

public void initialize(RigidBodyTransform transform, TwistReadOnly twist)
{
 twist.checkReferenceFrameMatch(bodyFrame, bodyFrame.getParent(), bodyFrame);
 transform.getRotation(orientation);
 stateVector.set(orientationStart + 0, 0.0);
 stateVector.set(orientationStart + 1, 0.0);
 stateVector.set(orientationStart + 2, 0.0);
 stateVector.set(angularVelocityStart + 0, twist.getAngularPartX());
 stateVector.set(angularVelocityStart + 1, twist.getAngularPartY());
 stateVector.set(angularVelocityStart + 2, twist.getAngularPartZ());
 stateVector.set(angularAccelerationStart + 0, 0.0);
 stateVector.set(angularAccelerationStart + 1, 0.0);
 stateVector.set(angularAccelerationStart + 2, 0.0);
 transform.getTranslationVector().get(positionStart, stateVector);
 stateVector.set(linearVelocityStart + 0, twist.getLinearPartX());
 stateVector.set(linearVelocityStart + 1, twist.getLinearPartY());
 stateVector.set(linearVelocityStart + 2, twist.getLinearPartZ());
 stateVector.set(linearAccelerationStart + 0, 0.0);
 stateVector.set(linearAccelerationStart + 1, 0.0);
 stateVector.set(linearAccelerationStart + 2, 0.0);
}