Skip to content

Commit a5116d7

Browse files
sherm1sherm1
committed
Added CompositeBodyInertiasInM and benchmark
Avoid divides; inline small methods.
1 parent 46f7589 commit a5116d7

14 files changed

+228
-138
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: 31 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -37,35 +37,46 @@ void BodyNode<T>::CalcCompositeBodyInertiaInWorld_TipToBase(
3737
}
3838
}
3939

40+
// Notation used below
41+
//
42+
// `this` is mobilized body B. It's mobilizer's inboard frame (the "M" frame) is
43+
// denoted Mb. We're given B's spatial inertia M_BMb_Mb (taken about Mb's origin
44+
// Mbo and expressed in Mb). Our goal is to calculate B's composite body inertia
45+
// I_BMb_Mb, taken about Mbo but including all the bodies outboard of B as
46+
// though they were welded in their current configuration.
47+
//
48+
// Below we're going to treat B as the "parent" and work with its immediately
49+
// outboard bodies as "children" C. Because this is an inward sweep, we
50+
// already know I_CMc_Mc, each child's composite body inertia, taken about
51+
// the child's inboard frame Mc's origin Mco, and expressed in Mc. We need to
52+
// shift those to Mbo and re-express in Mb before summing them up.
4053
template <typename T>
4154
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);
55+
const FrameBodyPoseCache<T>& frame_body_pose_cache, // for M_BMb_Mb
56+
const PositionKinematicsCacheInM<T>& pcm, // for X_MbMc(q)
57+
std::vector<SpatialInertia<T>>* I_BMb_Mb_all) const {
58+
const MobodIndex index = mobod_index();
59+
DRAKE_ASSERT(index != world_mobod_index());
60+
DRAKE_ASSERT(I_BMb_Mb_all != nullptr);
4761

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

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;
67+
I_BMb_Mb = M_BMb_Mb; // Start with B's spatial inertia.
68+
// Then add in each child's composite body inertia.
5869
for (const BodyNode<T>* child : child_nodes()) {
5970
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.
71+
const SpatialInertia<T>& I_CMc_Mc = // Child's composite body inertia.
72+
(*I_BMb_Mb_all)[child_node_index];
73+
// Body B is the parent here.
6474
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;
75+
// Shift the child's composite body inertia to B's Mb frame origin Mbo, but
76+
// still expressed in Mc (so Mx==Mc here).
77+
SpatialInertia<T> I_CMb_Mx = I_CMc_Mc.Shift(-X_MbMc.translation());
78+
I_CMb_Mx.ReExpressInPlace(X_MbMc.rotation()); // Now Mx==Mb
79+
I_BMb_Mb += I_CMb_Mx;
6980
}
7081
}
7182

‎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;

‎multibody/tree/rotational_inertia.h

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -328,7 +328,7 @@ class RotationalInertia {
328328
/// @see operator+().
329329
// TODO(Mitiguy) Issue #6145, add direct unit test for this method.
330330
RotationalInertia<T>& operator+=(const RotationalInertia<T>& I_BP_E) {
331-
this->get_mutable_triangular_view() += I_BP_E.get_matrix();
331+
this->get_mutable_triangular_view() += I_BP_E.get_matrix(); // 6 flops
332332
return *this;
333333
}
334334

@@ -341,7 +341,7 @@ class RotationalInertia {
341341
/// @return The sum of `this` rotational inertia and `I_BP_E`.
342342
/// @see operator+=().
343343
RotationalInertia<T> operator+(const RotationalInertia<T>& I_BP_E) const {
344-
return RotationalInertia(*this) += I_BP_E;
344+
return RotationalInertia(*this) += I_BP_E; // 6 flops
345345
}
346346

347347
/// Subtracts a rotational inertia `I_BP_E` from `this` rotational inertia.
@@ -453,7 +453,8 @@ class RotationalInertia {
453453
/// @see operator/().
454454
RotationalInertia<T>& operator/=(const T& positive_scalar) {
455455
DRAKE_ASSERT_VOID(ThrowIfDivideByZeroOrNegativeScalar(positive_scalar));
456-
this->get_mutable_triangular_view() /= positive_scalar;
456+
const T oo_positive_scalar = 1 / positive_scalar;
457+
this->get_mutable_triangular_view() *= oo_positive_scalar;
457458
return *this;
458459
}
459460

@@ -795,7 +796,7 @@ class RotationalInertia {
795796
/// @see operator-=().
796797
RotationalInertia<T>& MinusEqualsUnchecked(
797798
const RotationalInertia<T>& I_BP_E) {
798-
this->get_mutable_triangular_view() -= I_BP_E.get_matrix();
799+
this->get_mutable_triangular_view() -= I_BP_E.get_matrix(); // 6 flops
799800
return *this;
800801
}
801802

0 commit comments

Comments
 (0)