/** * Computes the angular velocity vector from the time derivatives of the yaw-pitch-roll angles. * <p> * The resulting velocity is expressed in the world coordinate system, i.e. the coordinate system * before the yaw-pitch-roll angles. * </p> * * @param yawPitchRoll array containing in order the current yaw, pitch, and roll rotation * angles, i.e. around the z, y, and x axes respectively. * @param yawPitchRollRates the rates of change of in order the yaw, pitch, and roll angles. * @param angularVelocityToPack the angular velocity from the yaw-pitch-roll angles rate. * Modified. */ public static void computeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate(double[] yawPitchRoll, double[] yawPitchRollRates, Vector3DBasics angularVelocityToPack) { computeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2], yawPitchRollRates[0], yawPitchRollRates[1], yawPitchRollRates[2], angularVelocityToPack); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void computeYawPitchRollAngleRatesFromAngularVelocityInWorldFrame() 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); Vector3D angularVelocityInWorldFrame = new Vector3D(); RotationTools.computeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, angularVelocityInWorldFrame); double[] actualYawPitchRollRates = new double[3]; RotationTools.computeYawPitchRollAngleRatesFromAngularVelocityInWorldFrame(angularVelocityInWorldFrame, yaw, pitch, roll, actualYawPitchRollRates); assertEquals(yawRate, actualYawPitchRollRates[0], EPSILON); assertEquals(pitchRate, actualYawPitchRollRates[1], EPSILON); assertEquals(rollRate, actualYawPitchRollRates[2], EPSILON); } }
@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); } }