Partial update, responding to reviewer comments.

Author: Paul Mitiguy

multibody/tree/quaternion_floating_mobilizer.cc

@@ -314,8 +314,9 @@ QuaternionFloatingMobilizer<T>::QuaternionRateToAngularVelocityMatrix(
   const Eigen::Matrix<T, 3, 4> NrPlus_q_unit = CalcTwoTimesQMatrixTranspose(
       {q_unit[0], q_unit[1], q_unit[2], q_unit[3]});
 
-  // Returns the matrix that when multiplied by q̇_FM produces
-  // w_FM_F (M's angular velocity in F, expressed in F).
+  // Returns the matrix that when multiplied by q̇_FM produces w_FM_F
+  // (M's angular velocity in F, expressed in F). Note: When q_FM is a unit
+  // quaternion (so q_norm = 1), the returned value is denoted Nᵣ⁺(q̂_FM).
   return NrPlus_q_unit * dqnorm_dq;
 }
 
@@ -324,13 +325,11 @@ void QuaternionFloatingMobilizer<T>::DoCalcNMatrix(
     const systems::Context<T>& context, EigenPtr<MatrixX<T>> N) const {
   // Upper-left block (rotational part of the N matrix) is Nᵣ ≜ 0.5 Q_FM.
   // See QuaternionFloatingMobilizer::CalcQMatrix() for details.
+  // The lower-right block (translational part of the N matrix) is [I₃₃].
   N->template block<4, 3>(0, 0) = CalcQMatrixOverTwo(get_quaternion(context));
-  // Upper-right block
-  N->template block<4, 3>(0, 3).setZero();
-  // Lower-left block
-  N->template block<3, 3>(4, 0).setZero();
-  // Lower-right block (translational part of the N matrix).
-  N->template block<3, 3>(4, 3).setIdentity();
+  N->template block<4, 3>(0, 3).setZero();      // Upper-right block.
+  N->template block<3, 3>(4, 0).setZero();      // Lower-left block.
+  N->template block<3, 3>(4, 3).setIdentity();  // Lower-right block.
 }
 
 template <typename T>
@@ -343,13 +342,10 @@ void QuaternionFloatingMobilizer<T>::DoCalcNplusMatrix(
   // in frame F, expressed in F. Hence, this accounts for a non-unit q_FM.
   const Quaternion<T> q_FM = get_quaternion(context);
   Nplus->template block<3, 4>(0, 0) =
-      QuaternionRateToAngularVelocityMatrix(q_FM);
-  // Upper-right block
-  Nplus->template block<3, 3>(0, 4).setZero();
-  // Lower-left block
-  Nplus->template block<3, 4>(3, 0).setZero();
-  // Lower-right block
-  Nplus->template block<3, 3>(3, 4).setIdentity();
+      QuaternionRateToAngularVelocityMatrix(q_FM);  // Upper-left block.
+  Nplus->template block<3, 3>(0, 4).setZero();      // Upper-right block.
+  Nplus->template block<3, 4>(3, 0).setZero();      // Lower-left block.
+  Nplus->template block<3, 3>(3, 4).setIdentity();  // Lower-right block.
 }
 
 template <typename T>
@@ -366,11 +362,6 @@ void QuaternionFloatingMobilizer<T>::DoCalcNDotMatrix(
   // | q̇y |       | -qz    qw    qx | ⌊ ωz ⌋
   // ⌊ q̇z ⌋       ⌊  qy   -qx    qw ⌋
   //
-  // For the translational part of this mobilizer, the time-derivative of the
-  // generalized positions q̇ₜ = [ẋ, ẏ, ż]ᵀ are related to the translational
-  // generalized velocities v_FM_F = vₜ = [vx, vy, vz]ᵀ as q̇ₜ = Nₜ(q)⋅vₜ, where
-  // Nₜ(q) = [I₃₃] (3x3 identity matrix). Hence, Ṅₜ(q,q̇ᵣ) = [0₃₃] (zero matrix).
-
   // 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);
@@ -381,6 +372,11 @@ void QuaternionFloatingMobilizer<T>::DoCalcNDotMatrix(
   // q_FM = [qw, qx, qy, qz]ᵀ, so Ṅᵣ(qᵣ,q̇ᵣ) = 0.5 * Q(q̇_FM).
   const Eigen::Matrix<T, 4, 3> NrDotMatrix = CalcQMatrixOverTwo(qdot_FM);
 
+  // For the translational part of this mobilizer, the time-derivative of the
+  // generalized positions q̇ₜ = [ẋ, ẏ, ż]ᵀ are related to the translational
+  // generalized velocities v_FM_F = vₜ = [vx, vy, vz]ᵀ as q̇ₜ = Nₜ(q)⋅vₜ, where
+  // Nₜ(q) = [I₃₃] (3x3 identity matrix). Hence, Ṅₜ(q,q̇ᵣ) = [0₃₃] (zero matrix).
+
   // 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.
   Ndot->template block<4, 3>(0, 3).setZero();      // Upper-right block.
@@ -402,11 +398,6 @@ void QuaternionFloatingMobilizer<T>::DoCalcNplusDotMatrix(
   // ⌊ ωz ⌋       | -qz   -qy    qx   qw | | q̇y |
   //                                       ⌊ q̇z ⌋
   //
-  // For the translational part of this mobilizer, the translational generalized
-  // velocities v_FM_F = vₜ = [vx, vy, vz]ᵀ are related to the time-derivatives
-  // of the generalized positions q̇ₜ = [ẋ, ẏ, ż]ᵀ as vₜ = N⁺ₜ(q)⋅q̇ₜ, where
-  // Nₜ(q) = [I₃₃] (3x3 identity matrix). Thus, Ṅ⁺ₜ(q,q̇ₜ) = [0₃₃] (zero matrix).
-
   // 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);
@@ -416,11 +407,16 @@ void QuaternionFloatingMobilizer<T>::DoCalcNplusDotMatrix(
   // In view of the documentation in CalcQMatrix(), since
   // N⁺ᵣ(q_FM) = 2 * (Q_FM)ᵀ, where Q_FM is linear in the elements of
   // q_FM = [qw, qx, qy, qz]ᵀ, hence Ṅ⁺ᵣ(q̇_FM) = 2 * (Q̇_FM)ᵀ.
-  // TODO(Mitiguy) Ensure this calculation is consistent with the precise
-  //  definition of this function (the definition is also a TODO).
+  // TODO(Mitiguy) Ensure this calculation provides the time derivative of the
+  //  calculation in DoCalcNplusMatrix().
   const Eigen::Matrix<T, 3, 4> NrPlusDot =
       CalcTwoTimesQMatrixTranspose(qdot_FM);
 
+  // For the translational part of this mobilizer, the translational generalized
+  // velocities v_FM_F = vₜ = [vx, vy, vz]ᵀ are related to the time-derivatives
+  // of the generalized positions q̇ₜ = [ẋ, ẏ, ż]ᵀ as vₜ = N⁺ₜ(q)⋅q̇ₜ, where
+  // Nₜ(q) = [I₃₃] (3x3 identity matrix). Thus, Ṅ⁺ₜ(q,q̇ₜ) = [0₃₃] (zero matrix).
+
   // 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.
   NplusDot->template block<3, 3>(0, 4).setZero();    // Upper-right block.

multibody/tree/quaternion_floating_mobilizer.h

@@ -293,22 +293,49 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
     return velocity.get_coeffs();
   }
 
-  // This function calculates this mobilizers N matrix using the quaternion in
-  // context, _without_ normalization -- differs from DoCalcNplusMatrix().
+  // This function calculates this mobilizer's N matrix using the quaternion in
+  // context, _without_ normalization, which differs from DoCalcNplusMatrix().
+  // This mobilizer's N(q) matrix relates q̇ (time derivatives of 7 generalized
+  // positions) to v (6 generalized velocities) as q̇ = N(q)⋅v, where
+  // N(q) = ⎡ Nᵣ(q)  0₄₃ ⎤   0₄₃ is the 4x3 zero matrix.
+  //        ⎣ 0₃₃    I₃₃ ⎦   I₃₃ is the 3x3 identity matrix.
+  // Nᵣ(q) = 0.5 * QuaternionFloatingMobilizer::CalcQMatrix() is a 4x3 matrix.
+  // Note: The time-derivative of the quaternion qᵣ in context can be calculated
+  // q̇ᵣ = Nᵣ(q) w_FM_F, where w_FM_F is frame M's angular velocity in frame F,
+  // expressed in F. For a unit quaternion q̂ᵣ, we prove d/dt(q̂ᵣ) satisfies the
+  // "orthogonality constraint" i.e., d/dt(q̂ᵣ ⋅ q̂ᵣ = 1)  =>  q̂ᵣ ⋅ d/dt(q̂ᵣ) = 0
+  // via q̂ᵣ ⋅ d/dt(q̂ᵣ) = q̂ᵣ ⋅ Nᵣ(q̂ᵣ) w_FM_F = [0 0 0] w_FM_F = 0.
+  // For a non-unit quaternion qᵣ, the orthogonality constraint is proved via
+  // qᵣ ⋅ d/dt(qᵣ) =  qᵣ ⋅ Nᵣ(qᵣ) w_FM_F = |qᵣ| q̂ᵣ ⋅ |qᵣ| Nᵣ(q̂ᵣ) w_FM_F =
+  // |qᵣ|² q̂ᵣ ⋅ Nᵣ(q̂ᵣ) w_FM_F = |qᵣ|² [0 0 0] w_FM_F = 0, where this proof
+  // uses the fact Nᵣ(qᵣ) is linear in qᵣ and qᵣ = |qᵣ| q̂ᵣ.
+  // Summary: If the quaternion in context is a unit quaternion q̂ᵣ, then its
+  // time-derivative can be calculated as d/dt(q̂ᵣ) = Nᵣ(q̂ᵣ) w_FM_F.
+  // If the quaternion is context is a non-unit quaternion, then its time-
+  // derivative can be calculated d/dt(qᵣ) = Nᵣ(qᵣ) w_FM_F = |qᵣ| Nᵣ(q̂ᵣ) w_FM_F.
   void DoCalcNMatrix(const systems::Context<T>& context,
                      EigenPtr<MatrixX<T>> N) const final;
 
-  // This function calculates this mobilizers N matrix using the quaternion in
-  // context, _with_ normalization -- differs from DoCalcNMatrix().
+  // This function calculates this mobilizer's N⁺ matrix using the quaternion in
+  // context, _with_ normalization, which differs from DoCalcNMatrix().
+  // This mobilizer's N⁺(q) matrix relates v (6 generalized velocities) to q̇
+  // (time derivatives of 7 generalized positions) as v = N⁺(q)⋅q̇, where
+  // N⁺(q) = ⎡ Nᵣ(q)⁺  0₃₄ ⎤   0₃₄ is the 4x3 zero matrix.
+  //         ⎣ 0₃₃     I₃₃ ⎦   I₃₃ is the 3x3 identity matrix.
+  // Nᵣ⁺(q) is a 3x4 matrix formed by QuaternionRateToAngularVelocityMatrix().
+  // Note: Contextual definition of Nᵣ⁺ -- denoting q̂_FM = q_FM / |q_FM|,
+  // w_FM_F = Nᵣ⁺ * d/dt(q̂_FM), where w_FM_F is frame M's angular velocity
+  // in frame F, expressed in F. Hence, this accounts for a non-unit q_FM.
   void DoCalcNplusMatrix(const systems::Context<T>& context,
                          EigenPtr<MatrixX<T>> Nplus) const final;
 
-  // This function calculates this mobilizers Ṅ matrix using the quaternion in
-  // context, _without_ normalization.
+  // This function calculates this mobilizer's Ṅ matrix using the quaternion
+  // and angular velocity in the context.
   void DoCalcNDotMatrix(const systems::Context<T>& context,
                         EigenPtr<MatrixX<T>> Ndot) const final;
 
-  // TODO(Mitiguy) Provide a precise definition of this function.
+  // This function calculates this mobilizer's Ṅ⁺ matrix using the quaternion
+  // and angular velocity in the context.
   void DoCalcNplusDotMatrix(const systems::Context<T>& context,
                             EigenPtr<MatrixX<T>> NplusDot) const final;
 
@@ -350,17 +377,17 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
   // @note Herein, we denote the function that forms this matrix as Q(q).
   // When q is the quaternion q_FM that relates the orientation of frames F
   // F and M, we define the matrix Q_FM ≜ Q(q_FM).  When q_FM is a unit
-  // quaternion, we denote it as q̂_FM and Q̂_FM ≜ Q(q̂_FM).
+  // quaternion, we denote it as q̂_FM. Similarly, Q̂_FM ≜ Q(q̂_FM).
   // Many uses of Q_FM and Q̂_FM are associated with angular velocity expressed
   // in a particular frame. The examples below show them used in conjunction
   // with w_FM_F (frame M's angular velocity in frame F, expressed in F).
   // Another use of Q_FM and Q̂_FM are for rotational parts of this mobilizer's
-  // N and Nplus matrices, namely as Nᵣ ≜ 0.5 Q_FM and Nᵣ⁺ ≜ 2 (Q̂_FM)ᵀ.
+  // N and Nplus matrices, e.g., as Nᵣ ≜ 0.5 Q_FM and Nᵣ⁺ = 2 (Q̂_FM)ᵀ.
   //
   // q̇_FM = 0.5 * Q_FM * w_FM_F
   // q̈_FM = 0.5 * Q_FM * ẇ_FM_F - 0.25 ω² q_FM    Note: ω² = |w_FM_F|²
-  // w_FM_F = 2 * (Q̂_FM)ᵀ * q̇_FM
-  // ẇ_FM_F = 2 * (Q̂_FM)ᵀ * q̈_FM
+  // w_FM_F = 2 * (Q̂_FM)ᵀ * d/dt(q̂_FM)
+  // ẇ_FM_F = 2 * (Q̂_FM)ᵀ * d²/dt²(q̂_FM)
   //
   // @note Since the elements of the matrix returned by Q(q) depend linearly on
   // qw, qx, qy, qz, s * Q(q) = Q(s * q), where s is a scalar (e.g., 0.5 or 2).
@@ -394,7 +421,7 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
 
   // Helper to compute this mobilizer's rotational kinematic map Nᵣ⁺(q̂_FM) from
   // quaternion time derivative to angular velocity as w_FM_F = Nᵣ⁺ * d/dt(q̂_FM)
-  // where q̂_FM ≜ q_FM / |q_FM|, where w_FM_F is frame M's angular velocity
+  // where q̂_FM ≜ q_FM / |q_FM| and w_FM_F is frame M's angular velocity
   // in frame F, expressed in F. Hence, this accounts for a non-unit q_FM.
   // param[in] q_FM quaternion describing the orientation of frames F and M.
   // @note The argument q_FM can be a non-unit quaternion as this function