Partially respond to reviewer comments.

Author: Paul Mitiguy

multibody/tree/quaternion_floating_mobilizer.cc

@@ -281,23 +281,25 @@ void QuaternionFloatingMobilizer<T>::ProjectSpatialForce(
 template <typename T>
 Eigen::Matrix<T, 4, 3> QuaternionFloatingMobilizer<T>::CalcLMatrix(
     const Quaternion<T>& q_FM) {
-  // This L matrix helps us compute both N(q) and N⁺(q) since it turns out that:
-  //   N(q) = L(q_FM/2)
-  // and:
-  //   N⁺(q) = L(2 q_FM)ᵀ
+  // The L matrix helps compute Nᵣ(q_FM) and N⁺ᵣ(q_FM), which are the rotational
+  // parts of the N matrix N(q) and Nplus matrix N⁺(q). It turns out that:
+  //
+  //   Nᵣ(q_FM) = 0.5 * L(q_FM) = L(q_FM/2) and
+  //   N⁺ᵣ(q_FM) = 2 * L(q_FM)ᵀ = L(2 q_FM)ᵀ
+  //
   // See Eqs. 5 and 6 in Section 9.2 of Paul's book
   // [Mitiguy (August 7) 2017, §9.2], for the time derivative of the vector
   // component of the quaternion (Euler parameters). Notice however here we use
   // qs and qv for the "scalar" and "vector" components of the quaternion q_FM,
   // respectively, while Mitiguy uses ε₀ and ε (in bold), respectively.
-  // This mobilizer is parameterized by the angular velocity w_FM, i.e. time
+  // This mobilizer is parameterized by the angular velocity w_FM-F, i.e. time
   // derivatives of the vector component of the quaternion are taken in the F
   // frame. If you are confused by this, notice that the vector component of a
   // quaternion IS a vector, and therefore you must specify in what frame time
   // derivatives are taken.
   //
   // Notice this is equivalent to:
-  // Dt_F(q) = 1/2 * w_FM⋅q_FM, where ⋅ denotes the "quaternion product" and
+  // Dt_F(q_FM) = 1/2 * w_FM⋅q_FM, where ⋅ denotes the "quaternion product" and
   // both the vector component qv_FM of q_FM and w_FM are expressed in frame F.
   // Dt_F(q) is short for [Dt_F(q)]_F.
   // The expression above can be written as:
@@ -306,19 +308,21 @@ Eigen::Matrix<T, 4, 3> QuaternionFloatingMobilizer<T>::CalcLMatrix(
   //         = 1/2 * (-w_FM.dot(qv_F); (qs * Id - [qv_F]x) * w_FM)
   //         = L(q_FM/2) * w_FM
   // That is:
-  //        |         -qv_Fᵀ    |
-  // L(q) = | qs * Id - [qv_F]x |
-
-  const T qs = q_FM.w();             // The scalar component.
-  const Vector3<T> qv = q_FM.vec();  // The vector component.
-  const Vector3<T> mqv = -qv;        // minus qv.
+  //           |         -qv_Fᵀ    |   ⌈ -qx   -qy   -qz ⌉
+  // L(q_FM) = | qs * Id - [qv_F]x | = |  qw    qz   -qy |
+  //                                   | -qz    qw    qx |
+  //                                   ⌊  qy   -qx    qw ⌋
 
-  // NOTE: the rows of this matrix are in an order consistent with the order
-  // in which we store the quaternion in the state, with the scalar component
-  // first followed by the vector component.
-  return (Eigen::Matrix<T, 4, 3>() << mqv.transpose(), qs, qv.z(), mqv.y(),
-          mqv.z(), qs, qv.x(), qv.y(), mqv.x(), qs)
-      .finished();
+  const T& qw = q_FM.w();  // Scalar part of the quaternion.
+  const T& qx = q_FM.x();  // q_FM.vec() = [q_FM.x(), q_FM.y(), q_FM.z()] is
+  const T& qy = q_FM.y();  // the vector part of the quaternion.
+  const T& qz = q_FM.z();
+  // clang-format off
+  return (Eigen::Matrix<T, 4, 3>() << -qx, -qy, -qz,
+                                       qw,  qz, -qy,
+                                      -qz,  qw,  qx,
+                                       qy, -qx,  qw).finished();
+  // clang-format on
 }
 
 template <typename T>
@@ -339,20 +343,17 @@ QuaternionFloatingMobilizer<T>::QuaternionRateToAngularVelocityMatrix(
   const Matrix4<T> dqnorm_dq =
       (Matrix4<T>::Identity() - q_FM_tilde * q_FM_tilde.transpose()) / q_norm;
 
-  // With L given by CalcLMatrix we have:
-  // N⁺(q_tilde) = L(2 q_FM_tilde)ᵀ
-  return CalcLMatrix({2.0 * q_FM_tilde[0], 2.0 * q_FM_tilde[1],
-                      2.0 * q_FM_tilde[2], 2.0 * q_FM_tilde[3]})
-             .transpose() *
+  // With L given by CalcLMatrix(), we have N⁺(q_tilde) = 2 * L(q_FM_tilde)ᵀ.
+  return CalcTwoTimesLMatrixTranspose(
+             {q_FM_tilde[0], q_FM_tilde[1], q_FM_tilde[2], q_FM_tilde[3]}) *
          dqnorm_dq;
 }
 
 template <typename T>
 void QuaternionFloatingMobilizer<T>::DoCalcNMatrix(
     const systems::Context<T>& context, EigenPtr<MatrixX<T>> N) const {
   // Upper-left block
-  N->template block<4, 3>(0, 0) =
-      AngularVelocityToQuaternionRateMatrix(get_quaternion(context));
+  N->template block<4, 3>(0, 0) = CalcLMatrixOverTwo(get_quaternion(context));
   // Upper-right block
   N->template block<4, 3>(0, 3).setZero();
   // Lower-left block
@@ -397,14 +398,13 @@ void QuaternionFloatingMobilizer<T>::DoCalcNDotMatrix(
   // Calculate the time-derivative of the quaternion, i.e., q̇ᵣ = Nᵣ(q)⋅vᵣ.
   const Quaternion<T> q_FM = get_quaternion(context);
   const Vector3<T> w_FM_F = get_angular_velocity(context);
-  const Vector4<T> qdot = AngularVelocityToQuaternionRateMatrix(q_FM) * w_FM_F;
-  const Quaternion<T> half_qdot(0.5 * qdot[0], 0.5 * qdot[1], 0.5 * qdot[2],
-                                0.5 * qdot[3]);
+  const Vector4<T> qdot = CalcLMatrixOverTwo(q_FM) * w_FM_F;
+  const Quaternion<T> qdot_FM(qdot[0], qdot[1], qdot[2], qdot[3]);
 
-  // Leveraging comments and code in AngularVelocityToQuaternionRateMatrix()
-  // and noting that Nᵣ(qᵣ) = L(q_FM/2), where the elements of the matrix L are
-  // linear in q_FM = [qw, qx, qy, qz]ᵀ, so Ṅᵣ(qᵣ,q̇ᵣ) = L(q̇_FM/2).
-  const Eigen::Matrix<T, 4, 3> NrDotMatrix = CalcLMatrix(half_qdot);
+  // Leveraging comments and code in CalcLMatrixOverTwo() and noting that
+  // Nᵣ(qᵣ) = 0.5 * L(q_FM), where the elements of the matrix L are linear in
+  // q_FM = [qw, qx, qy, qz]ᵀ, so Ṅᵣ(qᵣ,q̇ᵣ) = 0.5 * L(q̇_FM).
+  const Eigen::Matrix<T, 4, 3> NrDotMatrix = CalcLMatrixOverTwo(qdot_FM);
 
   // Form the Ṅ(q,q̇) matrix associated with q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.
   Ndot->template block<4, 3>(0, 0) = NrDotMatrix;  // Upper-left block.
@@ -435,14 +435,13 @@ void QuaternionFloatingMobilizer<T>::DoCalcNplusDotMatrix(
   // Calculate the time-derivative of the quaternion, i.e., q̇ᵣ = Nᵣ(q)⋅vᵣ.
   const Quaternion<T> q_FM = get_quaternion(context);
   const Vector3<T> w_FM_F = get_angular_velocity(context);
-  const Vector4<T> qdot = AngularVelocityToQuaternionRateMatrix(q_FM) * w_FM_F;
-  const Quaternion<T> twice_qdot(2.0 * qdot[0], 2.0 * qdot[1], 2.0 * qdot[2],
-                                 2.0 * qdot[3]);
+  const Vector4<T> qdot = CalcLMatrixOverTwo(q_FM) * w_FM_F;
+  const Quaternion<T> qdot_FM(qdot[0], qdot[1], qdot[2], qdot[3]);
 
-  // Leveraging comments and code in AngularVelocityToQuaternionRateMatrix(()
-  // and noting that N⁺ᵣ(qᵣ) = L(2 * q_FM)ᵀ, where the elements of the matrix L
-  // are linear in q_FM = [qw, qx, qy, qz]ᵀ, so Ṅ⁺ᵣ(qᵣ,q̇ᵣ) = L(2 * q̇_FM)ᵀ.
-  const Eigen::Matrix<T, 3, 4> NrPlusDot = CalcLMatrix(twice_qdot).transpose();
+  // Since N⁺ᵣ(qᵣ) = 2 * L(q_FM)ᵀ, where L(q_FM) is linear in q_FM, hence
+  // Ṅ⁺ᵣ(qᵣ,q̇ᵣ) = 2 * L(q̇_FM)ᵀ.
+  const Eigen::Matrix<T, 3, 4> NrPlusDot =
+      CalcTwoTimesLMatrixTranspose(qdot_FM);
 
   // Form the Ṅ⁺(q,q̇) matrix associated with v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
   NplusDot->template block<3, 4>(0, 0) = NrPlusDot;  // Upper-left block.
@@ -456,10 +455,9 @@ void QuaternionFloatingMobilizer<T>::DoMapVelocityToQDot(
     const systems::Context<T>& context, const Eigen::Ref<const VectorX<T>>& v,
     EigenPtr<VectorX<T>> qdot) const {
   const Quaternion<T> q_FM = get_quaternion(context);
-  // Angular component, q̇_WB = N(q)⋅w_WB:
-  qdot->template head<4>() =
-      AngularVelocityToQuaternionRateMatrix(q_FM) * v.template head<3>();
-  // Translational component, ṗ_WB = v_WB:
+  // Angular component, q̇_FM = 0.5 * L(q_FM) ⋅ w_FM_F:
+  qdot->template head<4>() = CalcLMatrixOverTwo(q_FM) * v.template head<3>();
+  // Translational component, ṗ_FoMo_F = v_FMo_F:
   qdot->template tail<3>() = v.template tail<3>();
 }
 
@@ -501,7 +499,7 @@ void QuaternionFloatingMobilizer<T>::DoMapAccelerationToQDDot(
   const Vector3<T> w_FM_F = get_angular_velocity(context);
   const T w_squared_over_four = 0.25 * w_FM_F.squaredNorm();
   qddot->template head<4>() =
-      AngularVelocityToQuaternionRateMatrix(q_FM) * vdot.template head<3>() -
+      CalcLMatrixOverTwo(q_FM) * vdot.template head<3>() -
       w_squared_over_four * Vector4<T>(q_FM.w(), q_FM.x(), q_FM.y(), q_FM.z());
 
   // Calculate the 2nd-derivative of the position vector p_FoMo_F = [x, y, z]ᵀ
@@ -520,7 +518,7 @@ void QuaternionFloatingMobilizer<T>::DoMapQDDotToAcceleration(
   //
   // For the rotational part of this mobilizer, the 1st-derivatives of the
   // generalized velocities ẇ_FM_F = v̇ᵣ = [ẇx, ẇy, ẇz]ᵀ are related to the
-  // 2nd-derivatives of the generalized positions q̈̇_FM = q̇ᵣ = [q̈w, q̈x, q̈y, q̈z]ᵀ
+  // 2nd-derivatives of the generalized positions q̈_FM = q̇ᵣ = [q̈w, q̈x, q̈y, q̈z]ᵀ
   // as v̇ᵣ = N⁺ᵣ(q)⋅q̈_FM, where N⁺ᵣ(q) is the 3x4 matrix below.
   // Note: Perhaps surprisingly, Ṅ⁺ᵣ(q,q̇)⋅q̇ = [0, 0, 0]ᵀ.
   //

multibody/tree/quaternion_floating_mobilizer.h

@@ -331,26 +331,43 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
       const MultibodyTree<symbolic::Expression>& tree_clone) const final;
 
  private:
-  // Helper to compute the kinematic map N(q). L ∈ ℝ⁴ˣ³.
-  static Eigen::Matrix<T, 4, 3> CalcLMatrix(const Quaternion<T>& q);
-
-  // Returns the 4x3 Nᵣ(q) matrix that relates q̇_FM = Nᵣ(q) * w_FM_F, where
-  // q̇_FM = [q̇w, q̇x, q̇y, q̇z]ᵀ (time-derivative of quaternion for frames F and M)
-  // w_FM_F = [ωx, ωy, ωz]ᵀ (frame M's angular velocity in F, expressed in F).
+  // Returns the 4x3 matrix L(q_FM) where q_FM = [qw, qx, qy, qz]ᵀ is the
+  // quaternion relating frames F and M.
   //
-  // ⌈ q̇w ⌉       ⌈ -qx   -qy   -qz ⌉ ⌈ ωx ⌉
-  // | q̇x | = 0.5 |  qw    qz   -qy | | ωy |
-  // | q̇y |       | -qz    qw    qx | ⌊ ωz ⌋
-  // ⌊ q̇z ⌋       ⌊  qy   -qx    qw ⌋
+  //           ⌈ -qx   -qy   -qz ⌉
+  // L(q_FM) = |  qw    qz   -qy |
+  //           | -qz    qw    qx |
+  //           ⌊  qy   -qx    qw ⌋
   //
-  // Herein L(q_FM) is the 4x3 matrix shown above, so Nᵣ(q) = 0.5 * L(q_FM).
-  // Due to the linear nature of L(q_FM), 0.5 * L(q_FM) = L(0.5 * q_FM).
-  static Eigen::Matrix<T, 4, 3> AngularVelocityToQuaternionRateMatrix(
-      const Quaternion<T>& q_FM) {
+  // The matrix L(q_FM) is an intermediary for various quaternion-related
+  // calculations. For example, denoting w_FM_F = [ωx, ωy, ωz]ᵀ as frame M's
+  // angular velocity in frame F, expressed in F, the following involve L(q_FM):
+  //
+  // q̇_FM = 0.5 * L(q_FM) * w_FM_F
+  // q̈_FM = 0.5 * L(q_FM) * ẇ_FM_F - 0.25 ω² q_FM    Note: ω² = |w_FM_F|²
+  // w_FM_F = 2 * L(q_FM)ᵀ * q̇_FM
+  // ẇ_FM_F = 2 * L(q_FM)ᵀ * q̇_FM
+  // Nᵣ(q_FM) = 0.5 * L(q_FM)  Nᵣ is the rotational part of the N matrix.
+  // N⁺ᵣ(q_FM) = 2 * L(q_FM)ᵀ  N⁺ᵣ is the rotational part of the Nplus matrix.
+  static Eigen::Matrix<T, 4, 3> CalcLMatrix(const Quaternion<T>& q);
+
+  // Returns the 4x3 matrix 0.5 * L(q_FM), where q_FM = [qw, qx, qy, qz] is the
+  // quaternion relating frames F and M. As documented in CalcLMatrix(),
+  // L(q_FM) depends linearly on q_FM, so 0.5 * L(q_FM) = L(0.5 * q_FM).
+  static Eigen::Matrix<T, 4, 3> CalcLMatrixOverTwo(const Quaternion<T>& q_FM) {
     return CalcLMatrix(
         {0.5 * q_FM.w(), 0.5 * q_FM.x(), 0.5 * q_FM.y(), 0.5 * q_FM.z()});
   }
 
+  // Returns the 3x4 matrix 2 * L(q_FM)ᵀ, where q_FM = [qw, qx, qy, qz] is the
+  // quaternion relating frames F and M. As documented in CalcLMatrix(),
+  // L(q_FM) depends linearly on q_FM, so 2 * L(q_FM)ᵀ = L(2 * q_FM)ᵀ.
+  static Eigen::Matrix<T, 3, 4> CalcTwoTimesLMatrixTranspose(
+      const Quaternion<T>& q_FM) {
+    return CalcLMatrix({2 * q_FM.w(), 2 * q_FM.x(), 2 * q_FM.y(), 2 * q_FM.z()})
+        .transpose();
+  }
+
   // Helper to compute the kinematic map N⁺(q) from quaternion time derivative
   // to angular velocity for which w_WB = N⁺(q)⋅q̇_WB.
   // This method can take a non unity quaternion q_tilde such that