/** * 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); }