Create method to convert angular acceleration to 2nd-time-derivative …

…of roll-pitch-yaw

automotive/agent_trajectory.h

@@ -52,7 +52,7 @@ class PoseVelocity final {
   /// translation and the z-component of rotation.
   Eigen::Vector3d pose3() const {
     const math::RollPitchYaw<double> rpy(rotation_);
-    const double w_z = rpy.get_yaw_angle();
+    const double w_z = rpy.yaw_angle();
     return Eigen::Vector3d{translation_.x(), translation_.y(), w_z};
   }
 

automotive/maliput/api/lane_data.h

@@ -102,13 +102,13 @@ class Rotation {
   //                          most call-sites should probably be using something
   //                          else (e.g., quaternion) anyway.
   /// Returns the roll component of the Rotation (in radians).
-  double roll() const { return rpy().get_roll_angle(); }
+  double roll() const { return rpy().roll_angle(); }
 
   /// Returns the pitch component of the Rotation (in radians).
-  double pitch() const { return rpy().get_pitch_angle(); }
+  double pitch() const { return rpy().pitch_angle(); }
 
   /// Returns the yaw component of the Rotation (in radians).
-  double yaw() const { return rpy().get_yaw_angle(); }
+  double yaw() const { return rpy().yaw_angle(); }
 
  private:
   explicit Rotation(const Quaternion<double>& quat) : quaternion_(quat) {}

automotive/pure_pursuit.cc

@@ -37,7 +37,7 @@ T PurePursuit<T>::Evaluate(const PurePursuitParams<T>& pp_params,
 
   const T x = pose.get_translation().translation().x();
   const T y = pose.get_translation().translation().y();
-  const T heading = math::RollPitchYaw<T>(pose.get_rotation()).get_yaw_angle();
+  const T heading = math::RollPitchYaw<T>(pose.get_rotation()).yaw_angle();
 
   const T delta_r = -(goal_position.x() - x) * sin(heading) +
                     (goal_position.y() - y) * cos(heading);

automotive/test/agent_trajectory_test.cc

@@ -55,7 +55,7 @@ GTEST_TEST(PoseVelocityTest, Accessors) {
   EXPECT_TRUE(CompareMatrices(actual.velocity().rotational(), w));
   EXPECT_TRUE(CompareMatrices(actual.velocity().translational(), v));
   const Vector3d expected_pose3{translation.x(), translation.y(),
-                                rpy.get_yaw_angle()};
+                                rpy.yaw_angle()};
   EXPECT_TRUE(CompareMatrices(actual.pose3(), expected_pose3));
   EXPECT_EQ(actual.speed(), sqrt(pow(v(0), 2) + pow(v(1), 2) + pow(v(2), 2)));
 }

bindings/pydrake/math_py.cc

@@ -60,9 +60,9 @@ PYBIND11_MODULE(math, m) {
            py::arg("roll"), py::arg("pitch"), py::arg("yaw"))
       .def(py::init<const RotationMatrix<T>&>(), py::arg("R"))
       .def("vector", &RollPitchYaw<T>::vector)
-      .def("get_roll_angle", &RollPitchYaw<T>::get_roll_angle)
-      .def("get_pitch_angle", &RollPitchYaw<T>::get_pitch_angle)
-      .def("get_yaw_angle", &RollPitchYaw<T>::get_yaw_angle)
+      .def("roll_angle", &RollPitchYaw<T>::roll_angle)
+      .def("pitch_angle", &RollPitchYaw<T>::pitch_angle)
+      .def("yaw_angle", &RollPitchYaw<T>::yaw_angle)
       .def("ToQuaternion", &RollPitchYaw<T>::ToQuaternion);
 
   py::class_<RotationMatrix<T>>(m, "RotationMatrix")

bindings/pydrake/test/math_test.py

@@ -108,7 +108,7 @@ def test_roll_pitch_yaw(self):
         self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
         rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
         self.assertTupleEqual(
-            (rpy.get_roll_angle(), rpy.get_pitch_angle(), rpy.get_yaw_angle()),
+            (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
             (0, 0, 0))
         q_I = Quaternion()
         self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))

examples/quadrotor/quadrotor_plant.cc

@@ -86,13 +86,13 @@ void QuadrotorPlant<T>::DoCalcTimeDerivatives(
   // For rotors 1 and 3, get the -Bz measure of its aerodynamic torque on B.
   // Sum the net Bz measure of the aerodynamic torque on B.
   // Note: Rotors 0 and 2 rotate one way and rotors 1 and 3 rotate the other.
-  const Vector4<T> uM_Bz = kM_ * u;
-  const T Mz = uM_Bz(0) - uM_Bz(1) + uM_Bz(2) - uM_Bz(3);
+  const Vector4<T> uTau_Bz = kM_ * u;
+  const T Mz = uTau_Bz(0) - uTau_Bz(1) + uTau_Bz(2) - uTau_Bz(3);
 
   // Form the net moment on B about Bcm, expressed in B. The net moment accounts
   // for all contact and distance forces (aerodynamic and gravity forces) on B.
   // Note: Since the net moment on B is about Bcm, gravity does not contribute.
-  const Vector3<T> M(Mx, My, Mz);
+  const Vector3<T> Tau_B(Mx, My, Mz);
 
   // Calculate local celestial body's (Earth's) gravity force on B, expressed in
   // the Newtonian frame N (a.k.a the inertial or World frame).
@@ -112,32 +112,17 @@ void QuadrotorPlant<T>::DoCalcTimeDerivatives(
   const Vector3<T> xyzDDt = Fnet_N / m_;  // Equal to a_NBcm_N.
 
   // Use rpy and rpyDt to calculate B's angular velocity in N, expressed in B.
-  const Vector3<T> w_BN_B = rpy.RollPitchYawDtToAngularVelocityD(rpyDt);
-
-  // To compute B's angular acceleration in N, expressed in B, due to the net
-  // moment on B, rearrange Euler rigid body equation to solve for alpha_BN_B.
-  // Euler's equation: M = I_.Dot(alpha_BN_B) + w.Cross(I_.Dot(w)).
-  const Vector3<T> wIw = w_BN_B.cross(I_ * w_BN_B);      // Expressed in B.
-  const Vector3<T> alf_BN_B = I_.ldlt().solve(M - wIw);  // Expressed in B.
-
-  // For subsequent calculation (below) form B's angular acceleration in N
-  // expressed in N, and B's angular velocity in N expressed in N.
-  const Vector3<T> alf_BN_N = R_NB * alf_BN_B;           // Expressed in N.
-  const Vector3<T> w_BN_N = R_NB * w_BN_B;               // Expressed in N.
-
-  // TODO(mitiguy) replace angularvel2rpydotMatrix with new RollPitchYaw method.
-  // TODO(mitiguy) continue fixing documentation for this example (here to end).
-  Matrix3<T> Phi;
-  typename drake::math::Gradient<Matrix3<T>, 3>::type dPhi;
-  typename drake::math::Gradient<Matrix3<T>, 3, 2>::type* ddPhi = nullptr;
-  angularvel2rpydotMatrix(rpy.vector(), Phi, &dPhi, ddPhi);
-
-  const Eigen::Matrix<T, 9, 1> dPhi_x_rpyDt_vec = dPhi * rpyDt;
-  const Eigen::Map<const Matrix3<T>> dPhi_x_rpyDt(dPhi_x_rpyDt_vec.data());
-  const Matrix3<T> R_NB_Dt = rpy.OrdinaryDerivativeRotationMatrix(rpyDt);
-  const Vector3<T> rpyDDt = Phi * alf_BN_N
-                          + dPhi_x_rpyDt * w_BN_N
-                          + Phi * R_NB_Dt * w_BN_B;
+  const Vector3<T> w_BN_B = rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt);
+
+  // To compute α (B's angular acceleration in N) due to the net moment 𝛕 on B,
+  // rearrange Euler rigid body equation  𝛕 = I α + ω × (I ω)  and solve for α.
+  const Vector3<T> wIw = w_BN_B.cross(I_ * w_BN_B);            // Expressed in B
+  const Vector3<T> alpha_NB_B = I_.ldlt().solve(Tau_B - wIw);  // Expressed in B
+  const Vector3<T> alpha_NB_N = R_NB * alpha_NB_B;             // Expressed in N
+
+  // Calculate the 2nd time-derivative of rpy.
+  const Vector3<T> rpyDDt =
+      rpy.CalcRpyDDtFromAngularAccelInParent(rpyDt, alpha_NB_N);
 
   // Recomposing the derivatives vector.
   VectorX<T> xDt(12);

math/roll_pitch_yaw.cc

@@ -14,6 +14,87 @@ template <typename T>
 RollPitchYaw<T>::RollPitchYaw(const Eigen::Quaternion<T>& quaternion) :
     RollPitchYaw(quaternion, RotationMatrix<T>(quaternion)) {}
 
+// <h3>Theory</h3>
+//
+// This algorithm was created October 2016 by Paul Mitiguy for TRI (Toyota).
+// We believe this is a new algorithm (not previously published).
+// Some theory/formulation of this algorithm is provided below.  More detail
+// is in Chapter 6 Rotation Matrices II [Mitiguy 2017] (reference below).
+// <pre>
+// Notation: Angles q1, q2, q3 designate SpaceXYZ "roll, pitch, yaw" angles.
+//           A quaternion can be defined in terms of an angle-axis rotation by
+//           an angle `theta` about a unit vector `lambda`.  For example,
+//           consider right-handed orthogonal unit vectors Ax, Ay, Az and
+//           Bx, By, Bz fixed in a frame A and a rigid body B, respectively.
+//           Initially, Bx = Ax, By = Ay, Bz = Az, then B is subjected to a
+//           right-handed rotation relative to frame A by an angle `theta`
+//           about `lambda = L1*Ax + L2*Ay + L3*Az = L1*Bx + L2*By + L3*Bz`.
+//           The elements of `quaternion` are defined e0, e1, e2, e3 as
+//           `e0 = cos(theta/2)`, `e1 = L1*sin(theta/2)`,
+//           `e2 = L2*sin(theta/2)`, `e3 = L3*sin(theta/2)`.
+//
+// Step 1. The 3x3 rotation matrix R is only used (in conjunction with the
+//         atan2 function) to accurately calculate the pitch angle q2.
+//         Note: Since only 5 elements of R are used, the algorithm could be
+//         made slightly more efficient by computing/passing only those 5
+//         elements (e.g., not calculating the other 4 elements) and/or
+//         manipulating the relationship between `R` and quaternion to
+//         further reduce calculations.
+//
+// Step 2. Realize the quaternion passed to the function can be regarded as
+//         resulting from multiplication of certain 4x4 and 4x1 matrices, or
+//         multiplying three rotation quaternions (Hamilton product), to give:
+//         e0 = sin(q1/2)*sin(q2/2)*sin(q3/2) + cos(q1/2)*cos(q2/2)*cos(q3/2)
+//         e1 = sin(q3/2)*cos(q1/2)*cos(q2/2) - sin(q1/2)*sin(q2/2)*cos(q3/2)
+//         e2 = sin(q1/2)*sin(q3/2)*cos(q2/2) + sin(q2/2)*cos(q1/2)*cos(q3/2)
+//         e3 = sin(q1/2)*cos(q2/2)*cos(q3/2) - sin(q2/2)*sin(q3/2)*cos(q1/2)
+//
+//         Reference for step 2: Chapter 6 Rotation Matrices II [Mitiguy 2017]
+//
+// Step 3.  Since q2 has already been calculated (in Step 1), substitute
+//          cos(q2/2) = A and sin(q2/2) = f*A.
+//          Note: The final results are independent of A and f = tan(q2/2).
+//          Note: -pi/2 <= q2 <= pi/2  so -0.707 <= [A = cos(q2/2)] <= 0.707
+//          and  -1 <= [f = tan(q2/2)] <= 1.
+//
+// Step 4.  Referring to Step 2 form: (1+f)*e1 + (1+f)*e3 and rearrange to:
+//          sin(q1/2+q3/2) = (e1+e3)/(A*(1-f))
+//
+//          Referring to Step 2 form: (1+f)*e0 - (1+f)*e2 and rearrange to:
+//          cos(q1/2+q3/2) = (e0-e2)/(A*(1-f))
+//
+//          Combine the two previous results to produce:
+//          1/2*( q1 + q3 ) = atan2( e1+e3, e0-e2 )
+//
+// Step 5.  Referring to Step 2 form: (1-f)*e1 - (1-f)*e3 and rearrange to:
+//          sin(q1/5-q3/5) = -(e1-e3)/(A*(1+f))
+//
+//          Referring to Step 2 form: (1-f)*e0 + (1-f)*e2 and rearrange to:
+//          cos(q1/2-q3/2) = (e0+e2)/(A*(1+f))
+//
+//          Combine the two previous results to produce:
+//          1/2*( q1 - q3 ) = atan2( e3-e1, e0+e2 )
+//
+// Step 6.  Combine Steps 4 and 5 and solve the linear equations for q1, q3.
+//          Use zA, zB to handle case in which both atan2 arguments are 0.
+//          zA = (e1+e3==0  &&  e0-e2==0) ? 0 : atan2( e1+e3, e0-e2 );
+//          zB = (e3-e1==0  &&  e0+e2==0) ? 0 : atan2( e3-e1, e0+e2 );
+//          Solve: 1/2*( q1 + q3 ) = zA     To produce:  q1 = zA + zB
+//                 1/2*( q1 - q3 ) = zB                  q3 = zA - zB
+//
+// Step 7.  As necessary, modify angles by 2*PI to return angles in range:
+//          -pi   <= q1 <= pi
+//          -pi/2 <= q2 <= pi/2
+//          -pi   <= q3 <= pi
+//
+// [Mitiguy, 2017]: "Advanced Dynamics and Motion Simulation,
+//                   For professional engineers and scientists,"
+//                   Prodigy Press, Sunnyvale CA, 2017 (Paul Mitiguy).
+//                   Available at www.MotionGenesis.com
+// </pre>
+// @note This algorithm is specific to SpaceXYZ (roll-pitch-yaw) order.
+// It is easily modified for other SpaceIJK and BodyIJI rotation sequences.
+// @author Paul Mitiguy
 template<typename T>
 RollPitchYaw<T>::RollPitchYaw(const Eigen::Quaternion<T>& quaternion,
                               const RotationMatrix<T>& rotation_matrix) {