@@ -304,33 +304,55 @@ class BodyNode : public MultibodyElement<T> {
304
304
// This node must fill in its nv x nv diagonal block in M, and then
305
305
// sweep down to World filling in its diagonal contributions as in the
306
306
// inner loop of algorithm 9.3, using the appropriate
307
- // CalcMassMatrixOffDiagonalHelper ().
308
- virtual void CalcMassMatrixContribution_TipToBase (
307
+ // CalcMassMatrixOffDiagonalInWorldHelper ().
308
+ virtual void CalcMassMatrixContributionInWorld_TipToBase (
309
309
const PositionKinematicsCache<T>& pc,
310
- const std::vector<SpatialInertia<T>>& Mc_B_W_cache ,
310
+ const std::vector<SpatialInertia<T>>& I_BBo_W_cache ,
311
311
const std::vector<Vector6<T>>& H_PB_W_cache,
312
312
EigenPtr<MatrixX<T>> M) const = 0;
313
313
314
+ // Same, but calculated in body M frames rather than World.
315
+ virtual void CalcMassMatrixContributionInM_TipToBase (
316
+ const T* positions, const PositionKinematicsCacheInM<T>& pcm,
317
+ const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
318
+ EigenPtr<MatrixX<T>> M) const = 0;
319
+
314
320
// There are six functions for calculating the off-diagonal blocks, one for
315
- // each possible size Rnv of body R (k)'s inboard mobilizer (welds don't
321
+ // each possible size Bnv of body B (k)'s inboard mobilizer (welds don't
316
322
// contribute here). This allows us to use fixed-size 2d matrices in the
317
323
// implementation as we sweep the inboard bodies. Use the separate
318
- // dispatcher class CalcMassMatrixOffDiagonalDispatcher (defined below)
319
- // to generate the properly sized call for body R (k).
320
- #define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK ( Rnv ) \
321
- virtual void CalcMassMatrixOffDiagonalBlock##Rnv( \
322
- int R_start_in_v , const std::vector<Vector6<T>>& H_PB_W_cache, \
323
- const Eigen::Matrix<T, 6 , Rnv >& Fm_CBo_W, EigenPtr<MatrixX<T>> M) \
324
+ // dispatcher class CalcMassMatrixOffDiagonalInWorldDispatcher (defined below)
325
+ // to generate the properly sized call for body B (k).
326
+ #define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD ( Bnv ) \
327
+ virtual void CalcMassMatrixOffDiagonalBlockInWorld##Bnv( \
328
+ int B_start_in_v , const std::vector<Vector6<T>>& H_PB_W_cache, \
329
+ const Eigen::Matrix<T, 6 , Bnv >& Fm_CBo_W, EigenPtr<MatrixX<T>> M) \
324
330
const = 0
325
331
326
- DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (1 );
327
- DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (2 );
328
- DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (3 );
329
- DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (4 );
330
- DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (5 );
331
- DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (6 );
332
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD (1 );
333
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD (2 );
334
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD (3 );
335
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD (4 );
336
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD (5 );
337
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD (6 );
338
+
339
+ #undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD
332
340
333
- #undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK
341
+ // Repeat for the M-frame algorithm.
342
+ #define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (Bnv ) \
343
+ virtual void CalcMassMatrixOffDiagonalBlockInM##Bnv( \
344
+ const T* positions, const PositionKinematicsCacheInM<T>& pcm, \
345
+ int B_start_in_v, const Eigen::Matrix<T, 6 , Bnv>& Fm_CMp_Mp, \
346
+ EigenPtr<MatrixX<T>> M) const = 0
347
+
348
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (1 );
349
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (2 );
350
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (3 );
351
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (4 );
352
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (5 );
353
+ DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (6 );
354
+
355
+ #undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M
334
356
335
357
// This method is used by MultibodyTree within a base-to-tip loop to compute
336
358
// this node's kinematics that depend on the generalized accelerations, i.e.
@@ -723,32 +745,60 @@ class BodyNode : public MultibodyElement<T> {
723
745
const Mobilizer<T>* mobilizer_{nullptr };
724
746
};
725
747
726
- // During mass matrix computation, this dispatcher is invoked by the
727
- // composite body R(k) on each of the bodies on the path to World.
728
- template <typename T, int Rnv>
729
- class CalcMassMatrixOffDiagonalDispatcher ;
730
-
731
- #define SPECIALIZE_MASS_MATRIX_DISPATCHER (Rnv ) \
732
- template <typename T> \
733
- class CalcMassMatrixOffDiagonalDispatcher <T, Rnv> { \
734
- public: \
735
- static void Dispatch (const BodyNode<T>& body_node, int R_start_in_v, \
736
- const std::vector<Vector6<T>>& H_PB_W_cache, \
737
- const Eigen::Matrix<T, 6 , Rnv>& Fm_CBo_W, \
738
- EigenPtr<MatrixX<T>> M) { \
739
- body_node.CalcMassMatrixOffDiagonalBlock ##Rnv ( \
740
- R_start_in_v, H_PB_W_cache, Fm_CBo_W, M); \
741
- } \
748
+ // During CalcMassMatrix() (in World), this dispatcher is invoked by the
749
+ // composite body B, whose inboard mobilizer has Bnv dofs, on each of the bodies
750
+ // ("parent nodes") on its inboard path to World.
751
+ template <typename T, int Bnv>
752
+ class CalcMassMatrixOffDiagonalInWorldDispatcher ;
753
+
754
+ #define SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (Bnv ) \
755
+ template <typename T> \
756
+ class CalcMassMatrixOffDiagonalInWorldDispatcher <T, Bnv> { \
757
+ public: \
758
+ static void Dispatch (const BodyNode<T>& parent_node, int B_start_in_v, \
759
+ const std::vector<Vector6<T>>& H_PB_W_cache, \
760
+ const Eigen::Matrix<T, 6 , Bnv>& Fm_CPo_W, \
761
+ EigenPtr<MatrixX<T>> M) { \
762
+ parent_node.CalcMassMatrixOffDiagonalBlockInWorld ##Bnv ( \
763
+ B_start_in_v, H_PB_W_cache, Fm_CPo_W, M); \
764
+ } \
765
+ }
766
+
767
+ SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (1 );
768
+ SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (2 );
769
+ SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (3 );
770
+ SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (4 );
771
+ SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (5 );
772
+ SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER (6 );
773
+
774
+ #undef SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER
775
+
776
+ // Same as above except for CalcMassMatrixInM().
777
+ template <typename T, int Bnv>
778
+ class CalcMassMatrixOffDiagonalInMDispatcher ;
779
+
780
+ #define SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (Bnv ) \
781
+ template <typename T> \
782
+ class CalcMassMatrixOffDiagonalInMDispatcher <T, Bnv> { \
783
+ public: \
784
+ static void Dispatch (const BodyNode<T>& parent_node, const T* positions, \
785
+ const PositionKinematicsCacheInM<T>& pcm, \
786
+ int B_start_in_v, \
787
+ const Eigen::Matrix<T, 6 , Bnv>& Fm_CMp_Mp, \
788
+ EigenPtr<MatrixX<T>> M) { \
789
+ parent_node.CalcMassMatrixOffDiagonalBlockInM ##Bnv ( \
790
+ positions, pcm, B_start_in_v, Fm_CMp_Mp, M); \
791
+ } \
742
792
}
743
793
744
- SPECIALIZE_MASS_MATRIX_DISPATCHER (1 );
745
- SPECIALIZE_MASS_MATRIX_DISPATCHER (2 );
746
- SPECIALIZE_MASS_MATRIX_DISPATCHER (3 );
747
- SPECIALIZE_MASS_MATRIX_DISPATCHER (4 );
748
- SPECIALIZE_MASS_MATRIX_DISPATCHER (5 );
749
- SPECIALIZE_MASS_MATRIX_DISPATCHER (6 );
794
+ SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (1 );
795
+ SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (2 );
796
+ SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (3 );
797
+ SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (4 );
798
+ SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (5 );
799
+ SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER (6 );
750
800
751
- #undef SPECIALIZE_MASS_MATRIX_DISPATCHER
801
+ #undef SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER
752
802
753
803
} // namespace internal
754
804
} // namespace multibody
0 commit comments