Skip to content

Commit 47e1f39

Browse files
sherm1sherm1
committed
Added CalcMassMatrixInM()
1 parent 59a7d33 commit 47e1f39

14 files changed

+564
-202
lines changed

‎multibody/benchmarking/cassie.cc

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -158,6 +158,17 @@ class Cassie : public benchmark::Fixture {
158158
}
159159
}
160160

161+
// Runs the MassMatrixViaID benchmark.
162+
// NOLINTNEXTLINE(runtime/references)
163+
void DoMassMatrixViaID(benchmark::State& state) {
164+
DRAKE_DEMAND(want_grad_vdot(state) == false);
165+
DRAKE_DEMAND(want_grad_u(state) == false);
166+
for (auto _ : state) {
167+
InvalidateState();
168+
plant_->CalcMassMatrixViaInverseDynamics(*context_, &mass_matrix_out_);
169+
}
170+
}
171+
161172
// Runs the MassMatrix benchmark.
162173
// NOLINTNEXTLINE(runtime/references)
163174
void DoMassMatrix(benchmark::State& state) {
@@ -169,6 +180,17 @@ class Cassie : public benchmark::Fixture {
169180
}
170181
}
171182

183+
// Runs the MassMatrixInM benchmark.
184+
// NOLINTNEXTLINE(runtime/references)
185+
void DoMassMatrixInM(benchmark::State& state) {
186+
DRAKE_DEMAND(want_grad_vdot(state) == false);
187+
DRAKE_DEMAND(want_grad_u(state) == false);
188+
for (auto _ : state) {
189+
InvalidateState();
190+
plant_->CalcMassMatrixInM(*context_, &mass_matrix_out_);
191+
}
192+
}
193+
172194
// Runs the InverseDynamics benchmark.
173195
// NOLINTNEXTLINE(runtime/references)
174196
void DoInverseDynamics(benchmark::State& state) {
@@ -391,6 +413,14 @@ BENCHMARK_REGISTER_F(CassieDouble, PosAndVelKinematicsInM)
391413
->Unit(benchmark::kMicrosecond)
392414
->Arg(kWantNoGrad);
393415

416+
// NOLINTNEXTLINE(runtime/references)
417+
BENCHMARK_DEFINE_F(CassieDouble, MassMatrixViaID)(benchmark::State& state) {
418+
DoMassMatrixViaID(state);
419+
}
420+
BENCHMARK_REGISTER_F(CassieDouble, MassMatrixViaID)
421+
->Unit(benchmark::kMicrosecond)
422+
->Arg(kWantNoGrad);
423+
394424
// NOLINTNEXTLINE(runtime/references)
395425
BENCHMARK_DEFINE_F(CassieDouble, MassMatrix)(benchmark::State& state) {
396426
DoMassMatrix(state);
@@ -399,6 +429,14 @@ BENCHMARK_REGISTER_F(CassieDouble, MassMatrix)
399429
->Unit(benchmark::kMicrosecond)
400430
->Arg(kWantNoGrad);
401431

432+
// NOLINTNEXTLINE(runtime/references)
433+
BENCHMARK_DEFINE_F(CassieDouble, MassMatrixInM)(benchmark::State& state) {
434+
DoMassMatrixInM(state);
435+
}
436+
BENCHMARK_REGISTER_F(CassieDouble, MassMatrixInM)
437+
->Unit(benchmark::kMicrosecond)
438+
->Arg(kWantNoGrad);
439+
402440
// NOLINTNEXTLINE(runtime/references)
403441
BENCHMARK_DEFINE_F(CassieDouble, InverseDynamics)(benchmark::State& state) {
404442
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_mass_matrix_test.cc

Lines changed: 25 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -21,11 +21,12 @@ using test::LimitMalloc;
2121
namespace multibody {
2222
namespace {
2323

24-
// We verify the computation of the mass matrix by comparing two significantly
24+
// We verify the computation of the mass matrix by comparing three significantly
2525
// different implementations:
26-
// - CalcMassMatrix(): uses the Composite Body Algorithm.
26+
// - CalcMassMatrix(): Composite Body Algorithm, in the World frame.
27+
// - CalcMassMatrixInM(): Composite Body Algorithm, in mobilizer M frames.
2728
// - CalcMassMatrixViaInverseDynamics(): uses inverse dynamics to compute each
28-
// column of the mass matrix at a time.
29+
// column of the mass matrix one at a time.
2930
class MultibodyPlantMassMatrixTests : public ::testing::Test {
3031
public:
3132
void LoadUrl(const std::string& url) {
@@ -60,15 +61,26 @@ class MultibodyPlantMassMatrixTests : public ::testing::Test {
6061
// mass matrix by comparing the results from CalcMassMatrix() and
6162
// CalcMassMatrixViaInverseDynamics().
6263
void VerifyMassMatrixComputation(const Context<double>& context) {
63-
// Compute mass matrix via the Composite Body Algorithm.
64-
MatrixX<double> Mcba(plant_.num_velocities(), plant_.num_velocities());
65-
plant_.CalcMassMatrix(context, &Mcba);
64+
// Compute mass matrix via the Composite Body Algorithm in World.
65+
MatrixX<double> Mcba_in_W(plant_.num_velocities(), plant_.num_velocities());
66+
plant_.CalcMassMatrix(context, &Mcba_in_W);
6667

6768
// After a first warm-up call, subsequent calls to CalcMassMatrix<double>()
6869
// should never allocate.
6970
{
7071
LimitMalloc guard;
71-
plant_.CalcMassMatrix(context, &Mcba);
72+
plant_.CalcMassMatrix(context, &Mcba_in_W);
73+
}
74+
75+
// Compute mass matrix via the Composite Body Algorithm in M frames.
76+
MatrixX<double> Mcba_in_M(plant_.num_velocities(), plant_.num_velocities());
77+
plant_.CalcMassMatrixInM(context, &Mcba_in_M);
78+
79+
// After a first warm-up call, subsequent calls to
80+
// CalcMassMatrixInM<double>() should never allocate.
81+
{
82+
LimitMalloc guard;
83+
plant_.CalcMassMatrixInM(context, &Mcba_in_M);
7284
}
7385

7486
// Compute mass matrix using inverse dynamics for each column.
@@ -80,10 +92,12 @@ class MultibodyPlantMassMatrixTests : public ::testing::Test {
8092
// squared root of the number of elements in the matrix, this tolerance is
8193
// effectively being scaled by the RMS value of the elements in the mass
8294
// matrix.
83-
const double kTolerance = 10.0 * std::numeric_limits<double>::epsilon() *
84-
Mcba.norm() / plant_.num_velocities();
85-
EXPECT_TRUE(
86-
CompareMatrices(Mcba, Mid, kTolerance, MatrixCompareType::relative));
95+
const double tolerance = 10.0 * std::numeric_limits<double>::epsilon() *
96+
Mcba_in_W.norm() / plant_.num_velocities();
97+
EXPECT_TRUE(CompareMatrices(Mcba_in_W, Mid, tolerance,
98+
MatrixCompareType::relative));
99+
EXPECT_TRUE(CompareMatrices(Mcba_in_M, Mid, tolerance,
100+
MatrixCompareType::relative));
87101
}
88102

89103
protected:

‎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

0 commit comments

Comments
 (0)