/** * Computes the angular velocity vector from the time derivatives of the yaw-pitch-roll angles. * <p> * The resulting velocity is expressed in the local coordinate system described by 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 computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(double[] yawPitchRoll, double[] yawPitchRollRates, Vector3DBasics angularVelocityToPack) { computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2], yawPitchRollRates[0], yawPitchRollRates[1], yawPitchRollRates[2], angularVelocityToPack); }
double rollRate = 0.1 * -Math.sin(angle) * angleDot; RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, angularVelocity);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void computeYawPitchRollAngleRatesFromAngularVelocityInBodyFrame() 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 angularVelocityInBodyFrame = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, angularVelocityInBodyFrame); double[] actualYawPitchRollRates = new double[3]; RotationTools.computeYawPitchRollAngleRatesFromAngularVelocityInBodyFrame(angularVelocityInBodyFrame, yaw, pitch, roll, actualYawPitchRollRates); assertEquals("Iteration: " + i, yawRate, actualYawPitchRollRates[0], EPSILON); assertEquals("Iteration: " + i, pitchRate, actualYawPitchRollRates[1], EPSILON); assertEquals("Iteration: " + i, 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); } }
double rollRate = rollMagnitude * derivativeFactor; Vector3D angularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, angularVelocity); FrameVector3D frameAngularVelcoity = new FrameVector3D(pelvisFrame, angularVelocity); frameAngularVelcoity.changeFrame(worldFrame);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate() throws Exception { double dt = 1.0e-8; 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); double previousYaw = yaw - yawRate * dt; double previousPitch = pitch - pitchRate * dt; double previousRoll = roll - rollRate * dt; Quaternion rotation = new Quaternion(); rotation.setYawPitchRoll(yaw, pitch, roll); Quaternion previousRotation = new Quaternion(); previousRotation.setYawPitchRoll(previousYaw, previousPitch, previousRoll); Vector3D expectedAngularVelocity = new Vector3D(); Quaternion difference = new Quaternion(); difference.difference(previousRotation, rotation); difference.getRotationVector(expectedAngularVelocity); expectedAngularVelocity.scale(1.0 / dt); Vector3D actualAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity); EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, 1.0e-7); } }