@@ -24,110 +24,108 @@ namespace multibody {
24
24
namespace internal {
25
25
26
26
template <typename T, class ConcreteMobilizer >
27
- void BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixContribution_TipToBase(
28
- const PositionKinematicsCache<T>& pc,
29
- const std::vector<SpatialInertia<T>>& Mc_B_W_cache,
30
- const std::vector<Vector6<T>>& H_PB_W_cache, EigenPtr<MatrixX<T>> M) const {
27
+ void BodyNodeImpl<T, ConcreteMobilizer>::
28
+ CalcMassMatrixContributionInWorld_TipToBase (
29
+ const PositionKinematicsCache<T>& pc,
30
+ const std::vector<SpatialInertia<T>>& I_BBo_W_cache,
31
+ const std::vector<Vector6<T>>& H_PB_W_cache,
32
+ EigenPtr<MatrixX<T>> M) const {
31
33
// Welds only contribute to composite mass properties. Their effect on the
32
34
// mass matrix gets included when we get to the composite's non-weld inboard
33
35
// mobilizer.
34
36
if constexpr (kNv != 0 ) {
35
37
// This node's 6x6 composite body inertia.
36
- const SpatialInertia<T>& Mc_C_W = Mc_B_W_cache [mobod_index ()];
38
+ const SpatialInertia<T>& I_BBo_W = I_BBo_W_cache [mobod_index ()];
37
39
38
40
// Across-mobilizer 6 x kNv hinge matrix, from C's parent Cp to C.
39
- const auto H_CpC_W = get_H (H_PB_W_cache); // 6 x kNv fixed size Map.
41
+ const auto H_PB_W = get_H (H_PB_W_cache); // 6 x kNv fixed size Map.
40
42
41
43
// The composite body algorithm considers the system at rest, when
42
- // generalized velocities are zero.
44
+ // generalized velocities are zero. Here m=kNv, this body's mobilizer's
45
+ // number of dofs.
43
46
// Now if we consider this node's generalized accelerations as the matrix
44
- // vm_dot = Iₘ, the identity matrix in ℝᵐˣᵐ, the spatial acceleration A_WC
47
+ // vm_dot = Iₘ, the identity matrix in ℝᵐˣᵐ, the spatial acceleration A_WB
45
48
// is in ℝ⁶ˣᵐ. That is, we are considering each case in which all
46
49
// generalized accelerations are zero but the m-th generalized
47
50
// acceleration for this node equals one.
48
51
// This node's spatial acceleration can be written as:
49
- // A_WC = Φᵀ(p_CpC ) * A_WCp + Ac_WC + Ab_CpC_W + H_CpC_W * vm_dot
50
- // where A_WCp is the spatial acceleration of the parent node's body Cp ,
51
- // Ac_WC include the centrifugal and Coriolis terms, and Ab_CpC_W is the
52
- // spatial acceleration bias of the hinge Jacobian matrix H_CpC_W .
52
+ // A_WB = Φᵀ(p_PB ) * A_WP + A_WB + Ab_PB_W + H_PB_W * vm_dot
53
+ // where A_WP is the spatial acceleration of the parent node's body P ,
54
+ // A_WB includes the centrifugal and Coriolis terms, and Ab_PC_W is the
55
+ // spatial acceleration bias of the hinge Jacobian matrix H_PB_W .
53
56
// Now, since all generalized accelerations but vm_dot are zero, then
54
- // A_WCp is zero. Since the system is at rest, Ac_WC and Ab_CpC_W are
55
- // zero.
56
- // Therefore, for vm_dot = Iₘ, we have that A_WC = H_CpC_W.
57
- const auto & A_WC = H_CpC_W; // 6 x kNv, fixed-size Map.
57
+ // A_WP is zero. Since the system is at rest, A_WB and Ab_PB_W are also
58
+ // zero. Therefore, for vm_dot = Iₘ, we have that A_WB = H_PB_W.
59
+ const auto & A_WB = H_PB_W; // 6 x kNv, fixed-size Map.
58
60
59
61
// If we consider the closed system composed of the composite body held by
60
62
// its mobilizer, the Newton-Euler equations state:
61
- // Fm_CCo_W = Mc_C_W * A_WC + Fb_C_W
62
- // where Fm_CCo_W is the spatial force at this node's mobilizer.
63
- // Since the system is at rest, we have Fb_C_W = 0 and thus:
64
- // Done as a general matrix multiply, this is 66 * kNv flops.
65
- // TODO(sherm1) do better!
66
- const Eigen::Matrix<T, 6 , kNv > Fm_CCo_W = Mc_C_W * A_WC;
63
+ // Fm_BBo_W = I_BBo_W * A_WB + Fb_BBo_W
64
+ // where Fm_BBo_W is the spatial force at this node's mobilizer.
65
+ // Since the system is at rest, we have Fb_BBo_W = 0 and thus:
66
+ const Eigen::Matrix<T, 6 , kNv > Fm_BBo_W = I_BBo_W * A_WB; // expensive
67
67
68
- const int composite_start_in_v = mobilizer ().velocity_start_in_v ();
68
+ const int B_start_in_v = mobilizer ().velocity_start_in_v ();
69
69
70
70
// Diagonal block corresponding to current node (mobod_index).
71
- M->template block <kNv , kNv >(composite_start_in_v, composite_start_in_v ) =
72
- H_CpC_W .transpose () * Fm_CCo_W ; // kNv * kNv * 11 flops
71
+ M->template block <kNv , kNv >(B_start_in_v, B_start_in_v ) =
72
+ H_PB_W .transpose () * Fm_BBo_W ; // kNv * kNv * 11 flops
73
73
74
- // We recurse the tree inwards from C all the way to the root. We define
74
+ // We recurse the tree inwards from B all the way to the root. We define
75
75
// the frames:
76
- // - B: the frame for the current node, body_node.
77
- // - Bc: B's child node frame, child_node.
78
- // - P: B's parent node frame.
79
- const BodyNode<T>* child_node = this ; // Child starts at frame C.
80
- const BodyNode<T>* body_node = this -> parent_body_node () ; // Inboard body .
81
- Eigen::Matrix<T, 6 , kNv > Fm_CBo_W = Fm_CCo_W;
82
-
83
- while (body_node ->mobod_index () != world_mobod_index ()) {
84
- const Vector3<T>& p_BoBc_W = pc. get_p_PoBo_W (child_node-> mobod_index ());
85
- // In place rigid shift of the spatial force in each column of
86
- // Fm_CBo_W, from Bc to Bo . Before this computation, Fm_CBo_W actually
87
- // stores Fm_CBc_W from the previous recursion. At the end of this
88
- // computation, Fm_CBo_W stores the spatial force on composite body C,
89
- // shifted to Bo , and expressed in the world W. That is, we are doing
90
- // Fm_CBo_W = Fm_CBc_W .Shift(p_BcB_W ).
91
-
92
- // This is SpatialForce<T>::ShiftInPlace(&Fm_CBo_W , -p_BoBc_W ) but
76
+ // - C: child composite body, initially B
77
+ // - P: parent composite body, initially B's inboard body
78
+ const BodyNode<T>* child_node = this ;
79
+ const BodyNode<T>* parent_node = this -> parent_body_node ();
80
+ Eigen::Matrix<T, 6 , kNv > Fm_CPo_W = Fm_BBo_W ; // Initially Fm_CCo_W .
81
+
82
+ while (parent_node-> mobod_index () != world_mobod_index ()) {
83
+ const Vector3<T>& p_PC_W = pc. get_p_PoBo_W (child_node ->mobod_index ());
84
+
85
+ // In place rigid shift of the spatial force in each column of Fm_CCo_W,
86
+ // from Co to Po . Before this computation, Fm_CPo_W actually stores
87
+ // Fm_CCo_W from the previous recursion. At the end of this computation,
88
+ // Fm_CPo_W stores the spatial force on child composite body C, shifted to
89
+ // parent origin Po , and expressed in the world W. That is, we are doing
90
+ // Fm_CPo_W = Fm_CCo_W .Shift(p_CoPo_W ).
91
+
92
+ // This is SpatialForce<T>::ShiftInPlace(&Fm_CCo_W , -p_PoCo_W ) but
93
93
// done with fixed sizes (no loop if kNv==1). 12 * kNv flops
94
- // TODO(sherm1) Consider moving this to a templatized ShiftInPlace
95
- // API in SpatialForce (and other spatial vector classes?).
96
94
for (int col = 0 ; col < kNv ; ++col) {
97
95
// Ugly Eigen intermediate types; don't look!
98
- auto torque = Fm_CBo_W .template block <3 , 1 >(0 , col);
99
- const auto force = Fm_CBo_W .template block <3 , 1 >(3 , col);
100
- torque += p_BoBc_W .cross (force); // + because we're negating p_BoBc
96
+ auto torque = Fm_CPo_W .template block <3 , 1 >(0 , col);
97
+ const auto force = Fm_CPo_W .template block <3 , 1 >(3 , col);
98
+ torque += p_PC_W .cross (force); // + because we're negating p_PC
101
99
}
102
100
103
101
CalcMassMatrixOffDiagonalDispatcher<T, kNv >::Dispatch (
104
- *body_node, composite_start_in_v , H_PB_W_cache, Fm_CBo_W , M);
102
+ *parent_node, B_start_in_v , H_PB_W_cache, Fm_CPo_W , M);
105
103
106
- child_node = body_node ; // Update child node Bc .
107
- body_node = child_node->parent_body_node (); // Update node B .
104
+ child_node = parent_node ; // Update child node C .
105
+ parent_node = child_node->parent_body_node (); // Update parent node P .
108
106
}
109
107
}
110
108
}
111
109
112
- // This is the inner loop of the CalcMassMatrix() algorithm. Rnv is the
113
- // size of the outer-loop body node R 's mobilizer, kNv is the size of the
114
- // current body B 's ConcreteMobilizer encountered on R 's inboard sweep.
115
- // Cost is Rnv *kNv*11 flops.
116
- #define DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (Rnv ) \
110
+ // This is the inner loop of the CalcMassMatrix() algorithm. Bnv is the
111
+ // size of the outer-loop body node B 's mobilizer, kNv is the size of the
112
+ // current inboard body P 's ConcreteMobilizer encountered on B 's inboard sweep.
113
+ // Cost is Bnv *kNv*11 flops.
114
+ #define DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK (Bnv ) \
117
115
template <typename T, class ConcreteMobilizer > \
118
116
void \
119
- BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixOffDiagonalBlock##Rnv ( \
120
- int R_start_in_v , const std::vector<Vector6<T>>& H_PB_W_cache, \
121
- const Eigen::Matrix<T, 6 , Rnv >& Fm_CBo_W , EigenPtr<MatrixX<T>> M) \
117
+ BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixOffDiagonalBlock##Bnv ( \
118
+ int B_start_in_v , const std::vector<Vector6<T>>& H_PB_W_cache, \
119
+ const Eigen::Matrix<T, 6 , Bnv >& Fm_CPo_W , EigenPtr<MatrixX<T>> M) \
122
120
const { \
123
121
if constexpr (kNv != 0 ) { \
124
- const auto H_PB_W = get_H (H_PB_W_cache); /* 6 x kNv fixed-size Map */ \
125
- const Eigen::Matrix<T, kNv , Rnv > HtFm = H_PB_W .transpose () * Fm_CBo_W ; \
126
- const int body_start_in_v = mobilizer ().velocity_start_in_v (); \
122
+ const auto H_PC_W = get_H (H_PB_W_cache); /* 6 x kNv fixed-size Map */ \
123
+ const Eigen::Matrix<T, kNv , Bnv > HtFm = H_PC_W .transpose () * Fm_CPo_W ; \
124
+ const int P_start_in_v = mobilizer ().velocity_start_in_v (); \
127
125
/* Update the appropriate block and its symmetric partner. */ \
128
- auto block = M->template block <kNv , Rnv>(body_start_in_v, R_start_in_v); \
126
+ auto block = M->template block <kNv , Bnv>(P_start_in_v, B_start_in_v); \
129
127
block = HtFm; \
130
- M->template block <Rnv , kNv >(R_start_in_v, body_start_in_v ) = \
128
+ M->template block <Bnv , kNv >(B_start_in_v, P_start_in_v ) = \
131
129
block.transpose (); \
132
130
} \
133
131
}
@@ -141,6 +139,52 @@ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6)
141
139
142
140
#undef DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK
143
141
142
+ template <typename T, class ConcreteMobilizer >
143
+ void BodyNodeImpl<T, ConcreteMobilizer>::
144
+ CalcMassMatrixContributionInM_TipToBase (
145
+ const PositionKinematicsCacheInM<T>& pcm,
146
+ const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
147
+ EigenPtr<MatrixX<T>> M) const {
148
+ // TODO(sherm1) Write this & corresponding off-diagonal.
149
+ unused (pcm, I_BMo_M_cache, M);
150
+ }
151
+
152
+ // This is the inner loop of the CalcMassMatrix() algorithm. Bnv is the
153
+ // size of the outer-loop body node B's mobilizer, kNv is the size of the
154
+ // current inboard body P's ConcreteMobilizer encountered on B's inboard sweep.
155
+ // C is the outboard (child) body whose inboard body was P.
156
+ // Cost depends on the force projection inline for P's mobilizer (very cheap
157
+ // for 1-dof mobilizers).
158
+ #define DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (Bnv ) \
159
+ template <typename T, class ConcreteMobilizer > \
160
+ void \
161
+ BodyNodeImpl<T, ConcreteMobilizer>::CalcMassMatrixOffDiagonalBlock##Bnv( \
162
+ int B_start_in_v, const Eigen::Matrix<T, 6 , Bnv>& Fm_CMp_Mp, \
163
+ EigenPtr<MatrixX<T>> M) const { \
164
+ if constexpr (kNv != 0 ) { \
165
+ const Eigen::Matrix<T, kNv , Bnv> HtFm; \
166
+ for (int j=0 ; j < Bnv; ++j) { \
167
+ /* Calculates tau = H_FM_Mᵀ * F_PMp_M */ \
168
+ mobilizer_->calc_tau_from_M (X_FM, q, Fm_CMp_Mp.col (j), &HtFm.col (j)); \
169
+ } \
170
+ const int P_start_in_v = mobilizer ().velocity_start_in_v (); \
171
+ /* Update the appropriate block and its symmetric partner. */ \
172
+ auto block = M->template block <kNv , Bnv>(P_start_in_v, B_start_in_v); \
173
+ block = HtFm; \
174
+ M->template block <Bnv, kNv >(B_start_in_v, P_start_in_v) = \
175
+ block.transpose (); \
176
+ } \
177
+ }
178
+
179
+ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (1 )
180
+ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (2 )
181
+ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (3 )
182
+ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (4 )
183
+ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (5 )
184
+ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M (6 )
185
+
186
+ #undef DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M
187
+
144
188
// Macro used to explicitly instantiate implementations for every mobilizer.
145
189
#define EXPLICITLY_INSTANTIATE_IMPLS (T ) \
146
190
template class BodyNodeImpl <T, CurvilinearMobilizer<T>>; \
0 commit comments