Changes to function name, matrix name, documentation.

Author: Paul Mitiguy

multibody/tree/quaternion_floating_mobilizer.cc

@@ -279,44 +279,11 @@ void QuaternionFloatingMobilizer<T>::ProjectSpatialForce(
 }
 
 template <typename T>
-Eigen::Matrix<T, 4, 3> QuaternionFloatingMobilizer<T>::CalcLMatrix(
+Eigen::Matrix<T, 4, 3> QuaternionFloatingMobilizer<T>::CalcQMatrix(
     const Quaternion<T>& 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_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_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.
-  // Using Dt_F(q) as an abbreviation of [Dt_F(q_FM)]_F.
-  // The expression above can be written as:
-  // Dt_F(q) = 1/2 * (-w_FM.dot(qv_F); qs * w_FM + w_FM.cross(qv_F))
-  //         = 1/2 * (-w_FM.dot(qv_F); qs * w_FM - qv_F.cross(w_FM))
-  //         = 1/2 * (-w_FM.dot(qv_F); (qs * Id - [qv_F]x) * w_FM)
-  //         = 1/2 * L(q_FM) * w_FM
-  //         = L(q_FM/2) * w_FM
-  // That is:
-  //           |         -qv_Fᵀ    |   ⌈ -qx   -qy   -qz ⌉
-  // L(q_FM) = | qs * Id - [qv_F]x | = |  qw    qz   -qy |
-  //                                   | -qz    qw    qx |
-  //                                   ⌊  qy   -qx    qw ⌋
-
-  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& qw = q_FM.w();
+  const T& qx = q_FM.x();
+  const T& qy = q_FM.y();
   const T& qz = q_FM.z();
   // clang-format off
   return (Eigen::Matrix<T, 4, 3>() << -qx, -qy, -qz,
@@ -344,29 +311,31 @@ 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) = 2 * L(q_FM_tilde)ᵀ.
-  return CalcTwoTimesLMatrixTranspose(
+  // From documentation in CalcQMatrix(), N⁺(q_tilde) = 2 * (Q_FM_tilde)ᵀ.
+  return CalcTwoTimesQMatrixTranspose(
              {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) = CalcLMatrixOverTwo(get_quaternion(context));
+  // Upper-left block (rotational part of the N matrix) is 0.5 * Matrix(q_FM).
+  // See QuaternionFloatingMobilizer::CalcQMatrix() for details.
+  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
+  // Lower-right block (translational part of the N matrix).
   N->template block<3, 3>(4, 3).setIdentity();
 }
 
 template <typename T>
 void QuaternionFloatingMobilizer<T>::DoCalcNplusMatrix(
     const systems::Context<T>& context, EigenPtr<MatrixX<T>> Nplus) const {
-  // Upper-left block
+  // Upper-left block (rotational part of the N⁺ matrix) is [2 * Matrix(q_FM)]ᵀ
+  // See QuaternionFloatingMobilizer::CalcQMatrix() for details.
   Nplus->template block<3, 4>(0, 0) =
       QuaternionRateToAngularVelocityMatrix(get_quaternion(context));
   // Upper-right block
@@ -399,12 +368,12 @@ 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 = CalcLMatrixOverTwo(q_FM) * w_FM_F;
+  const Vector4<T> qdot = CalcQMatrixOverTwo(q_FM) * w_FM_F;
   const Quaternion<T> qdot_FM(qdot[0], qdot[1], qdot[2], qdot[3]);
 
-  // Since Nᵣ(qᵣ) = 0.5 * L(q_FM), where L(q_FM) is linear in the elements of
-  // q_FM = [qw, qx, qy, qz]ᵀ, so Ṅᵣ(qᵣ,q̇ᵣ) = 0.5 * L(q̇_FM).
-  const Eigen::Matrix<T, 4, 3> NrDotMatrix = CalcLMatrixOverTwo(qdot_FM);
+  // Since Nᵣ(qᵣ) = 0.5 * Q(q_FM), where Q(q_FM) is linear in the elements of
+  // q_FM = [qw, qx, qy, qz]ᵀ, so Ṅᵣ(qᵣ,q̇ᵣ) = 0.5 * Q(q̇_FM).
+  const Eigen::Matrix<T, 4, 3> NrDotMatrix = CalcQMatrixOverTwo(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,13 +404,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 = CalcLMatrixOverTwo(q_FM) * w_FM_F;
+  const Vector4<T> qdot = CalcQMatrixOverTwo(q_FM) * w_FM_F;
   const Quaternion<T> qdot_FM(qdot[0], qdot[1], qdot[2], qdot[3]);
 
-  // Since N⁺ᵣ(qᵣ) = 2 * L(q_FM)ᵀ, where L(q_FM) is linear in the elements of
-  // q_FM = [qw, qx, qy, qz]ᵀq_FM, hence Ṅ⁺ᵣ(qᵣ,q̇ᵣ) = 2 * L(q̇_FM)ᵀ.
+  // Since N⁺ᵣ(qᵣ) = 2 * Q(q_FM)ᵀ, where Q(q_FM) is linear in the elements of
+  // q_FM = [qw, qx, qy, qz]ᵀq_FM, hence Ṅ⁺ᵣ(qᵣ,q̇ᵣ) = 2 * Q(q̇_FM)ᵀ.
   const Eigen::Matrix<T, 3, 4> NrPlusDot =
-      CalcTwoTimesLMatrixTranspose(qdot_FM);
+      CalcTwoTimesQMatrixTranspose(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.
@@ -455,8 +424,8 @@ 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̇_FM = 0.5 * L(q_FM) ⋅ w_FM_F:
-  qdot->template head<4>() = CalcLMatrixOverTwo(q_FM) * v.template head<3>();
+  // Angular component, q̇_FM = 0.5 * Q(q_FM) ⋅ w_FM_F:
+  qdot->template head<4>() = CalcQMatrixOverTwo(q_FM) * v.template head<3>();
   // Translational component, ṗ_FoMo_F = v_FMo_F:
   qdot->template tail<3>() = v.template tail<3>();
 }
@@ -479,16 +448,15 @@ void QuaternionFloatingMobilizer<T>::DoMapAccelerationToQDDot(
     const Eigen::Ref<const VectorX<T>>& vdot,
     EigenPtr<VectorX<T>> qddot) const {
   // This function maps vdot to qddot by calculating q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.
-  // It first calculates the rotational part, then the translational part.
   //
   // For the rotational part of this mobilizer, the 2nd-derivatives of the
   // generalized positions q̈_FM = [q̈w, q̈x, q̈y, q̈z]ᵀ are related to the
   // 1st-derivatives of generalized velocities ω̇ = ẇ_FM_F = [ω̇x, ω̇y, ω̇z]ᵀ as
-  // shown below where L(q_FM) is the 4x3 matrix documented in CalcLMatrix(),
-  // ω = [ωx, ωy, ωz]ᵀ, ω² = |ω|², and 0.5 L̇(q̇_FM)⋅ω = -0.25 ω² q_FM.
+  // shown below where Q_FM is the 4x3 matrix documented in CalcQMatrix(),
+  // ω = [ωx, ωy, ωz]ᵀ, ω² = |ω|², and 0.5 Q̇_FM⋅ω = -0.25 ω² q_FM.
   //
-  // q̈_FM = 0.5 L(q_FM)⋅ω̇ + 0.5 L̇(q̇_FM)⋅ω
-  //      = 0.5 L(q_FM)⋅ω̇ - 0.25 ω² q_FM
+  // q̈_FM = 0.5 Q_FM⋅ω̇ + 0.5 Q̇_FM⋅ω
+  //      = 0.5 Q_FM⋅ω̇ - 0.25 ω² q_FM
   //
   // Formulas and proofs in Sections 9.3 and 9.6 of [Mitiguy, August 2025].
   // [Mitiguy, August 2025] Mitiguy, P. Advanced Dynamics & Motion Simulation.
@@ -497,12 +465,12 @@ 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>() =
-      CalcLMatrixOverTwo(q_FM) * vdot.template head<3>() -
+      CalcQMatrixOverTwo(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]ᵀ
-  // in terms of 1st-derivative of the velocity v_FoM_F = [vx, vy, vz]ᵀ as
-  // [ẍ, ÿ, z̈]ᵀ = [v̇x, v̇y, v̇z]ᵀ.
+  // For the translational part of this mobilizer, the 2nd-derivative of the
+  // position vector p_FoMo_F = [x, y, z]ᵀ is related to the 1st-derivative of
+  // the velocity v_FoM_F = [vx, vy, vz]ᵀ as [ẍ, ÿ, z̈]ᵀ = [v̇x, v̇y, v̇z]ᵀ.
   qddot->template tail<3>() = vdot.template tail<3>();
 }
 

multibody/tree/quaternion_floating_mobilizer.h

@@ -331,49 +331,58 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
       const MultibodyTree<symbolic::Expression>& tree_clone) const final;
 
  private:
-  // Returns the 4x3 matrix L(q_FM) where q_FM = [qw, qx, qy, qz]ᵀ is the
-  // quaternion relating frames F and M.
-  //
-  //           ⌈ -qx   -qy   -qz ⌉
-  // L(q_FM) = |  qw    qz   -qy |
+  // Forms a 4x3 matrix whose elements depend linearly on the 4 elements of
+  // the quaternion q = [qw, qx, qy, qz] as shown below.
+  // @param[in] q a generic quaternion which is not necessarily a unit
+  // quaternion or a quaternion associated with a rotation matrix.
+  // @returns  ⌈ -qx   -qy   -qz ⌉
+  //           |  qw    qz   -qy |
   //           | -qz    qw    qx |
   //           ⌊  qy   -qx    qw ⌋
   //
-  // Note: 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 rotational part of the N
-  // and N⁺ matrices are 0.5 * L(q_FM) and 2 * L(q_FM)ᵀ, respectively. Others:
+  // @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).
+  // Many uses of Q_FM are associated with an angular velocity expressed in
+  // a particular frame. The examples below show Q_FM used in conjunction
+  // with w_FM_F (frame M's angular velocity in frame F, expressed in F).
+  //
+  // 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
   //
-  // 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
+  // @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).
   //
-  // Note: Since L(q_FM) depends linearly on q_FM, s * L(q_FM) = L(s * q_FM),
-  // where s is a scalar (e.g., 0.5 or 2).
-  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.  See documentation in CalcLMatrix(),
-  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()});
+  // Formulas, uses, and proofs are in Sections 9.3 and 9.6 of [Mitiguy].
+  // [Mitiguy, August 2025] Mitiguy, P. Advanced Dynamics & Motion Simulation.
+  // Textbook available at www.MotionGenesis.com
+  static Eigen::Matrix<T, 4, 3> CalcQMatrix(const Quaternion<T>& q);
+
+  // Efficiently calculates the 4x3 matrix 0.5 * CalcQMatrix(q).
+  // @param[in] q a generic quaternion which is not necessarily a unit
+  // quaternion or a quaternion associated with a rotation matrix.
+  // @see QuaternionFloatingMobilizer::CalcQMatrix().
+  static Eigen::Matrix<T, 4, 3> CalcQMatrixOverTwo(const Quaternion<T>& q) {
+    return CalcQMatrix({0.5 * q.w(), 0.5 * q.x(), 0.5 * q.y(), 0.5 * q.z()});
   }
 
-  // Returns the 3x4 matrix 2 * L(q_FM)ᵀ, where q_FM = [qw, qx, qy, qz] is the
-  // quaternion relating frames F and M. See documentation in CalcLMatrix().
-  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()})
+  // Efficiently calculates the 3x4 matrix [2 * CalcQMatrix(q)]ᵀ.
+  // @param[in] q a generic quaternion which is not necessarily a unit
+  // quaternion or a quaternion associated with a rotation matrix.
+  // @see QuaternionFloatingMobilizer::CalcQMatrix().
+  static Eigen::Matrix<T, 3, 4> CalcTwoTimesQMatrixTranspose(
+      const Quaternion<T>& q) {
+    return CalcQMatrix({2 * q.w(), 2 * q.x(), 2 * q.y(), 2 * q.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.
+  // to angular velocity for which w_FM_F = N⁺(q)⋅q̇_FM.
   // This method can take a non unity quaternion q_tilde such that
-  // w_WB = N⁺(q_tilde)⋅q̇_tilde_WB also holds true.
-  // With L given by CalcLMatrix we have:
-  // N⁺(q) = L(2 q_FM)ᵀ
+  // w_FM_F = N⁺(q_tilde)⋅q̇_tilde_FM also holds true.
+  // @returns N⁺(q_tilde)
   static Eigen::Matrix<T, 3, 4> QuaternionRateToAngularVelocityMatrix(
       const Quaternion<T>& q);