Skip to content

Commit e07f044

Browse files
sherm1sherm1
committed
Added CompositeBodyInertiasInM and benchmark
Avoid divides; inline small methods.
1 parent ade5925 commit e07f044

14 files changed

+261
-162
lines changed

‎multibody/benchmarking/cassie.cc

Lines changed: 44 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,28 @@ class Cassie : public benchmark::Fixture {
112112
}
113113
}
114114

115+
// Runs the CompositeBodyInertiaInWorld benchmark.
116+
// NOLINTNEXTLINE(runtime/references)
117+
void DoCompositeBodyInertiaInWorld(benchmark::State& state) {
118+
DRAKE_DEMAND(want_grad_vdot(state) == false);
119+
DRAKE_DEMAND(want_grad_u(state) == false);
120+
for (auto _ : state) {
121+
InvalidateState();
122+
plant_->EvalCompositeBodyInertiaInWorldCache(*context_);
123+
}
124+
}
125+
126+
// Runs the CompositeBodyInertiaInM benchmark.
127+
// NOLINTNEXTLINE(runtime/references)
128+
void DoCompositeBodyInertiaInM(benchmark::State& state) {
129+
DRAKE_DEMAND(want_grad_vdot(state) == false);
130+
DRAKE_DEMAND(want_grad_u(state) == false);
131+
for (auto _ : state) {
132+
InvalidateState();
133+
plant_->EvalCompositeBodyInertiaInMCache(*context_);
134+
}
135+
}
136+
115137
// Runs the PosAndVelKinematics benchmark.
116138
// NOLINTNEXTLINE(runtime/references)
117139
void DoPosAndVelKinematics(benchmark::State& state) {
@@ -325,15 +347,33 @@ BENCHMARK_REGISTER_F(CassieDouble, PositionKinematics)
325347
->Unit(benchmark::kMicrosecond)
326348
->Arg(kWantNoGrad);
327349

350+
BENCHMARK_DEFINE_F(CassieDouble, PositionKinematicsInM)
328351
// NOLINTNEXTLINE(runtime/references)
329-
BENCHMARK_DEFINE_F(CassieDouble,
330-
PositionKinematicsInM)(benchmark::State& state) {
352+
(benchmark::State& state) {
331353
DoPositionKinematicsInM(state);
332354
}
333355
BENCHMARK_REGISTER_F(CassieDouble, PositionKinematicsInM)
334356
->Unit(benchmark::kMicrosecond)
335357
->Arg(kWantNoGrad);
336358

359+
BENCHMARK_DEFINE_F(CassieDouble, CompositeBodyInertiaInWorld)
360+
// NOLINTNEXTLINE(runtime/references)
361+
(benchmark::State& state) {
362+
DoCompositeBodyInertiaInWorld(state);
363+
}
364+
BENCHMARK_REGISTER_F(CassieDouble, CompositeBodyInertiaInWorld)
365+
->Unit(benchmark::kMicrosecond)
366+
->Arg(kWantNoGrad);
367+
368+
BENCHMARK_DEFINE_F(CassieDouble, CompositeBodyInertiaInM)
369+
// NOLINTNEXTLINE(runtime/references)
370+
(benchmark::State& state) {
371+
DoCompositeBodyInertiaInM(state);
372+
}
373+
BENCHMARK_REGISTER_F(CassieDouble, CompositeBodyInertiaInM)
374+
->Unit(benchmark::kMicrosecond)
375+
->Arg(kWantNoGrad);
376+
337377
// NOLINTNEXTLINE(runtime/references)
338378
BENCHMARK_DEFINE_F(CassieDouble, PosAndVelKinematics)(benchmark::State& state) {
339379
DoPosAndVelKinematics(state);
@@ -342,9 +382,9 @@ BENCHMARK_REGISTER_F(CassieDouble, PosAndVelKinematics)
342382
->Unit(benchmark::kMicrosecond)
343383
->Arg(kWantNoGrad);
344384

385+
BENCHMARK_DEFINE_F(CassieDouble, PosAndVelKinematicsInM)
345386
// NOLINTNEXTLINE(runtime/references)
346-
BENCHMARK_DEFINE_F(CassieDouble,
347-
PosAndVelKinematicsInM)(benchmark::State& state) {
387+
(benchmark::State& state) {
348388
DoPosAndVelKinematicsInM(state);
349389
}
350390
BENCHMARK_REGISTER_F(CassieDouble, PosAndVelKinematicsInM)

‎multibody/tree/body_node.cc

Lines changed: 57 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -10,62 +10,85 @@ namespace internal {
1010
template <typename T>
1111
BodyNode<T>::~BodyNode() = default;
1212

13+
// Notation used below
14+
//
15+
// `this` is mobilized body B, with body frame B. We're given B's spatial
16+
// inertia M_BBo_W, taken about B's origin Bo and expressed in world frame W.
17+
// Our goal is to calculate B's composite body inertia I_BBo_W, taken about Bo
18+
// but including all the bodies outboard of B as though they were welded in
19+
// their current configuration.
20+
//
21+
// Below we're going to treat B as the "parent" and work with its immediately
22+
// outboard bodies as "children" C. Because this is an inward sweep, we already
23+
// know I_CCo_W, each child's composite body inertia, taken about the child's
24+
// body frame origin Co, and expressed in W. We need to shift those to Bo before
25+
// summing them up.
1326
template <typename T>
1427
void BodyNode<T>::CalcCompositeBodyInertiaInWorld_TipToBase(
15-
const PositionKinematicsCache<T>& pc,
28+
const PositionKinematicsCache<T>& pc, // for p_BoCo_W
1629
const std::vector<SpatialInertia<T>>& M_BBo_W_all,
17-
std::vector<SpatialInertia<T>>* Mc_BBo_W_all) const {
18-
DRAKE_ASSERT(mobod_index() != world_mobod_index());
19-
DRAKE_ASSERT(Mc_BBo_W_all != nullptr);
30+
std::vector<SpatialInertia<T>>* I_BBo_W_all) const {
31+
const MobodIndex index = mobod_index();
32+
DRAKE_ASSERT(index != world_mobod_index());
33+
DRAKE_ASSERT(I_BBo_W_all != nullptr);
2034

2135
// This mobod's spatial inertia (given).
22-
const SpatialInertia<T>& M_BBo_W = M_BBo_W_all[mobod_index()];
36+
const SpatialInertia<T>& M_BBo_W = M_BBo_W_all[index];
2337
// This mobod's composite body inertia (to be calculated).
24-
SpatialInertia<T>& Mc_BBo_W = (*Mc_BBo_W_all)[mobod_index()];
38+
SpatialInertia<T>& I_BBo_W = (*I_BBo_W_all)[index];
2539

26-
// Composite body inertia for this node B, about its frame's origin Bo, and
27-
// expressed in the world frame W. Add composite body inertia contributions
28-
// from all children (already calculated).
29-
Mc_BBo_W = M_BBo_W;
40+
I_BBo_W = M_BBo_W; // Start with B's spatial inertia.
41+
// Then add in each child's composite body inertia, 76 flops per.
3042
for (const BodyNode<T>* child : child_nodes()) {
3143
const MobodIndex child_node_index = child->mobod_index();
32-
// Composite body inertia for child body C, about Co, expressed in W.
33-
const SpatialInertia<T>& Mc_CCo_W = (*Mc_BBo_W_all)[child_node_index];
34-
// Shift to Bo and add it to the composite body inertia of B.
44+
const SpatialInertia<T>& I_CCo_W = (*I_BBo_W_all)[child_node_index];
45+
// Body B is the parent "P" here and C is the body "B" (in pc).
3546
const Vector3<T>& p_BoCo_W = pc.get_p_PoBo_W(child_node_index);
36-
Mc_BBo_W += Mc_CCo_W.Shift(-p_BoCo_W); // i.e., by p_CoBo
47+
I_BBo_W += I_CCo_W.Shift(-p_BoCo_W); // i.e., by p_CoBo, 76 flops
3748
}
3849
}
3950

51+
// Notation used below
52+
//
53+
// `this` is mobilized body B. It's mobilizer's inboard frame (the "M" frame) is
54+
// denoted Mb. We're given B's spatial inertia M_BMb_Mb (taken about Mb's origin
55+
// Mbo and expressed in Mb). Our goal is to calculate B's composite body inertia
56+
// I_BMb_Mb, taken about Mbo but including all the bodies outboard of B as
57+
// though they were welded in their current configuration.
58+
//
59+
// Below we're going to treat B as the "parent" and work with its immediately
60+
// outboard bodies as "children" C. Because this is an inward sweep, we
61+
// already know I_CMc_Mc, each child's composite body inertia, taken about
62+
// the child's inboard frame Mc's origin Mco, and expressed in Mc. We need to
63+
// shift those to Mbo and re-express in Mb before summing them up.
4064
template <typename T>
4165
void BodyNode<T>::CalcCompositeBodyInertiaInM_TipToBase(
42-
const FrameBodyPoseCache<T>& frame_body_pose_cache, // for M_BMo_M
43-
const PositionKinematicsCacheInM<T>& pcm, // for X_MpM(q)
44-
std::vector<SpatialInertia<T>>* Mc_BMo_M_all) const {
45-
DRAKE_ASSERT(mobod_index() != world_mobod_index());
46-
DRAKE_ASSERT(Mc_BMo_M_all != nullptr);
66+
const FrameBodyPoseCache<T>& frame_body_pose_cache, // for M_BMb_Mb
67+
const PositionKinematicsCacheInM<T>& pcm, // for X_MbMc(q)
68+
std::vector<SpatialInertia<T>>* I_BMb_Mb_all) const {
69+
const MobodIndex index = mobod_index();
70+
DRAKE_ASSERT(index != world_mobod_index());
71+
DRAKE_ASSERT(I_BMb_Mb_all != nullptr);
4772

4873
// This mobod's spatial inertia (given).
49-
const SpatialInertia<T>& M_BMo_M =
50-
frame_body_pose_cache.get_M_BMo_M(mobod_index());
74+
const SpatialInertia<T>& M_BMb_Mb = frame_body_pose_cache.get_M_BMo_M(index);
5175
// This mobod's composite body inertia (to be calculated).
52-
SpatialInertia<T>& Mc_BMo_M = (*Mc_BMo_M_all)[mobod_index()];
76+
SpatialInertia<T>& I_BMb_Mb = (*I_BMb_Mb_all)[index];
5377

54-
// Composite body inertia for this node B, about its M frame's origin Mo, and
55-
// expressed in M. Add composite body inertia contributions from all children
56-
// (already calculated).
57-
Mc_BMo_M = M_BMo_M;
78+
I_BMb_Mb = M_BMb_Mb; // Start with B's spatial inertia.
79+
// Then add in each child's composite body inertia, 148 flops per.
5880
for (const BodyNode<T>* child : child_nodes()) {
5981
const MobodIndex child_node_index = child->mobod_index();
60-
// Composite body inertia for child body C, about Co, expressed in W.
61-
const SpatialInertia<T>& Mc_CMco_Mc = (*Mc_BMo_M_all)[child_node_index];
62-
// Shift to B's Mo, re-express in B's M, and add it to the composite body
63-
// inertia of B.
82+
const SpatialInertia<T>& I_CMc_Mc = // Child's composite body inertia.
83+
(*I_BMb_Mb_all)[child_node_index];
84+
// Body B is the parent here.
6485
const math::RigidTransform<T>& X_MbMc = pcm.get_X_MpM(child_node_index);
65-
// Shift to B's M frame origin Mbo, but still expressed in Mc.
66-
SpatialInertia<T> Mc_CMbo_M = Mc_CMco_Mc.Shift(-X_MbMc.translation());
67-
Mc_CMbo_M.ReExpressInPlace(X_MbMc.rotation()); // Now expressed in Mb
68-
Mc_BMo_M += Mc_CMbo_M;
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
6992
}
7093
}
7194

‎multibody/tree/body_node.h

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -635,18 +635,18 @@ class BodyNode : public MultibodyElement<T> {
635635
// It must contain already up-to-date composite body inertias for all the
636636
// children of `this` node.
637637
//
638-
// @pre CalcCompositeBodyInertia_TipToBase() must have already been called
639-
// for the children nodes (and, by recursive precondition, all outboard nodes
640-
// in the tree.)
638+
// @pre CalcCompositeBodyInertiaWorld_TipToBase() must have already been
639+
// called for the children nodes (and, by recursive precondition, all outboard
640+
// nodes in the tree.)
641641
virtual void CalcCompositeBodyInertiaInWorld_TipToBase(
642642
const PositionKinematicsCache<T>& pc,
643643
const std::vector<SpatialInertia<T>>& M_BBo_W_all,
644644
std::vector<SpatialInertia<T>>* Mc_BBo_W_all) const;
645645

646646
virtual void CalcCompositeBodyInertiaInM_TipToBase(
647-
const FrameBodyPoseCache<T>& frame_body_pose_cache, // for M_BMo_M
648-
const PositionKinematicsCacheInM<T>& pcm, // for X_MpM(q)
649-
std::vector<SpatialInertia<T>>* Mc_BMo_M_all) const;
647+
const FrameBodyPoseCache<T>& frame_body_pose_cache, // for M_BMo_M
648+
const PositionKinematicsCacheInM<T>& pcm, // for X_MpM(q)
649+
std::vector<SpatialInertia<T>>* Mc_BMo_M_all) const;
650650

651651
// Forms LLT factorization of articulated rigid body's hinge inertia matrix.
652652
// @param[in] D_B Articulated rigid body hinge matrix.

‎multibody/tree/body_node_world.h

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

143-
void CalcCompositeBodyInertia_TipToBase(
143+
void CalcCompositeBodyInertiaInWorld_TipToBase(
144144
const PositionKinematicsCache<T>&, const std::vector<SpatialInertia<T>>&,
145145
std::vector<SpatialInertia<T>>*) const final {
146146
DRAKE_UNREACHABLE();
147147
}
148148

149+
void CalcCompositeBodyInertiaInM_TipToBase(
150+
const FrameBodyPoseCache<T>&, const PositionKinematicsCacheInM<T>&,
151+
std::vector<SpatialInertia<T>>*) const final {
152+
DRAKE_UNREACHABLE();
153+
}
154+
149155
void CalcSpatialAccelerationBias(
150156
const FrameBodyPoseCache<T>&, const T*, const PositionKinematicsCache<T>&,
151157
const T*, const VelocityKinematicsCache<T>&,

‎multibody/tree/multibody_tree.cc

Lines changed: 29 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1442,7 +1442,8 @@ void MultibodyTree<T>::CalcSpatialInertiasInWorld(
14421442
frame_body_pose_cache.get_M_BBo_B(body.mobod_index());
14431443
// Re-express body B's spatial inertia in the world frame W.
14441444
SpatialInertia<T>& M_BBo_W = (*M_B_W_all)[body.mobod_index()];
1445-
M_BBo_W = M_BBo_B.ReExpress(R_WB);
1445+
M_BBo_W = M_BBo_B; // Wrong frame.
1446+
M_BBo_W.ReExpressInPlace(R_WB); // Fixed.
14461447
}
14471448
}
14481449

@@ -1566,15 +1567,34 @@ void MultibodyTree<T>::CalcCompositeBodyInertiasInWorld(
15661567
EvalSpatialInertiaInWorldCache(context);
15671568

15681569
// Perform tip-to-base recursion for each composite body, skipping the world.
1569-
for (int level = forest_height() - 1; level > 0; --level) {
1570-
for (MobodIndex mobod_index : body_node_levels_[level]) {
1571-
// Node corresponding to the base of composite body C. We'll add in
1572-
// everything outboard of this node.
1573-
const BodyNode<T>& composite_node = *body_nodes_[mobod_index];
1570+
for (MobodIndex mobod_index(num_mobods() - 1); mobod_index > 0;
1571+
--mobod_index) {
1572+
// Node corresponding to the base of composite body C. We'll add in
1573+
// everything outboard of this node.
1574+
const BodyNode<T>& composite_node = *body_nodes_[mobod_index];
15741575

1575-
composite_node.CalcCompositeBodyInertiaInWorld_TipToBase(pc, M_BBo_W_all,
1576-
&*Mc_BBo_W_all);
1577-
}
1576+
composite_node.CalcCompositeBodyInertiaInWorld_TipToBase(pc, M_BBo_W_all,
1577+
&*Mc_BBo_W_all);
1578+
}
1579+
}
1580+
1581+
template <typename T>
1582+
void MultibodyTree<T>::CalcCompositeBodyInertiasInM(
1583+
const systems::Context<T>& context,
1584+
std::vector<SpatialInertia<T>>* I_BMo_M_all) const {
1585+
const FrameBodyPoseCache<T>& frame_body_pose_cache =
1586+
EvalFrameBodyPoses(context);
1587+
const PositionKinematicsCacheInM<T>& pcm = EvalPositionKinematicsInM(context);
1588+
1589+
// Perform tip-to-base recursion for each composite body, skipping the world.
1590+
for (MobodIndex mobod_index(num_mobods() - 1); mobod_index > 0;
1591+
--mobod_index) {
1592+
// Node corresponding to the base of the composite body. We'll add in
1593+
// everything outboard of this node.
1594+
const BodyNode<T>& composite_node = *body_nodes_[mobod_index];
1595+
1596+
composite_node.CalcCompositeBodyInertiaInM_TipToBase(frame_body_pose_cache,
1597+
pcm, &*I_BMo_M_all);
15781598
}
15791599
}
15801600

‎multibody/tree/multibody_tree_system.cc

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -270,16 +270,26 @@ void MultibodyTreeSystem<T>::Finalize() {
270270
{position_kinematics_cache_entry().ticket()})
271271
.cache_index();
272272

273-
// Allocate cache entry for composite-body inertias Mc_B_W(q) for each body.
273+
// Allocate cache entry for composite-body inertias I_BBo_W(q) for each body.
274274
cache_indexes_.composite_body_inertia_in_world =
275275
this->DeclareCacheEntry(
276-
std::string("composite body inertia in world (Mc_B_W)"),
276+
std::string("composite body inertia in world (I_BBo_W)"),
277277
std::vector<SpatialInertia<T>>(internal_tree().num_bodies(),
278278
SpatialInertia<T>::NaN()),
279279
&MultibodyTreeSystem<T>::CalcCompositeBodyInertiasInWorld,
280280
{position_kinematics_cache_entry().ticket()})
281281
.cache_index();
282282

283+
// Allocate cache entry for composite-body inertias I_BMo_M(q) for each body.
284+
cache_indexes_.composite_body_inertia_in_m =
285+
this->DeclareCacheEntry(
286+
std::string("composite body inertia in M (I_BMo_M)"),
287+
std::vector<SpatialInertia<T>>(internal_tree().num_bodies(),
288+
SpatialInertia<T>::NaN()),
289+
&MultibodyTreeSystem<T>::CalcCompositeBodyInertiasInM,
290+
{position_kinematics_cache_entry().ticket()})
291+
.cache_index();
292+
283293
// Declare cache entries dependent on velocities (and parameters & positions).
284294

285295
// Allocate velocity cache.

‎multibody/tree/multibody_tree_system.h

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -185,14 +185,23 @@ class MultibodyTreeSystem : public systems::LeafSystem<T> {
185185
}
186186

187187
/* Returns a reference to the up-to-date cache of composite-body inertias
188-
in the given Context, recalculating it first if necessary. */
188+
I_BBo_W in the given Context, recalculating it first if necessary. */
189189
const std::vector<SpatialInertia<T>>& EvalCompositeBodyInertiaInWorldCache(
190190
const systems::Context<T>& context) const {
191191
this->ValidateContext(context);
192192
return this->get_cache_entry(cache_indexes_.composite_body_inertia_in_world)
193193
.template Eval<std::vector<SpatialInertia<T>>>(context);
194194
}
195195

196+
/* Returns a reference to the up-to-date cache of composite-body inertias
197+
I_BMo_M in the given Context, recalculating it first if necessary. */
198+
const std::vector<SpatialInertia<T>>& EvalCompositeBodyInertiaInMCache(
199+
const systems::Context<T>& context) const {
200+
this->ValidateContext(context);
201+
return this->get_cache_entry(cache_indexes_.composite_body_inertia_in_m)
202+
.template Eval<std::vector<SpatialInertia<T>>>(context);
203+
}
204+
196205
/* Returns a reference to the up-to-date cache of per-body bias terms in
197206
the given Context, recalculating it first if necessary.
198207
For a body B, this is the bias term `Fb_Bo_W(q, v)` in the equation
@@ -456,16 +465,14 @@ class MultibodyTreeSystem : public systems::LeafSystem<T> {
456465

457466
void CalcCompositeBodyInertiasInWorld(
458467
const systems::Context<T>& context,
459-
std::vector<SpatialInertia<T>>* composite_body_inertias) const {
460-
internal_tree().CalcCompositeBodyInertiasInWorld(context,
461-
composite_body_inertias);
468+
std::vector<SpatialInertia<T>>* I_BBo_W_all) const {
469+
internal_tree().CalcCompositeBodyInertiasInWorld(context, I_BBo_W_all);
462470
}
463471

464472
void CalcCompositeBodyInertiasInM(
465473
const systems::Context<T>& context,
466-
std::vector<SpatialInertia<T>>* composite_body_inertias_in_m) const {
467-
internal_tree().CalcCompositeBodyInertiasInM(context,
468-
composite_body_inertias_in_m);
474+
std::vector<SpatialInertia<T>>* I_BMo_M_all) const {
475+
internal_tree().CalcCompositeBodyInertiasInM(context, I_BMo_M_all);
469476
}
470477

471478
void CalcAcrossNodeJacobianWrtVExpressedInWorld(
@@ -591,6 +598,7 @@ class MultibodyTreeSystem : public systems::LeafSystem<T> {
591598
systems::CacheIndex position_kinematics;
592599
systems::CacheIndex position_kinematics_in_m;
593600
systems::CacheIndex spatial_inertia_in_world;
601+
systems::CacheIndex composite_body_inertia_in_m;
594602
systems::CacheIndex composite_body_inertia_in_world;
595603
systems::CacheIndex spatial_acceleration_bias;
596604
systems::CacheIndex velocity_kinematics;

0 commit comments

Comments
 (0)