Skip to content

Commit 7f188e9

Browse files
committed
Working on mass matrix in M
1 parent 41583c9 commit 7f188e9

File tree

9 files changed

+200
-84
lines changed

9 files changed

+200
-84
lines changed

multibody/plant/multibody_plant.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4337,6 +4337,15 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
43374337
internal_tree().CalcMassMatrix(context, M);
43384338
}
43394339

4340+
// Same mass matrix, calculated by working in each body's inboard mobilizer
4341+
// frame M, rather than in World. Should be faster.
4342+
void CalcMassMatrixInM(const systems::Context<T>& context,
4343+
EigenPtr<MatrixX<T>> M) const {
4344+
this->ValidateContext(context);
4345+
DRAKE_DEMAND(M != nullptr);
4346+
internal_tree().CalcMassMatrixInM(context, M);
4347+
}
4348+
43404349
/// This method allows users to map the state of `this` model, x, into a
43414350
/// vector of selected state xₛ with a given preferred ordering.
43424351
/// The mapping, or selection, is returned in the form of a selector matrix

multibody/tree/body_node.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -85,10 +85,10 @@ void BodyNode<T>::CalcCompositeBodyInertiaInM_TipToBase(
8585
const math::RigidTransform<T>& X_MbMc = pcm.get_X_MpM(child_node_index);
8686
// Shift the child's composite body inertia to B's Mb frame origin Mbo, but
8787
// still expressed in Mc (so Mx==Mc here).
88-
const Vector3<T> p_McMb_Mb = -X_MbMc.translation(); // 3 flops
88+
const Vector3<T> p_McMb_Mb = -X_MbMc.translation(); // 3 flops
8989
SpatialInertia<T> I_CMb_Mx = I_CMc_Mc.Shift(p_McMb_Mb); // 37 flops
9090
I_CMb_Mx.ReExpressInPlace(X_MbMc.rotation()); // Now Mx==Mb, 72 flops
91-
I_BMb_Mb += I_CMb_Mx; // 36 flops
91+
I_BMb_Mb += I_CMb_Mx; // 36 flops
9292
}
9393
}
9494

multibody/tree/body_node.h

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -305,12 +305,18 @@ class BodyNode : public MultibodyElement<T> {
305305
// sweep down to World filling in its diagonal contributions as in the
306306
// inner loop of algorithm 9.3, using the appropriate
307307
// CalcMassMatrixOffDiagonalHelper().
308-
virtual void CalcMassMatrixContribution_TipToBase(
308+
virtual void CalcMassMatrixContributionInWorld_TipToBase(
309309
const PositionKinematicsCache<T>& pc,
310-
const std::vector<SpatialInertia<T>>& Mc_B_W_cache,
310+
const std::vector<SpatialInertia<T>>& I_BBo_W_cache,
311311
const std::vector<Vector6<T>>& H_PB_W_cache,
312312
EigenPtr<MatrixX<T>> M) const = 0;
313313

314+
// Same, but calculated in body M frames rather than World.
315+
virtual void CalcMassMatrixContributionInM_TipToBase(
316+
const PositionKinematicsCacheInM<T>& pcm,
317+
const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
318+
EigenPtr<MatrixX<T>> M) const = 0;
319+
314320
// There are six functions for calculating the off-diagonal blocks, one for
315321
// each possible size Rnv of body R(k)'s inboard mobilizer (welds don't
316322
// contribute here). This allows us to use fixed-size 2d matrices in the
@@ -724,20 +730,20 @@ class BodyNode : public MultibodyElement<T> {
724730
};
725731

726732
// 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>
733+
// composite body B on each of the bodies on its inboard path to World.
734+
template <typename T, int Bnv>
729735
class CalcMassMatrixOffDiagonalDispatcher;
730736

731-
#define SPECIALIZE_MASS_MATRIX_DISPATCHER(Rnv) \
737+
#define SPECIALIZE_MASS_MATRIX_DISPATCHER(Bnv) \
732738
template <typename T> \
733-
class CalcMassMatrixOffDiagonalDispatcher<T, Rnv> { \
739+
class CalcMassMatrixOffDiagonalDispatcher<T, Bnv> { \
734740
public: \
735-
static void Dispatch(const BodyNode<T>& body_node, int R_start_in_v, \
741+
static void Dispatch(const BodyNode<T>& body_node, int B_start_in_v, \
736742
const std::vector<Vector6<T>>& H_PB_W_cache, \
737-
const Eigen::Matrix<T, 6, Rnv>& Fm_CBo_W, \
743+
const Eigen::Matrix<T, 6, Bnv>& Fm_CPo_W, \
738744
EigenPtr<MatrixX<T>> M) { \
739-
body_node.CalcMassMatrixOffDiagonalBlock##Rnv( \
740-
R_start_in_v, H_PB_W_cache, Fm_CBo_W, M); \
745+
body_node.CalcMassMatrixOffDiagonalBlock##Bnv( \
746+
B_start_in_v, H_PB_W_cache, Fm_CPo_W, M); \
741747
} \
742748
}
743749

multibody/tree/body_node_impl.h

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -92,12 +92,17 @@ class BodyNodeImpl final : public BodyNode<T> {
9292
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
9393
const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const final;
9494

95-
void CalcMassMatrixContribution_TipToBase(
95+
void CalcMassMatrixContributionInWorld_TipToBase(
9696
const PositionKinematicsCache<T>& pc,
97-
const std::vector<SpatialInertia<T>>& Mc_B_W_cache,
97+
const std::vector<SpatialInertia<T>>& I_BBo_W_cache,
9898
const std::vector<Vector6<T>>& H_PB_W_cache,
9999
EigenPtr<MatrixX<T>> M) const final;
100100

101+
void CalcMassMatrixContributionInM_TipToBase(
102+
const PositionKinematicsCacheInM<T>& pcm,
103+
const std::vector<SpatialInertia<T>>& I_BMo_M_cache,
104+
EigenPtr<MatrixX<T>> M) const final;
105+
101106
// Declare functions for the six sizes of mass matrix off-diagonal
102107
// blocks.
103108
#define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(Rnv) \

multibody/tree/body_node_impl_mass_matrix.cc

Lines changed: 108 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -24,110 +24,108 @@ namespace multibody {
2424
namespace internal {
2525

2626
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 {
3133
// Welds only contribute to composite mass properties. Their effect on the
3234
// mass matrix gets included when we get to the composite's non-weld inboard
3335
// mobilizer.
3436
if constexpr (kNv != 0) {
3537
// 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()];
3739

3840
// 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.
4042

4143
// 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.
4346
// 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
4548
// is in ℝ⁶ˣᵐ. That is, we are considering each case in which all
4649
// generalized accelerations are zero but the m-th generalized
4750
// acceleration for this node equals one.
4851
// 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.
5356
// 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.
5860

5961
// If we consider the closed system composed of the composite body held by
6062
// 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
6767

68-
const int composite_start_in_v = mobilizer().velocity_start_in_v();
68+
const int B_start_in_v = mobilizer().velocity_start_in_v();
6969

7070
// 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
7373

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
7575
// 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
9393
// 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?).
9694
for (int col = 0; col < kNv; ++col) {
9795
// 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
10199
}
102100

103101
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);
105103

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.
108106
}
109107
}
110108
}
111109

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) \
117115
template <typename T, class ConcreteMobilizer> \
118116
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) \
122120
const { \
123121
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(); \
127125
/* 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); \
129127
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) = \
131129
block.transpose(); \
132130
} \
133131
}
@@ -141,6 +139,52 @@ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6)
141139

142140
#undef DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK
143141

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+
144188
// Macro used to explicitly instantiate implementations for every mobilizer.
145189
#define EXPLICITLY_INSTANTIATE_IMPLS(T) \
146190
template class BodyNodeImpl<T, CurvilinearMobilizer<T>>; \

multibody/tree/body_node_world.h

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,12 +56,18 @@ class BodyNodeWorld final : public BodyNode<T> {
5656
DRAKE_UNREACHABLE();
5757
}
5858

59-
void CalcMassMatrixContribution_TipToBase(
59+
void CalcMassMatrixContributionInWorld_TipToBase(
6060
const PositionKinematicsCache<T>&, const std::vector<SpatialInertia<T>>&,
6161
const std::vector<Vector6<T>>&, EigenPtr<MatrixX<T>>) const final {
6262
DRAKE_UNREACHABLE();
6363
}
6464

65+
void CalcMassMatrixContributionInM_TipToBase(
66+
const PositionKinematicsCacheInM<T>&,
67+
const std::vector<SpatialInertia<T>>&, EigenPtr<MatrixX<T>>) const final {
68+
DRAKE_UNREACHABLE();
69+
}
70+
6571
#define DEFINE_DUMMY_OFF_DIAGONAL_BLOCK(Rnv) \
6672
void CalcMassMatrixOffDiagonalBlock##Rnv( \
6773
int, const std::vector<Vector6<T>>&, const Eigen::Matrix<T, 6, Rnv>&, \

0 commit comments

Comments
 (0)