MapQDDotToAcceleration() and MapAccelerationToQDDot().

Author: Paul Mitiguy

multibody/tree/quaternion_floating_mobilizer.cc

@@ -321,16 +321,6 @@ Eigen::Matrix<T, 4, 3> QuaternionFloatingMobilizer<T>::CalcLMatrix(
       .finished();
 }
 
-template <typename T>
-Eigen::Matrix<T, 4, 3>
-QuaternionFloatingMobilizer<T>::AngularVelocityToQuaternionRateMatrix(
-    const Quaternion<T>& q_FM) {
-  // With L given by CalcLMatrix we have:
-  // N(q) = L(q_FM/2)
-  return CalcLMatrix(
-      {q_FM.w() / 2.0, q_FM.x() / 2.0, q_FM.y() / 2.0, q_FM.z() / 2.0});
-}
-
 template <typename T>
 Eigen::Matrix<T, 3, 4>
 QuaternionFloatingMobilizer<T>::QuaternionRateToAngularVelocityMatrix(
@@ -485,6 +475,31 @@ void QuaternionFloatingMobilizer<T>::DoMapQDotToVelocity(
   v->template tail<3>() = qdot.template tail<3>();
 }
 
+template <typename T>
+void QuaternionFloatingMobilizer<T>::DoMapAccelerationToQDDot(
+    const systems::Context<T>& context,
+    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 parts, then the translational part.
+  //
+  // Calculate the 2nd-derivative of the quaternion q_FM = [qw, qx, qy, qz]ᵀ.
+  // q̈_FM = Nᵣ(q) [ẇx, ẇy, ẇz]ᵀ - 0.25 ω² q_FM, where ω² = |w_FM|².
+  // Formulas and proofs in Section 9.3 of [Mitiguy, August 2019].
+  // [Mitiguy, August 2019] Mitiguy, P. Advanced Dynamics & Motion Simulation.
+  //                        Available at www.MotionGenesis.com
+  const Quaternion<T> q_FM = get_quaternion(context);
+  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>() -
+      w_squared_over_four * Vector4<T>(q_FM.w(), q_FM.x(), q_FM.y(), q_FM.z());
+
+  // Calculate the 2nd-derivtive of the position vector p_FoMo_F = [x, y, z]ᵀ.
+  // [ẍ, ÿ, z̈]ᵀ = [v̇x, v̇y, v̇z]ᵀ.
+  qddot->template tail<3>() = vdot.template tail<3>();
+}
+
 template <typename T>
 std::pair<Eigen::Quaternion<T>, Vector3<T>>
 QuaternionFloatingMobilizer<T>::GetPosePair(

multibody/tree/quaternion_floating_mobilizer.h

@@ -313,6 +313,16 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
                            const Eigen::Ref<const VectorX<T>>& qdot,
                            EigenPtr<VectorX<T>> v) const final;
 
+  void DoMapAccelerationToQDDot(const systems::Context<T>& context,
+                                const Eigen::Ref<const VectorX<T>>& vdot,
+                                EigenPtr<VectorX<T>> qddot) const final;
+#if 0
+  // Maps qddot to vdot by calculating v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  void DoMapQDDotToAcceleration(const systems::Context<T>& context,
+                                const Eigen::Ref<const VectorX<T>>& qddot,
+                                EigenPtr<VectorX<T>> vdot) const final;
+#endif
+
   std::unique_ptr<Mobilizer<double>> DoCloneToScalar(
       const MultibodyTree<double>& tree_clone) const final;
 
@@ -325,12 +335,23 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
  private:
   // Helper to compute the kinematic map N(q). L ∈ ℝ⁴ˣ³.
   static Eigen::Matrix<T, 4, 3> CalcLMatrix(const Quaternion<T>& q);
-  // Helper to compute the kinematic map N(q) from angular velocity to
-  // quaternion time derivative for which q̇_WB = N(q)⋅w_WB.
-  // With L given by CalcLMatrix we have:
-  // N(q) = L(q_FM/2)
+
+  // 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).
+  //
+  // ⌈ 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 ⌋
+  //
+  // 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);
+      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()});
+  }
 
   // Helper to compute the kinematic map N⁺(q) from quaternion time derivative
   // to angular velocity for which w_WB = N⁺(q)⋅q̇_WB.