public void getRotation(Quat4d rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, rotationToPack); }
public double getYaw() { return RotationTools.computeYaw(this); }
public void setRotation(Matrix3d jointRotation) { RotationTools.convertMatrixToQuaternion(jointRotation, this.jointRotation); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public void getRotation(Matrix3d rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToMatrix(0.0, yawPitchRoll[1], 0.0, rotationToPack); }
public static void convertMatrixToYawPitchRoll(Matrix3d rotationMatrix, double[] yawPitchRollToPack) { yawPitchRollToPack[0] = computeYaw(rotationMatrix); yawPitchRollToPack[1] = computePitch(rotationMatrix); yawPitchRollToPack[2] = computeRoll(rotationMatrix); }
public void setRotation(Matrix3d rotationMatrix, int imuIndex) { Quat4d q = new Quat4d(); RotationTools.convertMatrixToQuaternion(rotationMatrix, q); RotationTools.checkQuaternionNormalized(q); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // q.set(rotationMatrix); p_qs.set(q.getW()); p_qx.set(q.getX()); p_qy.set(q.getY()); p_qz.set(q.getZ()); p_yaw.set(RotationTools.computeYaw(rotationMatrix)); p_pitch.set(RotationTools.computePitch(rotationMatrix)); p_roll.set(RotationTools.computeRoll(rotationMatrix)); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { if (Double.isNaN(roll)) { throw new RuntimeException("Orientation.setYawPitchRoll(). yaw = " + yaw + ", pitch = " + pitch + ", roll = " + roll); } RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, quaternion); normalize(); }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3D planeNormal) { Point3D position = footstep.getLocation(); Quaternion orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = orientation.getYaw(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3d planeNormal) { Point3d position = footstep.getLocation(); Quat4d orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = RotationTools.computeYaw(orientation); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomGetQuaternionFromYawAndZNormal() { int numTests = 100; Random random = new Random(7362L); Vector3D normal = new Vector3D(); Quaternion quatSolution = new Quaternion(); for (int i = 0; i < numTests; i++) { Quaternion quatToPack = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrixToPack = new RotationMatrix(); double yaw = quatToPack.getYaw(); rotationMatrixToPack.set(quatToPack); rotationMatrixToPack.getColumn(2, normal); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); boolean quaternionsAreEqual = RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON); assertTrue(quaternionsAreEqual); } }
public boolean epsilonEquals(FrameOrientation frameOrientation, double epsilon) { boolean referenceFramesMatch = referenceFrame == frameOrientation.referenceFrame; boolean quaternionsAreEqual = RotationTools.quaternionEpsilonEquals(quaternion, frameOrientation.quaternion, epsilon); return referenceFramesMatch && quaternionsAreEqual; }
public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, Matrix3d orientationResultToPack) { AxisAngle4d axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.set(axisAngleResult); }
private Matrix3d computeDesiredFootRotation(ReferenceFrame desiredHeadingFrame) { RigidBodyTransform footToSupportTransform = desiredHeadingFrame.getTransformToDesiredFrame(worldFrame); Matrix3d footToSupportRotation = new Matrix3d(); footToSupportTransform.getRotation(footToSupportRotation); double yaw = RotationTools.computeYaw(footToSupportRotation); double pitch = stepPitch.getDoubleValue(); double roll = 0.0; RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, footToSupportRotation); return footToSupportRotation; }
public static void convertYawPitchRollToMatrix(double[] yawPitchRoll, Matrix3d rotationMatrixToPack) { convertYawPitchRollToMatrix(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2], rotationMatrixToPack); }
public void setRotation(Matrix3d jointRotation) { RotationTools.convertMatrixToYawPitchRoll(jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, this.jointRotation); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public void getRotation(double[] yawPitchRoll) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, this.yawPitchRoll); yawPitchRoll[0] = 0.0; yawPitchRoll[1] = this.yawPitchRoll[1]; yawPitchRoll[2] = 0.0; }
yoFrameQuaternion.set(quat4dExpected); Quaternion quat4dActual = new Quaternion(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); assertTrue(RotationTools.axisAngleEpsilonEqualsIgnoreFlippedAxes(axisAngle4dExpected, axisAngle4dActual, EPS));
public HandstepPacket(RobotSide robotSide, Point3d location, Quat4d orientation, Vector3d surfaceNormal, double swingTrajectoryTime) { this.robotSide = robotSide; this.location = location; this.orientation = orientation; this.surfaceNormal = surfaceNormal; this.swingTrajectoryTime = swingTrajectoryTime; RotationTools.checkQuaternionNormalized(this.orientation); }
return axisAngleEpsilonEqualsIgnoreFlippedAxes(original, result, epsilon); trimAxisAngleMinusPiToPi(original, originalMinusPiToPi); AxisAngle resultMinusPiToPi = resultAxisAngleForEpsilonEquals.get(); trimAxisAngleMinusPiToPi(result, resultMinusPiToPi); return axisAngleEpsilonEqualsIgnoreFlippedAxes(original, result, epsilon); return axisAngleEpsilonEqualsIgnoreFlippedAxes(originalMinusPiToPi, resultMinusPiToPi, epsilon);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAxisAngleEpsilonEqualsIgnoreFlippedAxes() { for (int i = 0; i < 100; i++) { double randomAngle = AngleTools.generateRandomAngle(random); Vector3D randomAxisA = RandomGeometry.nextVector3D(random, 1.0); Vector3D randomAxisA_flipped = new Vector3D(randomAxisA); randomAxisA_flipped.negate(); AxisAngle axisAngleA = new AxisAngle(randomAxisA, randomAngle); AxisAngle axisAngleB = new AxisAngle(randomAxisA_flipped, -randomAngle); assertTrue(axisAngleA + "\n should equal:\n" + axisAngleB + "!", RotationTools.axisAngleEpsilonEqualsIgnoreFlippedAxes(axisAngleA, axisAngleB, EPSILON)); } for (int i = 0; i < 100; i++) { double randomAngle = AngleTools.generateRandomAngle(random); Vector3D randomAxisA = RandomGeometry.nextVector3D(random, 1.0); Vector3D randomAxisA_flipped = new Vector3D(randomAxisA); randomAxisA_flipped.negate(); AxisAngle axisAngleA = new AxisAngle(randomAxisA, randomAngle); AxisAngle axisAngleB = new AxisAngle(randomAxisA_flipped, randomAngle); assertTrue(axisAngleA + "\n should *NOT* equal:\n" + axisAngleB + "!", !RotationTools.axisAngleEpsilonEqualsIgnoreFlippedAxes(axisAngleA, axisAngleB, EPSILON)); } }