Consolidated code, added tests.

Author: Paul Mitiguy

multibody/tree/rpy_ball_mobilizer.cc

@@ -446,6 +446,36 @@ RpyBallMobilizer<T>::TemplatedDoCloneToScalar(
       outboard_frame_clone);
 }
 
+template <typename T>
+Vector3<T> RpyBallMobilizer<T>::CalcNplusDotTimesQdot(
+    const systems::Context<T>& context, const char* function_name) const {
+  using std::cos;
+  using std::sin;
+  const Vector3<T> angles = get_angles(context);
+  const T cp = cos(angles[1]);
+  const T sp = sin(angles[1]);
+  const T sy = sin(angles[2]);
+  const T cy = cos(angles[2]);
+  if (abs(cp) < 1.0e-3) {
+    ThrowSinceCosPitchIsNearZero(angles[1], function_name);
+  }
+  // The algorithm below efficiently calculates Ṅ⁺(q,q̇)⋅q̇.
+  const Vector3<T> v = get_angular_velocity(context);
+  Vector3<T> qdot;
+  DoMapVelocityToQDot(context, v, &qdot);
+  const T& rdot = qdot[0];
+  const T& pdot = qdot[1];
+  const T& ydot = qdot[2];
+  const T pdot_ydot = pdot * ydot;
+  const T rdot_pdot = rdot * pdot;
+  const T rdot_ydot = rdot * ydot;
+  const T sp_rdot_pdot = sp * rdot_pdot;
+  const T cp_rdot_ydot = cp * rdot_ydot;
+  return Vector3<T>(-cy * pdot_ydot - cy * sp_rdot_pdot - sy * cp_rdot_ydot,
+                    -sy * pdot_ydot - sy * sp_rdot_pdot + cy * cp_rdot_ydot,
+                    -cp * rdot_pdot);
+}
+
 template <typename T>
 void RpyBallMobilizer<T>::DoMapAccelerationToQDDot(
     const systems::Context<T>& context,
@@ -466,47 +496,49 @@ void RpyBallMobilizer<T>::DoMapAccelerationToQDDot(
   //
   // There are various ways to get q̈ = [r̈, p̈, ÿ]ᵀ in terms of v̇ = [ω̇x, ω̇y, ω̇z]ᵀ.
   // A simple calculation directly solves for q̈ as q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.
-  // Since N⁺(q) is simpler than N(q), then Ṅ⁺(q,q̇) is much simpler than Ṅ(q,q̇),
-  // so a more efficient calculation of q̈ starts with v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈
-  // and then solves for q̈ = N⁺(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}. Since N(q)⋅N⁺(q) = I₃₃
-  // (the 3x3 identity matrix), Ṅ⋅N⁺ + N Ṅ⁺ = 0₃₃, which leads to Ṅ⁺ = N⁺⋅Ṅ⋅N⁺.
-  // Substituting this gives  q̈ = N⁺(q) {v̇ - N⁺⋅Ṅ⋅N⁺⋅q̇} = N⁺(q) {v̇ - N⁺⋅Ṅ⋅N⁺⋅q̇}
+  // However, N⁺(q) is simpler than N(q) so Ṅ⁺(q,q̇) is much simpler than Ṅ(q,q̇).
+  // Hence, a more efficient calculation of q̈ starts as v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈
+  // and then solves as q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.
   // --------------------------------------------------------------------------
-  using std::cos;
-  using std::sin;
-  const Vector3<T> angles = get_angles(context);
-  const T cp = cos(angles[1]);
-  const T sp = sin(angles[1]);
-  const T sy = sin(angles[2]);
-  const T cy = cos(angles[2]);
-  if (abs(cp) < 1.0e-3) {
-    const char* function_name_less_Do = __func__ + 2;
-    ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
-  }
+  const Vector3<T> vdotEtc = vdot - CalcNplusDotTimesQdot(context, __func__);
 
-  const Vector3<T> v = get_angular_velocity(context);
-  Vector3<T> qdot;
-  DoMapVelocityToQDot(context, v, &qdot);
-  const T& rdot = qdot[0];
-  const T& pdot = qdot[1];
-  const T& ydot = qdot[2];
-  const T pdot_ydot = pdot * ydot;
-  const T rdot_pdot = rdot * pdot;
-  const T rdot_ydot = rdot * ydot;
-  const T sp_rdot_pdot = sp * rdot_pdot;
-  const T cp_rdot_ydot = cp * rdot_ydot;
+  // Mathematical trick: Although the function below was designed to efficiently
+  // calculate q̇ = N(q)⋅v, it can be used to calculate q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.
+  DoMapVelocityToQDot(context, vdotEtc, qddot);
+}
 
-  const T& wxdot = vdot[0];
-  const T& wydot = vdot[1];
-  const T& wzdot = vdot[2];
+template <typename T>
+void RpyBallMobilizer<T>::DoMapQDDotToAcceleration(
+    const systems::Context<T>& context,
+    const Eigen::Ref<const VectorX<T>>& qddot,
+    EigenPtr<VectorX<T>> vdot) const {
+  // --------------------------------------------------------------------------
+  // Seen in MapQDotToVelocity(), the generalized velocities v = [ωx, ωy, ωz]ᵀ
+  // are related to q̇ = [ṙ, ṗ, ẏ]ᵀ (time-derivatives of generalized positions)
+  // as shown below, whose matrix form is v = N⁺(q)⋅q̇, where N⁺ is a 3x3 matrix.
+  //
+  // ⌈ ωx ⌉   ⌈cos(y) cos(p)  -sin(y)  0⌉ ⌈ ṙ ⌉
+  // | ωy | = |sin(y) cos(p)   cos(y)  0| | ṗ |
+  // ⌊ ωz ⌋   ⌊      -sin(p)       0   1⌋ ⌊ ẏ ⌋
+  //
+  // For this mobilizer v = w_FM_F =  [ωx, ωy, ωz]ᵀ is the mobilizer M frame's
+  // angular velocity in the mobilizer F frame, expressed in frame F and
+  // q = [r, p, y]ᵀ  denote roll, pitch, yaw angles (generalized positions).
+  //
+  // There are various ways to calculate v̇ = [ω̇x, ω̇y, ω̇z]ᵀ (the time-derivatives
+  // of the generalized velocities). A simple one is v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  // --------------------------------------------------------------------------
+  // Calculate Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  const Vector3<T> NplusDotTimesQdot = CalcNplusDotTimesQdot(context, __func__);
 
-  Vector3<T> rhs;
-  rhs[0] = wxdot + cy * pdot_ydot + cy * sp_rdot_pdot + sy * cp_rdot_ydot;
-  rhs[1] = wydot + sy * pdot_ydot + sy * sp_rdot_pdot - cy * cp_rdot_ydot;
-  rhs[2] = wzdot + cp * rdot_pdot;
+  // Mathematical trick: Although the function below was designed to efficiently
+  // calculate v = N⁺(q)⋅q̇, it can be used to calculate N⁺(q)⋅q̈.
+  DoMapQDotToVelocity(context, qddot, vdot);
 
-  // Mathematical trick to efficiently calculate the result.
-  DoMapVelocityToQDot(context, rhs, qddot);
+  // Calculate v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  vdot->coeffRef(0) += NplusDotTimesQdot[0];
+  vdot->coeffRef(1) += NplusDotTimesQdot[1];
+  vdot->coeffRef(2) += NplusDotTimesQdot[2];
 }
 
 template <typename T>

multibody/tree/rpy_ball_mobilizer.h

@@ -261,6 +261,10 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
   void DoCalcNplusDotMatrix(const systems::Context<T>& context,
                             EigenPtr<MatrixX<T>> NplusDot) const final;
 
+  // Calculate the term Ṅ⁺(q,q̇)⋅q̇ which appears in v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  Vector3<T> CalcNplusDotTimesQdot(const systems::Context<T>& context,
+                                   const char* function_name) const;
+
   // Maps the generalized velocity v, which corresponds to the angular velocity
   // w_FM, to time derivatives of roll-pitch-yaw angles θ₀, θ₁, θ₂ in qdot.
   //
@@ -305,6 +309,12 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
                                 const Eigen::Ref<const VectorX<T>>& vdot,
                                 EigenPtr<VectorX<T>> qddot) const final;
 
+  // Maps qddot to vdot, which for this mobilizer is v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  // For simple mobilizers v̇ = q̈. This mobilizer's N and Ṅ⁺ are more elaborate.
+  void DoMapQDDotToAcceleration(const systems::Context<T>& context,
+                                const Eigen::Ref<const VectorX<T>>& qddot,
+                                EigenPtr<VectorX<T>> vdot) const final;
+
   std::unique_ptr<Mobilizer<double>> DoCloneToScalar(
       const MultibodyTree<double>& tree_clone) const override;
 

multibody/tree/test/rpy_ball_mobilizer_test.cc

@@ -244,6 +244,26 @@ TEST_F(RpyBallMobilizerTest, MapAccelerationToQDDotAndViceVersa) {
   const Vector3<double> qddot_expected = Ndot * wxyz + N * vdot;
   EXPECT_TRUE(CompareMatrices(qddot, qddot_expected, kTolerance,
                               MatrixCompareType::relative));
+
+  // Compute the 3x3 N⁺(q) matrix and its time-derivative Ṅ⁺(q,q̇).
+  MatrixX<double> Nplus(3, 3), Nplusdot(3, 3);
+  mobilizer_->CalcNplusMatrix(*context_, &Nplus);
+  mobilizer_->CalcNplusDotMatrix(*context_, &Nplusdot);
+
+  // Starting with the previous q̈, use MapQDDotToAcceleration() to calculate v̇.
+  Vector3<double> wdot;
+  mobilizer_->MapQDDotToAcceleration(*context_, qddot, &wdot);
+
+  // Verify equivalence of v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈ and MapQDDotToAcceleration().
+  Vector3<double> qdot;
+  mobilizer_->MapVelocityToQDot(*context_, wxyz, &qdot);
+  const Vector3<double> vdot_expected = Nplusdot * qdot + Nplus * qddot;
+  EXPECT_TRUE(CompareMatrices(vdot_expected, vdot, 16 * kTolerance,
+                              MatrixCompareType::relative));
+
+  // Verify MapQDDotToAcceleration() is the inverse of MapAccelerationToQDDot().
+  EXPECT_TRUE(CompareMatrices(vdot, wdot, 16 * kTolerance,
+                              MatrixCompareType::relative));
 }
 
 TEST_F(RpyBallMobilizerTest, SingularityError) {