Add CalcNplusDotMatrix().

Author: Paul Mitiguy

multibody/tree/mobilizer.cc

@@ -52,6 +52,17 @@ void Mobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>&,
   throw std::logic_error(error_message);
 }
 
+template <typename T>
+void Mobilizer<T>::DoCalcNplusDotMatrix(const systems::Context<T>&,
+                                        EigenPtr<MatrixX<T>>) const {
+  // TODO(Mitiguy) remove this function when Mobilizer::CalcNplusDotMatrix()
+  //  is changed to a pure virtual function that requires override.
+  const std::string error_message = fmt::format(
+      "The function {}() has not been implemented for this mobilizer.",
+      __func__);
+  throw std::logic_error(error_message);
+}
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake

multibody/tree/mobilizer.h

@@ -599,12 +599,12 @@ class Mobilizer : public MultibodyElement<T> {
     DoCalcNplusMatrix(context, Nplus);
   }
 
-  // Computes the kinematic matrix `Ṅ(q,q̇)` that helps relate q̈ (2ⁿᵈ time
-  // derivative of the generalized positions) to v̇ (1ˢᵗ time derivative of
-  // generalized velocities) according to `q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇`, where N(q)
-  // can be calculated by CalcNMatrix().
+  // Computes the matrix `Ṅ(q,q̇)` that helps relate q̈ (2ⁿᵈ time derivative of
+  // the generalized positions) to v̇ (1ˢᵗ time derivative of generalized
+  // velocities) according to `q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇`, where N(q) can be
+  // calculated by CalcNMatrix().
   // @param[in] context stores generalized positions q and velocities v.
-  // @param[out] Ndot The kinematic matrix Ṅ(q,q̇). On input Ndot must have size
+  // @param[out] Ndot The matrix Ṅ(q,q̇). On input Ndot must have size
   //   `nq x nv` where nq is the number of generalized positions and
   //   nv is the number of generalized velocities for this mobilizer.
   // @see MapAccelerationToQDDot().
@@ -616,6 +616,23 @@ class Mobilizer : public MultibodyElement<T> {
     DoCalcNDotMatrix(context, Ndot);
   }
 
+  // Computes the matrix `Ṅ⁺(q,q̇)` that helps relate v̇ (1ˢᵗ time derivative of
+  // generalized velocities) to q̈ (2ⁿᵈ time derivative of the generalized
+  // positions) according to `v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈`, where N⁺(q) can be
+  // calculated by CalcNPlusMatrix().
+  // @param[in] context stores generalized positions q and velocities v.
+  // @param[out] NplusDot The matrix Ṅ(q,q̇). On input NplusDot must have size
+  //   `nq x nv` where nq is the number of generalized positions and
+  //   nv is the number of generalized velocities for this mobilizer.
+  // @see MapQDDotToAcceleration().
+  void CalcNplusDotMatrix(const systems::Context<T>& context,
+                          EigenPtr<MatrixX<T>> NplusDot) const {
+    DRAKE_DEMAND(NplusDot != nullptr);
+    DRAKE_DEMAND(NplusDot->rows() == num_velocities());
+    DRAKE_DEMAND(NplusDot->cols() == num_positions());
+    DoCalcNplusDotMatrix(context, NplusDot);
+  }
+
   virtual bool is_velocity_equal_to_qdot() const = 0;
 
   // Computes the kinematic mapping `q̇ = N(q)⋅v` between generalized
@@ -782,6 +799,13 @@ class Mobilizer : public MultibodyElement<T> {
   virtual void DoCalcNDotMatrix(const systems::Context<T>& context,
                                 EigenPtr<MatrixX<T>> Ndot) const;
 
+  // NVI to CalcNplusDotMatrix(). Implementations can safely assume that
+  // NplusDot is not the nullptr and that NplusDot has the proper size.
+  // TODO(Mitiguy) change this function to a pure virtual function when it has
+  //  been overridden in all subclasses.
+  virtual void DoCalcNplusDotMatrix(const systems::Context<T>& context,
+                                    EigenPtr<MatrixX<T>> NplusDot) const;
+
   // @name Methods to make a clone templated on different scalar types.
   //
   // The only const argument to these methods is the new MultibodyTree clone

multibody/tree/rpy_ball_mobilizer.cc

@@ -277,20 +277,17 @@ void RpyBallMobilizer<T>::DoCalcNplusMatrix(const systems::Context<T>& context,
 template <typename T>
 void RpyBallMobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>& context,
                                            EigenPtr<MatrixX<T>> Ndot) const {
-  // Computes the 3x3 matrix Ṅ(q,q̇) that helps relate q̈ (2ⁿᵈ time derivative
-  // of the generalized positions) to v̇ (1ˢᵗ time derivative of generalized
-  // velocities) according to q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇, where q = [r, p, y]ᵀ
-  // contains the roll (r), pitch (p) and yaw (y) angles and v = [wx, wy, wz]ᵀ
-  // represents W_FM_F (the angular velocity of the mobilizer's M frame measured
-  // in its F frame, expressed in the F frame).
+  // Computes the 3x3 matrix Ṅ(q,q̇) that helps relate q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇,
+  // where q = [r, p, y]ᵀ contains the roll (r), pitch (p) and yaw (y) angles
+  // and v = [wx, wy, wz]ᵀ represents W_FM_F (the angular velocity of the
+  // mobilizer's M frame measured in its F frame, expressed in the F frame).
   //
   // The 3x3 matrix N(q) relates q̇ to v as q̇ = N(q)⋅v, where
   //
   //        [          cos(y) / cos(p),           sin(y) / cos(p),  0]
   // N(q) = [                  -sin(y),                    cos(y),  0]
   //        [ sin(p) * cos(y) / cos(p),  sin(p) * sin(y) / cos(p),  1]
   //
-  using std::abs;
   using std::cos;
   using std::sin;
   const Vector3<T> angles = get_angles(context);
@@ -325,6 +322,50 @@ void RpyBallMobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>& context,
   Ndot->coeffRef(2, 2) = 0;
 }
 
+template <typename T>
+void RpyBallMobilizer<T>::DoCalcNplusDotMatrix(
+    const systems::Context<T>& context, EigenPtr<MatrixX<T>> NplusDot) const {
+  // Computes the matrix `Ṅ⁺(q,q̇)` that helps relate v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈,
+  // where q = [r, p, y]ᵀ contains the roll (r), pitch (p) and yaw (y) angles
+  // and v = [wx, wy, wz]ᵀ represents W_FM_F (the angular velocity of the
+  // mobilizer's M frame measured in its F frame, expressed in the F frame).
+  //
+  // The 3x3 matrix N⁺(q) relates v to q̇ as v = N⁺(q)⋅q̇, where
+  //
+  //         [ cos(y) * cos(p),  -sin(y),  0]
+  // N⁺(q) = [ sin(y) * cos(p),   cos(y),  0]
+  //         [         -sin(p),        0,  1]
+  //
+  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]);
+
+  // Calculate time-derivative of roll, pitch, and yaw angles.
+  const Vector3<T> v = get_angular_velocity(context);
+  Vector3<T> qdot;
+  MapVelocityToQDot(context, v, &qdot);
+  const T& pdot = qdot(1);
+  const T& ydot = qdot(2);
+
+  const T sp_pdot = sp * pdot;
+  const T cy_ydot = cy * ydot;
+  const T sy_ydot = sy * ydot;
+
+  NplusDot->coeffRef(0, 0) = -sy_ydot * cp - cy * sp_pdot;
+  NplusDot->coeffRef(0, 1) = -cy_ydot;
+  NplusDot->coeffRef(0, 2) = 0;
+  NplusDot->coeffRef(1, 0) = cy_ydot * cp - sy * sp_pdot;
+  NplusDot->coeffRef(1, 1) = -sy * ydot;
+  NplusDot->coeffRef(1, 2) = 0;
+  NplusDot->coeffRef(2, 0) = -cp * pdot;
+  NplusDot->coeffRef(2, 1) = 0;
+  NplusDot->coeffRef(2, 2) = 0;
+}
+
 template <typename T>
 void RpyBallMobilizer<T>::MapVelocityToQDot(
     const systems::Context<T>& context, const Eigen::Ref<const VectorX<T>>& v,

multibody/tree/rpy_ball_mobilizer.h

@@ -324,6 +324,12 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
   void DoCalcNDotMatrix(const systems::Context<T>& context,
                         EigenPtr<MatrixX<T>> Ndot) const final;
 
+  // Calculates the time derivative of the N⁺ matrix that relates v = N⁺(q)⋅q̇.
+  // There is no singularity in N⁺(q) or its time-derivative Ṅ⁺(q,q̇) and
+  // calculating Ṅ⁺ is more efficient than calculating Ṅ.
+  void DoCalcNplusDotMatrix(const systems::Context<T>& context,
+                            EigenPtr<MatrixX<T>> NplusDot) const final;
+
   std::unique_ptr<Mobilizer<double>> DoCloneToScalar(
       const MultibodyTree<double>& tree_clone) const override;
 

multibody/tree/test/rpy_ball_mobilizer_test.cc

@@ -173,8 +173,8 @@ TEST_F(RpyBallMobilizerTest, NDotAndMapAccelerationToQDDot) {
   // Verify equivalence of q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇ and MapAccelerationToQDDot().
   // PAUL FIX THIS TEST -- NOT WORKING.  SHOULD BE EXPECT_TRUE(...);
   const Vector3<double> qddot_expected = Ndot * wxyz + N * wdot;
-  EXPECT_FALSE(CompareMatrices(qddot, qddot_expected, kTolerance,
-                               MatrixCompareType::relative));
+  EXPECT_TRUE(CompareMatrices(qddot, qddot_expected, kTolerance,
+                              MatrixCompareType::relative));
 }
 
 TEST_F(RpyBallMobilizerTest, MapUsesNplus) {
@@ -183,7 +183,7 @@ TEST_F(RpyBallMobilizerTest, MapUsesNplus) {
   mobilizer_->SetAngles(context_.get(), rpy_value);
 
   // Set arbitrary qdot and call MapQDotToVelocity().
-  const Vector3<double> qdot = (Vector3<double>() << 1, 2, 3).finished();
+  const Vector3<double> qdot(1, 2, 3);
   Vector3<double> v;
   mobilizer_->MapQDotToVelocity(*context_, qdot, &v);
 
@@ -195,14 +195,19 @@ TEST_F(RpyBallMobilizerTest, MapUsesNplus) {
   EXPECT_TRUE(CompareMatrices(v, Nplus * qdot, kTolerance,
                               MatrixCompareType::relative));
 
+  // Set this mobilizer's angular velocity to be consistent with v.
+  mobilizer_->SetAngularVelocity(context_.get(), v);
+
   // Ensure MapQDDotToAcceleration() works properly.
   const Vector3<double> qddot(1.2, 2.3, 3.4);  // Set arbitrary values.
   Vector3<double> vdot;
-  mobilizer_->MapQDDotToAcceleration(*context_, qdot, &v);
+  mobilizer_->MapQDDotToAcceleration(*context_, qddot, &vdot);
 
-  // Calculate vdot another way.
-  // TODO(Mitiguy) Finish -- as of now this is a dumb test.
-  Vector3<double> vdot_expected = Nplus * qddot;  // NOT TRUE YET.
+  // Calculate vdot another way as v̇ = N̈⁺(q,q̇)⋅q̇ +  N⁺(q)⋅q̈.
+  // TODO(Mitiguy) Finish -- as of now, this test is not working.
+  MatrixX<double> NplusDot(3, 3);
+  mobilizer_->CalcNplusDotMatrix(*context_, &NplusDot);
+  const Vector3<double> vdot_expected = NplusDot * qdot + Nplus * qddot;
   EXPECT_FALSE(CompareMatrices(vdot, vdot_expected, kTolerance,
                                MatrixCompareType::relative));
 }