private static Pose3D computeCircleTrajectory(double time, double trajectoryTime, double circleRadius, Point3DReadOnly circleCenter, Quaternion circleRotation, QuaternionReadOnly constantOrientation, boolean ccw, double phase) { double theta = (ccw ? -time : time) / trajectoryTime * 2.0 * Math.PI + phase; double z = circleRadius * Math.sin(theta); double y = circleRadius * Math.cos(theta); Point3D point = new Point3D(0.0, y, z); circleRotation.transform(point); point.add(circleCenter); return new Pose3D(point, constantOrientation); }
/** * circular trajectory */ public static Pose3D computeCircleTrajectory(double time, double trajectoryTime, double circleRadius, Point3DReadOnly circleCenter, Quaternion circleRotation, QuaternionReadOnly constantOrientation, boolean ccw, double phase) { double theta = (ccw ? -time : time) / trajectoryTime * 2.0 * Math.PI + phase; double z = circleRadius * Math.sin(theta); double y = circleRadius * Math.cos(theta); Point3D point = new Point3D(0.0, y, z); circleRotation.transform(point); point.add(circleCenter); return new Pose3D(point, constantOrientation); } }
private void computeAngularVelocity(Vector3D angularVelocityToPack, Quaternion startRotationQuaternion, Quaternion endRotationQuaternion, double alphaDot) { if (startRotationQuaternion.dot(endRotationQuaternion) < 0.0) endRotationQuaternion.negate(); // compute relative orientation: orientation of interpolated frame w.r.t. start frame relativeRotationQuaternion.set(startRotationQuaternion); // R_W_S: orientation of start w.r.t. world relativeRotationQuaternion.conjugate(); // R_S_W: orientation of world w.r.t. start relativeRotationQuaternion.multiply(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); endRotationQuaternion.transform(angularVelocityToPack); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate() throws Exception { for (int i = 0; i < 1000; i++) { double yaw = RandomNumbers.nextDouble(random, Math.PI); double pitch = RandomNumbers.nextDouble(random, Math.PI / 2.0); double roll = RandomNumbers.nextDouble(random, Math.PI); double yawRate = RandomNumbers.nextDouble(random, 1.0); double pitchRate = RandomNumbers.nextDouble(random, 1.0); double rollRate = RandomNumbers.nextDouble(random, 1.0); Quaternion rotation = new Quaternion(); rotation.setYawPitchRoll(yaw, pitch, roll); Vector3D expectedAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, expectedAngularVelocity); rotation.transform(expectedAngularVelocity); Vector3D actualAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity); EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, EPSILON); } }
@Test public void testTransformationsWithQuaternionReadOnly() throws Exception { Random random = new Random(2342L); for (int i = 0; i < ITERATIONS; i++) { QuaternionReadOnly quaternion = EuclidCoreRandomTools.nextQuaternion(random); Tuple3DReadOnly tupleOriginal = EuclidCoreRandomTools.nextPoint3D(random, 10.0); Tuple3DBasics tupleExpected = new Point3D(); quaternion.transform(tupleOriginal, tupleExpected); Tuple3DBasics tupleActual = new Point3D(); tupleActual.setX(TransformationTools.computeTransformedX(quaternion, false, tupleOriginal)); tupleActual.setY(TransformationTools.computeTransformedY(quaternion, false, tupleOriginal)); tupleActual.setZ(TransformationTools.computeTransformedZ(quaternion, false, tupleOriginal)); EuclidCoreTestTools.assertTuple3DEquals(tupleExpected, tupleActual, EPSILON); Quaternion conjugatedQuaternion = new Quaternion(quaternion); conjugatedQuaternion.conjugate(); conjugatedQuaternion.transform(tupleOriginal, tupleExpected); tupleActual.setX(TransformationTools.computeTransformedX(quaternion, true, tupleOriginal)); tupleActual.setY(TransformationTools.computeTransformedY(quaternion, true, tupleOriginal)); tupleActual.setZ(TransformationTools.computeTransformedZ(quaternion, true, tupleOriginal)); EuclidCoreTestTools.assertTuple3DEquals(tupleExpected, tupleActual, EPSILON); } }
Vector3D actual = new Vector3D(); quaternion.transform(vectorOriginal, expected); orientation.transform(vectorOriginal, actual); EuclidCoreTestTools.assertTuple3DEquals(expected, actual, getEpsilon());
Vector3D actual = new Vector3D(); new Quaternion(rotationVector).transform(vectorOriginal, expected); orientation.transform(vectorOriginal, actual); EuclidCoreTestTools.assertTuple3DEquals(expected, actual, getEpsilon());