Skip to content

Commit

Permalink
Cache composite body inertias to eliminate temp allocation in CalcMas…
Browse files Browse the repository at this point in the history 
…sMatrix().

Modernize cache allocation code.
  • Loading branch information
@sherm1
sherm1 committed Aug 25, 2020
1 parent 59de1fd commit d42900c
Show file tree
Hide file tree
Showing 6 changed files with 223 additions and 194 deletions.
2 changes: 1 addition & 1 deletion examples/multibody/cassie_benchmark/test/cassie_bench.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ BENCHMARK_F(CassieDoubleFixture, DoubleMassMatrix)(benchmark::State& state) {
MatrixXd M(nv_, nv_);
for (auto _ : state) {
// @see LimitMalloc note above.
LimitMalloc guard(LimitReleaseOnly(175));
LimitMalloc guard(LimitReleaseOnly(174));
InvalidateState();
plant_->CalcMassMatrix(*context_, &M);
}
Expand Down
16 changes: 8 additions & 8 deletions multibody/tree/body_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1266,26 +1266,26 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
/// @param[in] M_B_W Spatial inertia for the body B associated with this node.
/// About B's origin Bo and expressed in the world frame W.
/// @param[in] pc Position kinematics cache.
/// @param[in] R_B_W_all Vector storing the composite body inertia for all
/// @param[in] Mc_B_W_all Vector storing the composite body inertia for all
/// bodies in the multibody system. It must contain already up-to-date
/// composite body inertias for all the children of `this` node.
/// @param[out] R_B_W The composite body inertia for `this` node. It must be
/// @param[out] Mc_B_W The composite body inertia for `this` node. It must be
/// non-nullptr.
/// @pre CalcCompositeBodyInertia_TipToBase() must have already been called
/// for the children nodes (and, by recursive precondition, all outboard nodes
/// in the tree.)
void CalcCompositeBodyInertia_TipToBase(
const SpatialInertia<T>& M_B_W,
const PositionKinematicsCache<T>& pc,
const std::vector<SpatialInertia<T>>& R_B_W_all,
SpatialInertia<T>* R_B_W) const {
const std::vector<SpatialInertia<T>>& Mc_B_W_all,
SpatialInertia<T>* Mc_B_W) const {
DRAKE_THROW_UNLESS(topology_.body != world_index());
DRAKE_THROW_UNLESS(R_B_W != nullptr);
DRAKE_THROW_UNLESS(Mc_B_W != nullptr);

// Composite body inertia R_B_W for this node B, about its frame's origin
// Bo, and expressed in the world frame W. Here we adopt the notation used
// in Jain's book.
*R_B_W = M_B_W;
*Mc_B_W = M_B_W;
// Add composite body inertia contributions from all children.
for (const BodyNode<T>* child : children_) {
// Shift vector p_CoBo_W.
Expand All @@ -1294,10 +1294,10 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {

// Composite body inertia for outboard child body C, about Co, expressed
// in W.
const SpatialInertia<T>& R_CCo_W = R_B_W_all[child->index()];
const SpatialInertia<T>& Mc_CCo_W = Mc_B_W_all[child->index()];

// Shift to Bo and add it to the composite body inertia of B.
*R_B_W += R_CCo_W.Shift(p_CoBo_W);
*Mc_B_W += Mc_CCo_W.Shift(p_CoBo_W);
}
}

Expand Down
90 changes: 54 additions & 36 deletions multibody/tree/multibody_tree.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -562,11 +562,11 @@ void MultibodyTree<T>::CalcVelocityKinematicsCache(
}

template <typename T>
void MultibodyTree<T>::CalcSpatialInertiaInWorldCache(
void MultibodyTree<T>::CalcSpatialInertiasInWorld(
const systems::Context<T>& context,
std::vector<SpatialInertia<T>>* M_B_W_cache) const {
DRAKE_THROW_UNLESS(M_B_W_cache != nullptr);
DRAKE_THROW_UNLESS(static_cast<int>(M_B_W_cache->size()) == num_bodies());
std::vector<SpatialInertia<T>>* M_B_W_all) const {
DRAKE_THROW_UNLESS(M_B_W_all != nullptr);
DRAKE_THROW_UNLESS(static_cast<int>(M_B_W_all->size()) == num_bodies());

const PositionKinematicsCache<T>& pc = this->EvalPositionKinematics(context);

Expand All @@ -582,15 +582,42 @@ void MultibodyTree<T>::CalcSpatialInertiaInWorldCache(
// This call has zero cost for rigid bodies.
const SpatialInertia<T> M_B = body.CalcSpatialInertiaInBodyFrame(context);
// Re-express body B's spatial inertia in the world frame W.
SpatialInertia<T>& M_B_W = (*M_B_W_cache)[body.node_index()];
SpatialInertia<T>& M_B_W = (*M_B_W_all)[body.node_index()];
M_B_W = M_B.ReExpress(R_WB);
}
}

template <typename T>
void MultibodyTree<T>::CalcSpatialAccelerationBiasCache(
void MultibodyTree<T>::CalcCompositeBodyInertiasInWorld(
const systems::Context<T>& context,
std::vector<SpatialAcceleration<T>>* Ab_WB_cache)
std::vector<SpatialInertia<T>>* Mc_B_W_all) const {
const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
const std::vector<SpatialInertia<T>>& M_B_W_all =
EvalSpatialInertiaInWorldCache(context);

// Perform tip-to-base recursion for each composite body, skipping the world.
for (int depth = tree_height() - 1; depth > 0; --depth) {
for (BodyNodeIndex composite_node_index : body_node_levels_[depth]) {
// Node corresponding to the composite body C.
const BodyNode<T>& composite_node = *body_nodes_[composite_node_index];

// This node's spatial inertia.
const SpatialInertia<T>& M_C_W = M_B_W_all[composite_node_index];

// Compute the spatial inertia Mc_C_W of the composite body C
// corresponding to the node with index composite_node_index. Computed
// about C's origin Co and expressed in the world frame W.
SpatialInertia<T>& Mc_C_W = (*Mc_B_W_all)[composite_node_index];
composite_node.CalcCompositeBodyInertia_TipToBase(M_C_W, pc, *Mc_B_W_all,
&Mc_C_W);
}
}
}

template <typename T>
void MultibodyTree<T>::CalcSpatialAccelerationBias(
const systems::Context<T>& context,
std::vector<SpatialAcceleration<T>>* Ab_WB_all)
const {
const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
const VelocityKinematicsCache<T>& vc = EvalVelocityKinematics(context);
Expand All @@ -599,21 +626,21 @@ void MultibodyTree<T>::CalcSpatialAccelerationBiasCache(
// For the world body we opted for leaving Ab_WB initialized to NaN so that
// an accidental usage (most likely indicating unnecessary math) in code would
// immediately trigger a trail of NaNs that we can track to the source.
(*Ab_WB_cache)[world_index()].SetNaN();
(*Ab_WB_all)[world_index()].SetNaN();
for (BodyNodeIndex body_node_index(1); body_node_index < num_bodies();
++body_node_index) {
const BodyNode<T>& node = *body_nodes_[body_node_index];
SpatialAcceleration<T>& Ab_WB = (*Ab_WB_cache)[body_node_index];
SpatialAcceleration<T>& Ab_WB = (*Ab_WB_all)[body_node_index];
node.CalcSpatialAccelerationBias(context, pc, vc, &Ab_WB);
}
}

template <typename T>
void MultibodyTree<T>::CalcArticulatedBodyForceBiasCache(
void MultibodyTree<T>::CalcArticulatedBodyForceBias(
const systems::Context<T>& context,
std::vector<SpatialForce<T>>* Zb_Bo_W_cache) const {
DRAKE_THROW_UNLESS(Zb_Bo_W_cache != nullptr);
DRAKE_THROW_UNLESS(static_cast<int>(Zb_Bo_W_cache->size()) == num_bodies());
std::vector<SpatialForce<T>>* Zb_Bo_W_all) const {
DRAKE_THROW_UNLESS(Zb_Bo_W_all != nullptr);
DRAKE_THROW_UNLESS(static_cast<int>(Zb_Bo_W_all->size()) == num_bodies());
const ArticulatedBodyInertiaCache<T>& abic =
EvalArticulatedBodyInertiaCache(context);
const std::vector<SpatialAcceleration<T>>& Ab_WB_cache =
Expand All @@ -623,23 +650,23 @@ void MultibodyTree<T>::CalcArticulatedBodyForceBiasCache(
// For the world body we opted for leaving Zb_Bo_W initialized to NaN so that
// an accidental usage (most likely indicating unnecessary math) in code would
// immediately trigger a trail of NaNs that we can track to the source.
(*Zb_Bo_W_cache)[world_index()].SetNaN();
(*Zb_Bo_W_all)[world_index()].SetNaN();
for (BodyNodeIndex body_node_index(1); body_node_index < num_bodies();
++body_node_index) {
const ArticulatedBodyInertia<T>& Pplus_PB_W =
abic.get_Pplus_PB_W(body_node_index);
const SpatialAcceleration<T>& Ab_WB = Ab_WB_cache[body_node_index];
SpatialForce<T>& Zb_Bo_W = (*Zb_Bo_W_cache)[body_node_index];
SpatialForce<T>& Zb_Bo_W = (*Zb_Bo_W_all)[body_node_index];
Zb_Bo_W = Pplus_PB_W * Ab_WB;
}
}

template <typename T>
void MultibodyTree<T>::CalcDynamicBiasCache(
void MultibodyTree<T>::CalcDynamicBiasForces(
const systems::Context<T>& context,
std::vector<SpatialForce<T>>* Fb_Bo_W_cache) const {
DRAKE_THROW_UNLESS(Fb_Bo_W_cache != nullptr);
DRAKE_THROW_UNLESS(static_cast<int>(Fb_Bo_W_cache->size()) == num_bodies());
std::vector<SpatialForce<T>>* Fb_Bo_W_all) const {
DRAKE_THROW_UNLESS(Fb_Bo_W_all != nullptr);
DRAKE_THROW_UNLESS(static_cast<int>(Fb_Bo_W_all->size()) == num_bodies());

const std::vector<SpatialInertia<T>>& spatial_inertia_in_world_cache =
EvalSpatialInertiaInWorldCache(context);
Expand All @@ -663,7 +690,7 @@ void MultibodyTree<T>::CalcDynamicBiasCache(
// W.
const SpatialVelocity<T>& V_WB = vc.get_V_WB(body.node_index());
const Vector3<T>& w_WB = V_WB.rotational();
SpatialForce<T>& Fb_Bo_W = (*Fb_Bo_W_cache)[body.node_index()];
SpatialForce<T>& Fb_Bo_W = (*Fb_Bo_W_all)[body.node_index()];
Fb_Bo_W = mass * SpatialForce<T>(
w_WB.cross(G_B_W * w_WB), /* rotational */
w_WB.cross(w_WB.cross(p_BoBcm_W)) /* translational */);
Expand Down Expand Up @@ -968,21 +995,20 @@ void MultibodyTree<T>::CalcMassMatrix(const systems::Context<T>& context,
// is the across-mobilizer Jacobian such that we can write
// V_PB_W = H_PB_W * v_B, with v_B the generalized velocities of body B's
// mobilizer.
// - In code we use the monogram notation R_C_W to denote the spatial inertia
// - In code we use the monogram notation Mc_C_W to denote the spatial inertia
// of composite body C, about it's frame origin Co, and expressed in the
// world frame W.
//
// - [Jain 2010] Jain, A., 2010. Robot and multibody dynamics: analysis and
// algorithms. Springer Science & Business Media, pp. 123-130.

const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
const std::vector<SpatialInertia<T>>& Mc_B_W_cache =
EvalCompositeBodyInertiaInWorldCache(context);
const std::vector<Vector6<T>>& H_PB_W_cache =
EvalAcrossNodeJacobianWrtVExpressedInWorld(context);
const std::vector<SpatialInertia<T>>& spatial_inertia_in_world_cache =
EvalSpatialInertiaInWorldCache(context);

// Temporary storage.
std::vector<SpatialInertia<T>> R_B_W_all(num_bodies());
Matrix6xUpTo6<T> Fm_CCo_W;

// The algorithm below does not recurse zero entries and therefore these must
Expand All @@ -995,16 +1021,8 @@ void MultibodyTree<T>::CalcMassMatrix(const systems::Context<T>& context,
// Node corresponding to the composite body C.
const BodyNode<T>& composite_node = *body_nodes_[composite_node_index];

// This node's spatial inertia.
const SpatialInertia<T>& M_C_W =
spatial_inertia_in_world_cache[composite_node_index];

// Compute the spatial inertia R_C_W of the composite body C corresponding
// to the node with index composite_node_index. Computed about C's origin
// Co and expressed in the world frame W.
SpatialInertia<T>& R_C_W = R_B_W_all[composite_node_index];
composite_node.CalcCompositeBodyInertia_TipToBase(M_C_W, pc, R_B_W_all,
&R_C_W);
// This node's composite body inertia.
const SpatialInertia<T>& Mc_C_W = Mc_B_W_cache[composite_node_index];

// Across-mobilizer hinge matrix, from C's parent Cp to C.
Eigen::Map<const MatrixUpTo6<T>> H_CpC_W =
Expand All @@ -1030,10 +1048,10 @@ void MultibodyTree<T>::CalcMassMatrix(const systems::Context<T>& context,

// If we consider the closed system composed of the composite body held by
// its mobilizer, the Newton-Euler equations state:
// Fm_CCo_W = R_C_W * A_WC + Fb_C_W
// Fm_CCo_W = Mc_C_W * A_WC + Fb_C_W
// where Fm_CCo_W is the spatial force at this node's mobilizer.
// Since the system is at rest, we have Fb_C_W = 0 and thus:
Fm_CCo_W = R_C_W * A_WC;
Fm_CCo_W = Mc_C_W * A_WC;

const int composite_start = composite_node.velocity_start();
const int composite_nv = composite_node.get_num_mobilizer_velocities();
Expand Down
56 changes: 39 additions & 17 deletions multibody/tree/multibody_tree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1477,35 +1477,51 @@ class MultibodyTree {
const PositionKinematicsCache<T>& pc,
VelocityKinematicsCache<T>* vc) const;

/// Computes the spatial inertia M_Bo_W(q) for each body B in the model about
/// Computes the spatial inertia M_B_W(q) for each body B in the model about
/// its frame origin Bo and expressed in the world frame W.
/// @param[in] context
/// The context storing the state of the model.
/// @param[out] M_B_W_cache
/// For each body in the model, entry Body::node_index() in M_B_W_cache
/// @param[out] M_B_W_all
/// For each body in the model, entry Body::node_index() in M_B_W_all
/// contains the updated spatial inertia `M_B_W(q)` for that body. On input
/// it must be a valid pointer to a vector of size num_bodies().
/// @throws std::exception if M_B_W_cache is nullptr or if its size is not
/// @throws std::exception if M_B_W_all is nullptr or if its size is not
/// num_bodies().
void CalcSpatialInertiaInWorldCache(
void CalcSpatialInertiasInWorld(
const systems::Context<T>& context,
std::vector<SpatialInertia<T>>* M_B_W_cache) const;
std::vector<SpatialInertia<T>>* M_B_W_all) const;

/// Computes the bias term `Fb_Bo_W(q, v)` for each body in the model.
/// Computes the composite body inertia Mc_B_W(q) for each body B in the
/// model about its frame origin Bo and expressed in the world frame W.
/// The composite body inertia is the effective mass properties B would have
/// if every joint outboard of B was welded in its current configuration.
/// @param[in] context
/// The context storing the state of the model.
/// @param[out] Mc_B_W_all
/// For each body in the model, entry Body::node_index() in M_B_W_all
/// contains the updated composite body inertia `Mc_B_W(q)` for that body.
/// On input it must be a valid pointer to a vector of size num_bodies().
/// @throws std::exception if Mc_B_W_all is nullptr or if its size is not
/// num_bodies().
void CalcCompositeBodyInertiasInWorld(
const systems::Context<T>& context,
std::vector<SpatialInertia<T>>* Mc_B_W_all) const;

/// Computes the bias force `Fb_Bo_W(q, v)` for each body in the model.
/// For a body B, this is the bias term `Fb_Bo_W` in the equation
/// `F_BBo_W = M_Bo_W * A_WB + Fb_Bo_W`, where `M_Bo_W` is the spatial inertia
/// about B's origin Bo, `A_WB` is the spatial acceleration of B in W and
/// `F_BBo_W` is the spatial force applied on B about Bo, expressed in W.
/// @param[in] context
/// The context storing the state of the model.
/// @param[out] Fb_Bo_W_cache
/// For each body in the model, entry Body::node_index() in Fb_Bo_W_cache
/// @param[out] Fb_Bo_W_all
/// For each body in the model, entry Body::node_index() in Fb_Bo_W_all
/// contains the updated bias term `Fb_Bo_W(q, v)` for that body. On input
/// it must be a valid pointer to a vector of size num_bodies().
/// @throws std::exception if Fb_Bo_W_cache is nullptr or if its size is not
/// num_bodies().
void CalcDynamicBiasCache(const systems::Context<T>& context,
std::vector<SpatialForce<T>>* Fb_Bo_W_cache) const;
void CalcDynamicBiasForces(const systems::Context<T>& context,
std::vector<SpatialForce<T>>* Fb_Bo_W_all) const;

/// Computes all the kinematic quantities that depend on the generalized
/// accelerations that is, the generalized velocities' time derivatives, and
Expand Down Expand Up @@ -2030,19 +2046,19 @@ class MultibodyTree {
/// Φᵀ(p_PB) * A_WP` the rigidly shifted spatial acceleration of the inboard
/// body P and `H_PB_W` and `vdot_B` its mobilizer's hinge matrix and
/// mobilities, respectively. See @ref abi_computing_accelerations for further
/// details. On output `Ab_WB_cache[body_node_index]`
/// details. On output `Ab_WB_all[body_node_index]`
/// contains `Ab_WB` for the body with node index `body_node_index`.
void CalcSpatialAccelerationBiasCache(
void CalcSpatialAccelerationBias(
const systems::Context<T>& context,
std::vector<SpatialAcceleration<T>>* Ab_WB_cache)
std::vector<SpatialAcceleration<T>>* Ab_WB_all)
const;

/// Computes the articulated body force bias `Zb_Bo_W = Pplus_PB_W * Ab_WB`
/// for each articulated body B. On output `Zb_Bo_W_cache[body_node_index]`
/// for each articulated body B. On output `Zb_Bo_W_all[body_node_index]`
/// contains `Zb_Bo_W` for the body B with node index `body_node_index`.
void CalcArticulatedBodyForceBiasCache(
void CalcArticulatedBodyForceBias(
const systems::Context<T>& context,
std::vector<SpatialForce<T>>* Zb_Bo_W_cache) const;
std::vector<SpatialForce<T>>* Zb_Bo_W_all) const;

/// @}

Expand Down Expand Up @@ -2621,6 +2637,12 @@ class MultibodyTree {
return tree_system_->EvalSpatialInertiaInWorldCache(context);
}

const std::vector<SpatialInertia<T>>& EvalCompositeBodyInertiaInWorldCache(
const systems::Context<T>& context) const {
DRAKE_ASSERT(tree_system_ != nullptr);
return tree_system_->EvalCompositeBodyInertiaInWorldCache(context);
}

// Evaluates the cache entry stored in context with the bias term
// Fb_Bo_W(q, v) for each body. These will be updated as needed.
const std::vector<SpatialForce<T>>& EvalDynamicBiasCache(
Expand Down
Loading

0 comments on commit d42900c

Please sign in to comment.