Skip to content

Commit b63fa6a

Browse files
sherm1sherm1
committed
Added CalcMassMatrixInM()
1 parent 7ede573 commit b63fa6a

13 files changed

+518
-191
lines changed

‎multibody/benchmarking/cassie.cc

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -169,6 +169,17 @@ class Cassie : public benchmark::Fixture {
169169
}
170170
}
171171

172+
// Runs the MassMatrixInM benchmark.
173+
// NOLINTNEXTLINE(runtime/references)
174+
void DoMassMatrixInM(benchmark::State& state) {
175+
DRAKE_DEMAND(want_grad_vdot(state) == false);
176+
DRAKE_DEMAND(want_grad_u(state) == false);
177+
for (auto _ : state) {
178+
InvalidateState();
179+
plant_->CalcMassMatrixInM(*context_, &mass_matrix_out_);
180+
}
181+
}
182+
172183
// Runs the InverseDynamics benchmark.
173184
// NOLINTNEXTLINE(runtime/references)
174185
void DoInverseDynamics(benchmark::State& state) {
@@ -399,6 +410,14 @@ BENCHMARK_REGISTER_F(CassieDouble, MassMatrix)
399410
->Unit(benchmark::kMicrosecond)
400411
->Arg(kWantNoGrad);
401412

413+
// NOLINTNEXTLINE(runtime/references)
414+
BENCHMARK_DEFINE_F(CassieDouble, MassMatrixInM)(benchmark::State& state) {
415+
DoMassMatrixInM(state);
416+
}
417+
BENCHMARK_REGISTER_F(CassieDouble, MassMatrixInM)
418+
->Unit(benchmark::kMicrosecond)
419+
->Arg(kWantNoGrad);
420+
402421
// NOLINTNEXTLINE(runtime/references)
403422
BENCHMARK_DEFINE_F(CassieDouble, InverseDynamics)(benchmark::State& state) {
404423
DoInverseDynamics(state);

‎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/plant/test/multibody_plant_test.cc

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2991,12 +2991,16 @@ TEST_F(SplitPendulum, MassMatrix) {
29912991
// state.
29922992
const double theta = M_PI / 3;
29932993

2994-
MatrixX<double> M(1, 1);
2994+
MatrixX<double> M_id(1, 1), M_in_world(1, 1), M_in_m(1, 1);
29952995
pin_->set_angle(context_.get(), theta);
2996-
plant_.CalcMassMatrixViaInverseDynamics(*context_, &M);
2996+
plant_.CalcMassMatrixViaInverseDynamics(*context_, &M_id);
2997+
plant_.CalcMassMatrix(*context_, &M_in_world);
2998+
plant_.CalcMassMatrixInM(*context_, &M_in_m);
29972999

29983000
// We can only expect values within the precision specified in the sdf file.
2999-
EXPECT_NEAR(M(0, 0), Io, 1.0e-6);
3001+
EXPECT_NEAR(M_id(0, 0), Io, 1.0e-6);
3002+
EXPECT_NEAR(M_in_world(0, 0), Io, 1.0e-6);
3003+
EXPECT_NEAR(M_in_m(0, 0), Io, 1.0e-6);
30003004
}
30013005

30023006
// This test ensures that we can create a symbolic mass matrix successfully.
@@ -3035,14 +3039,17 @@ TEST_F(SplitPendulum, SymbolicMassMatrix) {
30353039

30363040
// Calculate the mass matrix two different ways and verify that evaluating
30373041
// the resulting expressions yields the same result numerically.
3038-
Eigen::MatrixX<symbolic::Expression> M(1, 1), M_id(1, 1);
3039-
sym_plant->CalcMassMatrix(*sym_context, &M);
3042+
Eigen::MatrixX<symbolic::Expression> M_id(1, 1), M_in_world(1, 1),
3043+
M_in_m(1, 1);
30403044
sym_plant->CalcMassMatrixViaInverseDynamics(*sym_context, &M_id);
3045+
sym_plant->CalcMassMatrix(*sym_context, &M_in_world);
3046+
sym_plant->CalcMassMatrixInM(*sym_context, &M_in_m);
30413047

30423048
const symbolic::Environment env{{q_var(0), 2.0}, {v_var(0), 10.},
30433049
{m_var(0), 3.0}, {m_var(1), 4.0},
30443050
{l_var(0), 5.0}, {l_var(1), 6.0}};
3045-
EXPECT_NEAR(M(0, 0).Evaluate(env), M_id(0, 0).Evaluate(env), 1e-14);
3051+
EXPECT_NEAR(M_in_world(0, 0).Evaluate(env), M_id(0, 0).Evaluate(env), 1e-14);
3052+
EXPECT_NEAR(M_in_m(0, 0).Evaluate(env), M_id(0, 0).Evaluate(env), 1e-14);
30463053

30473054
// Generate symbolic expressions for a few more quantities here just as a
30483055
// sanity check that we can do so. We won't look at the results.

‎multibody/tree/body_node.cc

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -83,12 +83,11 @@ void BodyNode<T>::CalcCompositeBodyInertiaInM_TipToBase(
8383
(*I_BMb_Mb_all)[child_node_index];
8484
// Body B is the parent here.
8585
const math::RigidTransform<T>& X_MbMc = pcm.get_X_MpM(child_node_index);
86-
// Shift the child's composite body inertia to B's Mb frame origin Mbo, but
87-
// still expressed in Mc (so Mx==Mc here).
88-
const Vector3<T> p_McMb_Mb = -X_MbMc.translation(); // 3 flops
89-
SpatialInertia<T> I_CMb_Mx = I_CMc_Mc.Shift(p_McMb_Mb); // 37 flops
90-
I_CMb_Mx.ReExpressInPlace(X_MbMc.rotation()); // Now Mx==Mb, 72 flops
91-
I_BMb_Mb += I_CMb_Mx; // 36 flops
86+
const math::RotationMatrix<T>& R_MbMc = X_MbMc.rotation();
87+
const Vector3<T>& p_MbMc_Mb = X_MbMc.translation();
88+
const SpatialInertia<T> I_CMc_Mb = I_CMc_Mc.ReExpress(R_MbMc); // 72 flops
89+
const SpatialInertia<T> I_CMb_Mb = I_CMc_Mb.Shift(-p_MbMc_Mb); // 40 flops
90+
I_BMb_Mb += I_CMb_Mb; // 36 flops
9291
}
9392
}
9493

‎multibody/tree/body_node.h

Lines changed: 90 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -304,33 +304,55 @@ class BodyNode : public MultibodyElement<T> {
304304
// This node must fill in its nv x nv diagonal block in M, and then
305305
// sweep down to World filling in its diagonal contributions as in the
306306
// inner loop of algorithm 9.3, using the appropriate
307-
// CalcMassMatrixOffDiagonalHelper().
308-
virtual void CalcMassMatrixContribution_TipToBase(
307+
// CalcMassMatrixOffDiagonalInWorldHelper().
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 T* positions, 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
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
316322
// contribute here). This allows us to use fixed-size 2d matrices in the
317323
// 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) \
324330
const = 0
325331

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
332340

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
334356

335357
// This method is used by MultibodyTree within a base-to-tip loop to compute
336358
// this node's kinematics that depend on the generalized accelerations, i.e.
@@ -723,32 +745,60 @@ class BodyNode : public MultibodyElement<T> {
723745
const Mobilizer<T>* mobilizer_{nullptr};
724746
};
725747

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+
} \
742792
}
743793

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);
750800

751-
#undef SPECIALIZE_MASS_MATRIX_DISPATCHER
801+
#undef SPECIALIZE_MASS_MATRIX_IN_M_DISPATCHER
752802

753803
} // namespace internal
754804
} // namespace multibody

‎multibody/tree/body_node_impl.h

Lines changed: 34 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -92,28 +92,48 @@ 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 T* positions, 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.
103-
#define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(Rnv) \
104-
void CalcMassMatrixOffDiagonalBlock##Rnv( \
105-
int R_start_in_v, const std::vector<Vector6<T>>& H_PB_W_cache, \
106-
const Eigen::Matrix<T, 6, Rnv>& Fm_CCo_W, EigenPtr<MatrixX<T>> M) \
108+
#define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(Bnv) \
109+
void CalcMassMatrixOffDiagonalBlockInWorld##Bnv( \
110+
int B_start_in_v, const std::vector<Vector6<T>>& H_PB_W_cache, \
111+
const Eigen::Matrix<T, 6, Bnv>& Fm_CCo_W, EigenPtr<MatrixX<T>> M) \
107112
const final
108113

109-
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(1);
110-
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(2);
111-
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(3);
112-
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(4);
113-
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(5);
114-
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6);
115-
116-
#undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK
114+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(1);
115+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(2);
116+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(3);
117+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(4);
118+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(5);
119+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(6);
120+
121+
#undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD
122+
123+
#define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(Bnv) \
124+
void CalcMassMatrixOffDiagonalBlockInM##Bnv( \
125+
const T* positions, const PositionKinematicsCacheInM<T>& pcm, \
126+
int B_start_in_v, const Eigen::Matrix<T, 6, Bnv>& Fm_CMp_Mp, \
127+
EigenPtr<MatrixX<T>> M) const final
128+
129+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(1);
130+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(2);
131+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(3);
132+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(4);
133+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(5);
134+
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M(6);
135+
136+
#undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_M
117137

118138
void CalcSpatialAcceleration_BaseToTip(
119139
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,

0 commit comments

Comments
 (0)