Drake
MultibodyTree< T > Class Template Reference

MultibodyTree provides a representation for a physical system consisting of a collection of interconnected rigid and deformable bodies. More...

#include <drake/multibody/multibody_tree/body.h>

Public Member Functions

 MultibodyTree ()
 Creates a MultibodyTree containing only a world body. More...
 
int get_num_frames () const
 Returns the number of Frame objects in the MultibodyTree. More...
 
int get_num_bodies () const
 Returns the number of bodies in the MultibodyTree including the world body. More...
 
int get_num_mobilizers () const
 Returns the number of mobilizers in the MultibodyTree. More...
 
int get_num_positions () const
 Returns the number of generalized positions of the model. More...
 
int get_num_velocities () const
 Returns the number of generalized velocities of the model. More...
 
int get_num_states () const
 Returns the total size of the state vector in the model. More...
 
int get_tree_height () const
 Returns the height of the tree data structure of this MultibodyTree. More...
 
const Body< T > & get_world_body () const
 Returns a constant reference to the world body. More...
 
const BodyFrame< T > & get_world_frame () const
 Returns a constant reference to the world frame. More...
 
const Body< T > & get_body (BodyIndex body_index) const
 Returns a constant reference to the body with unique index body_index. More...
 
const Frame< T > & get_frame (FrameIndex frame_index) const
 Returns a constant reference to the frame with unique index frame_index. More...
 
const Mobilizer< T > & get_mobilizer (MobilizerIndex mobilizer_index) const
 Returns a constant reference to the mobilizer with unique index mobilizer_index. More...
 
bool topology_is_valid () const
 Returns true if this MultibodyTree was finalized with Finalize() after all multibody elements were added, and false otherwise. More...
 
const MultibodyTreeTopologyget_topology () const
 Returns the topology information for this multibody tree. More...
 
void Finalize ()
 This method must be called after all elements in the tree (joints, bodies, force elements, constraints) were added and before any computations are performed. More...
 
std::unique_ptr< systems::LeafContext< T > > CreateDefaultContext () const
 Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system. More...
 
void SetDefaults (systems::Context< T > *context) const
 Sets default values in the context. More...
 
void CalcPositionKinematicsCache (const systems::Context< T > &context, PositionKinematicsCache< T > *pc) const
 Computes into the position kinematics pc all the kinematic quantities that depend on the generalized positions only. More...
 
void CalcVelocityKinematicsCache (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, VelocityKinematicsCache< T > *vc) const
 Computes all the kinematic quantities that depend on the generalized velocities and stores them in the velocity kinematics cache vc. More...
 
void CalcAccelerationKinematicsCache (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, AccelerationKinematicsCache< T > *ac) const
 Computes all the kinematic quantities that depend on the generalized accelerations that is, the generalized velocities' time derivatives, and stores them in the acceleration kinematics cache ac. More...
 
void CalcSpatialAccelerationsFromVdot (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, std::vector< SpatialAcceleration< T >> *A_WB_array) const
 Given the state of this MultibodyTree in context and a known vector of generalized accelerations known_vdot, this method computes the spatial acceleration A_WB for each body as measured and expressed in the world frame W. More...
 
void CalcInverseDynamics (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, const std::vector< SpatialForce< T >> &Fapplied_Bo_W_array, const Eigen::Ref< const VectorX< T >> &tau_applied_array, std::vector< SpatialAcceleration< T >> *A_WB_array, std::vector< SpatialForce< T >> *F_BMo_W_array, EigenPtr< VectorX< T >> tau_array) const
 Given the state of this MultibodyTree in context and a known vector of generalized accelerations vdot, this method computes the set of generalized forces tau that would need to be applied at each Mobilizer in order to attain the specified generalized accelerations. More...
 
void CalcMassMatrixViaInverseDynamics (const systems::Context< T > &context, EigenPtr< MatrixX< T >> H) const
 Performs the computation of the mass matrix M(q) of the model using inverse dynamics, where the generalized positions q are stored in context. More...
 
void CalcBiasTerm (const systems::Context< T > &context, EigenPtr< VectorX< T >> Cv) const
 Computes the bias term C(q, v)v containing Coriolis and gyroscopic effects of the multibody equations of motion: More...
 
std::unique_ptr< MultibodyTree< T > > Clone () const
 Creates a deep copy of this MultibodyTree templated on the same scalar type T as this tree. More...
 
std::unique_ptr< MultibodyTree< AutoDiffXd > > ToAutoDiffXd () const
 Creates a deep copy of this MultibodyTree templated on AutoDiffXd. More...
 
template<typename ToScalar >
std::unique_ptr< MultibodyTree< ToScalar > > CloneToScalar () const
 Creates a deep copy of this MultibodyTree templated on the scalar type ToScalar. More...
 
Does not allow copy, move, or assignment
 MultibodyTree (const MultibodyTree &)=delete
 
MultibodyTreeoperator= (const MultibodyTree &)=delete
 
 MultibodyTree (MultibodyTree &&)=delete
 
MultibodyTreeoperator= (MultibodyTree &&)=delete
 
Methods to add new MultibodyTree elements.

To create a MultibodyTree users will add multibody elements like bodies, joints, force elements, constraints, etc, using one of these methods.

Once a user is done adding multibody elements, the Finalize() method must be called before invoking any MultibodyTree method. See Finalize() for details.

template<template< typename Scalar > class BodyType>
const BodyType< T > & AddBody (std::unique_ptr< BodyType< T >> body)
 Takes ownership of body and adds it to this MultibodyTree. More...
 
template<template< typename Scalar > class BodyType, typename... Args>
const BodyType< T > & AddBody (Args &&...args)
 Constructs a new body with type BodyType with the given args, and adds it to this MultibodyTree, which retains ownership. More...
 
template<template< typename Scalar > class FrameType>
const FrameType< T > & AddFrame (std::unique_ptr< FrameType< T >> frame)
 Takes ownership of frame and adds it to this MultibodyTree. More...
 
template<template< typename Scalar > class FrameType, typename... Args>
const FrameType< T > & AddFrame (Args &&...args)
 Constructs a new frame with type FrameType with the given args, and adds it to this MultibodyTree, which retains ownership. More...
 
template<template< typename Scalar > class MobilizerType>
const MobilizerType< T > & AddMobilizer (std::unique_ptr< MobilizerType< T >> mobilizer)
 Takes ownership of mobilizer and adds it to this MultibodyTree. More...
 
template<template< typename Scalar > class MobilizerType, typename... Args>
const MobilizerType< T > & AddMobilizer (Args &&...args)
 Constructs a new mobilizer with type MobilizerType with the given args, and adds it to this MultibodyTree, which retains ownership. More...
 
Methods to retrieve multibody element variants

Given two variants of the same MultibodyTree, these methods map an element in one variant, to its corresponding element in the other variant.

A concrete case is the call to ToAutoDiffXd() to obtain a MultibodyTree variant templated on AutoDiffXd from a MultibodyTree templated on double. Typically, a user holding a Body<double> (or any other multibody element in the original variant templated on double) would like to retrieve the corresponding Body<AutoDiffXd> variant from the new AutoDiffXd tree variant.

Consider the following code example:

// The user creates a model.
MultibodyTree<double> model;
// User adds a body and keeps a reference to it.
const RigidBody<double>& body = model.AddBody<RigidBody>(...);
// User creates an AutoDiffXd variant. Variants on other scalar types
// can be created with a call to CloneToScalar().
std::unique_ptr<MultibodyTree<Tvariant>> variant_model =
model.ToAutoDiffXd();
// User retrieves the AutoDiffXd variant corresponding to the original
// body added above.
variant_body = variant_model.get_variant(body);

MultibodyTree::get_variant() is templated on the multibody element type which is deduced from its only input argument. The returned element is templated on the scalar type T of the MultibodyTree on which this method is invoked.

template<template< typename > class MultibodyElement, typename Scalar >
std::enable_if_t< std::is_base_of< Frame< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant (const MultibodyElement< Scalar > &element) const
 SFINAE overload for Frame<T> elements. More...
 
template<template< typename > class MultibodyElement, typename Scalar >
std::enable_if_t< std::is_base_of< Body< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant (const MultibodyElement< Scalar > &element) const
 SFINAE overload for Body<T> elements. More...
 
template<template< typename > class MultibodyElement, typename Scalar >
std::enable_if_t< std::is_base_of< Mobilizer< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant (const MultibodyElement< Scalar > &element) const
 SFINAE overload for Mobilizer<T> elements. More...
 

Friends

template<typename >
class MultibodyTree
 

Detailed Description

template<typename T>
class drake::multibody::MultibodyTree< T >

MultibodyTree provides a representation for a physical system consisting of a collection of interconnected rigid and deformable bodies.

As such, it owns and manages each of the elements that belong to this physical system. Multibody dynamics elements include bodies, joints, force elements and constraints.

Template Parameters
TThe scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

  • double
  • AutoDiffXd

They are already available to link against in the containing library. No other values for T are currently supported.

Constructor & Destructor Documentation

MultibodyTree ( const MultibodyTree< T > &  )
delete
MultibodyTree ( MultibodyTree< T > &&  )
delete

Creates a MultibodyTree containing only a world body.

Member Function Documentation

const BodyType<T>& AddBody ( std::unique_ptr< BodyType< T >>  body)
inline

Takes ownership of body and adds it to this MultibodyTree.

Returns a constant reference to the body just added, which will remain valid for the lifetime of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Code to define spatial_inertia, a SpatialInertia<T> object ...
const RigidBody<T>& body =
model.AddBody(std::make_unique<RigidBody<T>>(spatial_inertia));
Exceptions
std::logic_errorif body is a nullptr.
std::logic_errorif Finalize() was already called on this tree.
Parameters
[in]bodyA unique pointer to a body to add to this MultibodyTree. The body class must be specialized on the same scalar type T as this MultibodyTree.
Returns
A constant reference of type BodyType to the created body. This reference which will remain valid for the lifetime of this MultibodyTree.
Template Parameters
BodyTypeThe type of the specific sub-class of Body to add. The template needs to be specialized on the same scalar type T of this MultibodyTree.

Here is the caller graph for this function:

const BodyType<T>& AddBody ( Args &&...  args)
inline

Constructs a new body with type BodyType with the given args, and adds it to this MultibodyTree, which retains ownership.

The BodyType will be specialized on the scalar type T of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Code to define spatial_inertia, a SpatialInertia<T> object ...
// Notice RigidBody is a template on a scalar type.
const RigidBody<T>& body = model.AddBody<RigidBody>(spatial_inertia);

Note that for dependent names you must use the template keyword (say for instance you have a MultibodyTree<T> member within your custom class):

MultibodyTree<T> model;
auto body = model.template AddBody<RigidBody>(Args...);
Exceptions
std::logic_errorif Finalize() was already called on this tree.
Parameters
[in]argsThe arguments needed to construct a valid Body of type BodyType. BodyType must provide a public constructor that takes these arguments.
Returns
A constant reference of type BodyType to the created body. This reference which will remain valid for the lifetime of this MultibodyTree.
Template Parameters
BodyTypeA template for the type of Body to construct. The template will be specialized on the scalar type T of this MultibodyTree.
const FrameType<T>& AddFrame ( std::unique_ptr< FrameType< T >>  frame)
inline

Takes ownership of frame and adds it to this MultibodyTree.

Returns a constant reference to the frame just added, which will remain valid for the lifetime of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Define body and X_BF ...
const FixedOffsetFrame<T>& frame =
model.AddFrame(std::make_unique<FixedOffsetFrame<T>>(body, X_BF));
Exceptions
std::logic_errorif frame is a nullptr.
std::logic_errorif Finalize() was already called on this tree.
Parameters
[in]frameA unique pointer to a frame to be added to this MultibodyTree. The frame class must be specialized on the same scalar type T as this MultibodyTree.
Returns
A constant reference of type FrameType to the created frame. This reference which will remain valid for the lifetime of this MultibodyTree.
Template Parameters
FrameTypeThe type of the specific sub-class of Frame to add. The template needs to be specialized on the same scalar type T of this MultibodyTree.

Here is the caller graph for this function:

const FrameType<T>& AddFrame ( Args &&...  args)
inline

Constructs a new frame with type FrameType with the given args, and adds it to this MultibodyTree, which retains ownership.

The FrameType will be specialized on the scalar type T of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Define body and X_BF ...
// Notice FixedOffsetFrame is a template an a scalar type.
const FixedOffsetFrame<T>& frame =
model.AddFrame<FixedOffsetFrame>(body, X_BF);

Note that for dependent names you must use the template keyword (say for instance you have a MultibodyTree<T> member within your custom class):

MultibodyTree<T> model;
// ... Define body and X_BF ...
const auto& frame =
model.template AddFrame<FixedOffsetFrame>(body, X_BF);
Exceptions
std::logic_errorif Finalize() was already called on this tree.
Parameters
[in]argsThe arguments needed to construct a valid Frame of type FrameType. FrameType must provide a public constructor that takes these arguments.
Returns
A constant reference of type FrameType to the created frame. This reference which will remain valid for the lifetime of this MultibodyTree.
Template Parameters
FrameTypeA template for the type of Frame to construct. The template will be specialized on the scalar type T of this MultibodyTree.
const MobilizerType<T>& AddMobilizer ( std::unique_ptr< MobilizerType< T >>  mobilizer)
inline

Takes ownership of mobilizer and adds it to this MultibodyTree.

Returns a constant reference to the mobilizer just added, which will remain valid for the lifetime of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Code to define inboard and outboard frames by calling
// MultibodyTree::AddFrame() ...
const RevoluteMobilizer<T>& pin =
model.AddMobilizer(std::make_unique<RevoluteMobilizer<T>>(
inboard_frame, elbow_outboard_frame,
Vector3d::UnitZ() /*revolute axis*/));

A Mobilizer effectively connects the two bodies to which the inboard and outboard frames belong.

Exceptions
std::logic_errorif mobilizer is a nullptr.
std::logic_errorif Finalize() was already called on this tree.
astd::runtime_error if the new mobilizer attempts to connect a frame with itself.
std::runtime_errorif attempting to connect two bodies with more than one mobilizer between them.
Parameters
[in]mobilizerA unique pointer to a mobilizer to add to this MultibodyTree. The mobilizer class must be specialized on the same scalar type T as this MultibodyTree. Notice this is a requirement of this method's signature and therefore an input mobilzer specialized on a different scalar type than that of this MultibodyTree's T will fail to compile.
Returns
A constant reference of type MobilizerType to the created mobilizer. This reference which will remain valid for the lifetime of this MultibodyTree.
Template Parameters
MobilizerTypeThe type of the specific sub-class of Mobilizer to add. The template needs to be specialized on the same scalar type T of this MultibodyTree.

Here is the caller graph for this function:

const MobilizerType<T>& AddMobilizer ( Args &&...  args)
inline

Constructs a new mobilizer with type MobilizerType with the given args, and adds it to this MultibodyTree, which retains ownership.

The MobilizerType will be specialized on the scalar type T of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Code to define inboard and outboard frames by calling
// MultibodyTree::AddFrame() ...
// Notice RevoluteMobilizer is a template an a scalar type.
const RevoluteMobilizer<T>& pin =
model.template AddMobilizer<RevoluteMobilizer>(
inboard_frame, outboard_frame,
Vector3d::UnitZ() /*revolute axis*/);

Note that for dependent names only you must use the template keyword (say for instance you have a MultibodyTree<T> member within your custom class).

Exceptions
std::logic_errorif Finalize() was already called on this tree.
astd::runtime_error if the new mobilizer attempts to connect a frame with itself.
std::runtime_errorif attempting to connect two bodies with more than one mobilizer between them.
Parameters
[in]argsThe arguments needed to construct a valid Mobilizer of type MobilizerType. MobilizerType must provide a public constructor that takes these arguments.
Returns
A constant reference of type MobilizerType to the created mobilizer. This reference which will remain valid for the lifetime of this MultibodyTree.
Template Parameters
MobilizerTypeA template for the type of Mobilizer to construct. The template will be specialized on the scalar type T of this MultibodyTree.
void CalcAccelerationKinematicsCache ( const systems::Context< T > &  context,
const PositionKinematicsCache< T > &  pc,
const VelocityKinematicsCache< T > &  vc,
const VectorX< T > &  known_vdot,
AccelerationKinematicsCache< T > *  ac 
) const

Computes all the kinematic quantities that depend on the generalized accelerations that is, the generalized velocities' time derivatives, and stores them in the acceleration kinematics cache ac.

These include:

  • Spatial acceleration A_WB for each body B in the model as measured and expressed in the world frame W.
Parameters
[in]contextThe context containing the state of the MultibodyTree model.
[in]pcA position kinematics cache object already updated to be in sync with context.
[in]vcA velocity kinematics cache object already updated to be in sync with context.
[in]known_vdotA vector with the generalized accelerations for the full MultibodyTree model.
[out]acA pointer to a valid, non nullptr, acceleration kinematics cache. This method aborts if ac is nullptr.
Precondition
The position kinematics pc must have been previously updated with a call to CalcPositionKinematicsCache().
The velocity kinematics vc must have been previously updated with a call to CalcVelocityKinematicsCache().
Exceptions
std::bad_castif context is not a MultibodyTreeContext.

Here is the call graph for this function:

Here is the caller graph for this function:

void CalcBiasTerm ( const systems::Context< T > &  context,
EigenPtr< VectorX< T >>  Cv 
) const

Computes the bias term C(q, v)v containing Coriolis and gyroscopic effects of the multibody equations of motion:

  M(q)v̇ + C(q, v)v = tau_app + ∑ J_WBᵀ(q) Fapp_Bo_W

where M(q) is the multibody model's mass matrix and tau_app consists of a vector applied generalized forces. The last term is a summation over all bodies in the model where Fapp_Bo_W is an applied spatial force on body B at Bo which gets projected into the space of generalized forces with the geometric Jacobian J_WB(q) which maps generalized velocities into body B spatial velocity as V_WB = J_WB(q)v.

Parameters
[in]contextThe context containing the state of the MultibodyTree model. It stores the generalized positions q and the generalized velocities v.
[out]CvOn output, Cv will contain the product C(q, v)v. It must be a valid (non-null) pointer to a column vector in ℛⁿ with n the number of generalized velocities (get_num_velocities()) of the model. This method aborts if Cv is nullptr or if it does not have the proper size.

Here is the caller graph for this function:

void CalcInverseDynamics ( const systems::Context< T > &  context,
const PositionKinematicsCache< T > &  pc,
const VelocityKinematicsCache< T > &  vc,
const VectorX< T > &  known_vdot,
const std::vector< SpatialForce< T >> &  Fapplied_Bo_W_array,
const Eigen::Ref< const VectorX< T >> &  tau_applied_array,
std::vector< SpatialAcceleration< T >> *  A_WB_array,
std::vector< SpatialForce< T >> *  F_BMo_W_array,
EigenPtr< VectorX< T >>  tau_array 
) const

Given the state of this MultibodyTree in context and a known vector of generalized accelerations vdot, this method computes the set of generalized forces tau that would need to be applied at each Mobilizer in order to attain the specified generalized accelerations.

Mathematically, this method computes:

  tau = M(q)v̇ + C(q, v)v - tau_app - ∑ J_WBᵀ(q) Fapp_Bo_W

where M(q) is the MultibodyTree mass matrix, C(q, v)v is the bias term containing Coriolis and gyroscopic effects and tau_app consists of a vector applied generalized forces. The last term is a summation over all bodies in the model where Fapp_Bo_W is an applied spatial force on body B at Bo which gets projected into the space of generalized forces with the geometric Jacobian J_WB(q) which maps generalized velocities into body B spatial velocity as V_WB = J_WB(q)v. This method does not compute explicit expressions for the mass matrix nor for the bias term, which would be of at least O(n²) complexity, but it implements an O(n) Newton-Euler recursive algorithm, where n is the number of bodies in the MultibodyTree. The explicit formation of the mass matrix M(q) would require the calculation of O(n²) entries while explicitly forming the product C(q, v) * v could require up to O(n³) operations (see [Featherstone 1987, §4]), depending on the implementation. The recursive Newton-Euler algorithm is the most efficient currently known general method for solving inverse dynamics [Featherstone 2008].

Parameters
[in]contextThe context containing the state of the MultibodyTree model.
[in]pcA position kinematics cache object already updated to be in sync with context.
[in]vcA velocity kinematics cache object already updated to be in sync with context.
[in]known_vdotA vector with the known generalized accelerations vdot for the full MultibodyTree model. Use Mobilizer::get_accelerations_from_array() to access entries into this array for a particular Mobilizer. You can use the mutable version of this method to write into this array.
[in]Fapplied_Bo_W_arrayA vector containing the spatial force Fapplied_Bo_W applied on each body at the body's frame origin Bo and expressed in the world frame W. Fapplied_Bo_W_array can have zero size which means there are no applied forces. To apply non-zero forces, Fapplied_Bo_W_array must be of size equal to the number of bodies in this MultibodyTree model. This array must be ordered by BodyNodeIndex, which for a given body can be retrieved with Body::get_node_index(). This method will abort if provided with an array that does not have a size of either get_num_bodies() or zero.
[in]tau_applied_arrayAn array of applied generalized forces for the entire model. For a given mobilizer, entries in this array can be accessed using the method Mobilizer::get_generalized_forces_from_array() while its mutable counterpart, Mobilizer::get_mutable_generalized_forces_from_array(), allows writing into this array. tau_applied_array can have zero size, which means there are no applied forces. To apply non-zero forces, tau_applied_array must be of size equal to the number to the number of generalized velocities in the model, see MultibodyTree::get_num_velocities(). This method will abort if provided with an array that does not have a size of either MultibodyTree::get_num_velocities() or zero.
[out]A_WB_arrayA pointer to a valid, non nullptr, vector of spatial accelerations containing the spatial acceleration A_WB for each body. It must be of size equal to the number of bodies. This method will abort if the the pointer is null or if A_WB_array is not of size get_num_bodies(). On output, entries will be ordered by BodyNodeIndex. To access the acceleration A_WB of given body B in this array, use the index returned by Body::get_node_index().
[out]F_BMo_W_arrayA pointer to a valid, non nullptr, vector of spatial forces containing, for each body B, the spatial force F_BMo_W corresponding to its inboard mobilizer reaction forces on body B applied at the origin Mo of the inboard mobilizer, expressed in the world frame W. It must be of size equal to the number of bodies in the MultibodyTree. This method will abort if the the pointer is null or if F_BMo_W_array is not of size get_num_bodies(). On output, entries will be ordered by BodyNodeIndex. To access a mobilizer's reaction force on given body B in this array, use the index returned by Body::get_node_index().
[out]tau_arrayOn output this array will contain the generalized forces that must be applied in order to achieve the desired generalized accelerations given by the input argument known_vdot. It must not be nullptr and it must be of size MultibodyTree::get_num_velocities(). Generalized forces for each Mobilizer can be accessed with Mobilizer::get_generalized_forces_from_array().
Warning
There is no mechanism to assert that either A_WB_array nor F_BMo_W_array are ordered by BodyNodeIndex. You can use Body::get_node_index() to obtain the node index for a given body.
Note
This method uses F_BMo_W_array and tau_array as the only local temporaries and therefore no additional dynamic memory allocation is performed.
Warning
F_BMo_W_array (tau_array) and Fapplied_Bo_W_array (tau_applied_array) can actually be the same array in order to reduce memory footprint and/or dynamic memory allocations. However the information in Fapplied_Bo_W_array (tau_applied_array) would be overwritten through F_BMo_W_array (tau_array). Make a copy if data must be preserved.
Precondition
The position kinematics pc must have been previously updated with a call to CalcPositionKinematicsCache().
The velocity kinematics vc must have been previously updated with a call to CalcVelocityKinematicsCache().
Exceptions
std::bad_castif context is not a MultibodyTreeContext.

Here is the call graph for this function:

Here is the caller graph for this function:

void CalcMassMatrixViaInverseDynamics ( const systems::Context< T > &  context,
EigenPtr< MatrixX< T >>  H 
) const

Performs the computation of the mass matrix M(q) of the model using inverse dynamics, where the generalized positions q are stored in context.

See CalcInverseDynamics().

Parameters
[in]contextThe context containing the state of the MultibodyTree model.
[out]HA valid (non-null) pointer to a squared matrix in ℛⁿˣⁿ with n the number of generalized velocities (get_num_velocities()) of the model. This method aborts if H is nullptr or if it does not have the proper size.

The algorithm used to build M(q) consists in computing one column of M(q) at a time using inverse dynamics. The result from inverse dynamics, with no applied forces, is the vector of generalized forces:

  tau = M(q)v̇ + C(q, v)v

where q and v are the generalized positions and velocities, respectively. When v = 0 the Coriolis and gyroscopic forces term C(q, v)v is zero. Therefore the i-th column of M(q) can be obtained performing inverse dynamics with an acceleration vector v̇ = eᵢ, with eᵢ the standard (or natural) basis of ℛⁿ with n the number of generalized velocities. We write this as:

  H.ᵢ(q) = M(q) * e_i

where H.ᵢ(q) (notice the dot for the rows index) denotes the i-th column in M(q).

Warning
This is an O(n²) algorithm. Avoid the explicit computation of the mass matrix whenever possible.

Here is the call graph for this function:

Here is the caller graph for this function:

void CalcPositionKinematicsCache ( const systems::Context< T > &  context,
PositionKinematicsCache< T > *  pc 
) const

Computes into the position kinematics pc all the kinematic quantities that depend on the generalized positions only.

These include:

  • For each body B, the pose X_BF of each of the frames F attached to body B.
  • Pose X_WB of each body B in the model as measured and expressed in the world frame W.
  • Across-mobilizer Jacobian matrices H_FM and H_PB_W.
  • Body specific quantities such as com_W and M_Bo_W.
Exceptions
std::bad_castif context is not a MultibodyTreeContext. Aborts if pc is nullptr.

Here is the caller graph for this function:

void CalcSpatialAccelerationsFromVdot ( const systems::Context< T > &  context,
const PositionKinematicsCache< T > &  pc,
const VelocityKinematicsCache< T > &  vc,
const VectorX< T > &  known_vdot,
std::vector< SpatialAcceleration< T >> *  A_WB_array 
) const

Given the state of this MultibodyTree in context and a known vector of generalized accelerations known_vdot, this method computes the spatial acceleration A_WB for each body as measured and expressed in the world frame W.

Parameters
[in]contextThe context containing the state of the MultibodyTree model.
[in]pcA position kinematics cache object already updated to be in sync with context.
[in]vcA velocity kinematics cache object already updated to be in sync with context.
[in]known_vdotA vector with the generalized accelerations for the full MultibodyTree model.
[out]A_WB_arrayA pointer to a valid, non nullptr, vector of spatial accelerations containing the spatial acceleration A_WB for each body. It must be of size equal to the number of bodies in the MultibodyTree. This method will abort if the the pointer is null or if A_WB_array is not of size get_num_bodies(). On output, entries will be ordered by BodyNodeIndex. These accelerations can be read in the proper order with Body::get_from_spatial_acceleration_array().
Precondition
The position kinematics pc must have been previously updated with a call to CalcPositionKinematicsCache().
The velocity kinematics vc must have been previously updated with a call to CalcVelocityKinematicsCache().
Exceptions
std::bad_castif context is not a MultibodyTreeContext.

Here is the call graph for this function:

Here is the caller graph for this function:

void CalcVelocityKinematicsCache ( const systems::Context< T > &  context,
const PositionKinematicsCache< T > &  pc,
VelocityKinematicsCache< T > *  vc 
) const

Computes all the kinematic quantities that depend on the generalized velocities and stores them in the velocity kinematics cache vc.

These include:

  • Spatial velocity V_WB for each body B in the model as measured and expressed in the world frame W.
  • Spatial velocity V_PB for each body B in the model as measured and expressed in the inboard (or parent) body frame P.
Precondition
The position kinematics pc must have been previously updated with a call to CalcPositionKinematicsCache().
Exceptions
std::bad_castif context is not a MultibodyTreeContext. Aborts if vc is nullptr.

Here is the caller graph for this function:

std::unique_ptr<MultibodyTree<T> > Clone ( ) const
inline

Creates a deep copy of this MultibodyTree templated on the same scalar type T as this tree.

std::unique_ptr<MultibodyTree<ToScalar> > CloneToScalar ( ) const
inline

Creates a deep copy of this MultibodyTree templated on the scalar type ToScalar.

The new deep copy is guaranteed to have exactly the same MultibodyTreeTopology as the original tree the method is called on. This method ensures the following cloning order:

  • Body objects, and their corresponding BodyFrame objects.
  • Frame objects.
  • If a Frame is attached to another frame, its parent frame is guaranteed to be created first.
  • Mobilizer objects are created last and therefore clones of the original Frame objects are guaranteed to already be part of the cloned tree.
    Precondition
    Finalize() must have already been called on this MultibodyTree.
std::unique_ptr< systems::LeafContext< T > > CreateDefaultContext ( ) const

Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system.

Precondition
The method Finalize() must be called before attempting to create a context in order for the MultibodyTree topology to be valid at the moment of allocation.
Exceptions
std::logic_errorIf users attempt to call this method on a MultibodyTree with an invalid topology.

Here is the caller graph for this function:

void Finalize ( )

This method must be called after all elements in the tree (joints, bodies, force elements, constraints) were added and before any computations are performed.

It essentially compiles all the necessary "topological information", i.e. how bodies, joints and, any other elements connect with each other, and performs all the required pre-processing to perform computations at a later stage.

If the finalize stage is successful, the topology of this MultibodyTree is validated, meaning that the topology is up-to-date after this call. No more multibody tree elements can be added after a call to Finalize().

Exceptions
std::logic_errorIf users attempt to call this method on an already finalized MultibodyTree.

Here is the call graph for this function:

Here is the caller graph for this function:

const Body<T>& get_body ( BodyIndex  body_index) const
inline

Returns a constant reference to the body with unique index body_index.

This method aborts in Debug builds when body_index does not correspond to a body in this multibody tree.

Here is the caller graph for this function:

const Frame<T>& get_frame ( FrameIndex  frame_index) const
inline

Returns a constant reference to the frame with unique index frame_index.

This method aborts in Debug builds when frame_index does not correspond to a frame in this multibody tree.

const Mobilizer<T>& get_mobilizer ( MobilizerIndex  mobilizer_index) const
inline

Returns a constant reference to the mobilizer with unique index mobilizer_index.

This method aborts in Debug builds when mobilizer_index does not correspond to a mobilizer in this multibody tree.

int get_num_bodies ( ) const
inline

Returns the number of bodies in the MultibodyTree including the world body.

Therefore the minimum number of bodies in a MultibodyTree is one.

Here is the caller graph for this function:

int get_num_frames ( ) const
inline

Returns the number of Frame objects in the MultibodyTree.

Frames include body frames associated with each of the bodies in the MultibodyTree including the world body. Therefore the minimum number of frames in a MultibodyTree is one.

Here is the caller graph for this function:

int get_num_mobilizers ( ) const
inline

Returns the number of mobilizers in the MultibodyTree.

Since the world has no Mobilizer, the number of mobilizers equals the number of bodies minus one, i.e. get_num_mobilizers() returns get_num_bodies() - 1.

Here is the caller graph for this function:

int get_num_positions ( ) const
inline

Returns the number of generalized positions of the model.

int get_num_states ( ) const
inline

Returns the total size of the state vector in the model.

int get_num_velocities ( ) const
inline

Returns the number of generalized velocities of the model.

const MultibodyTreeTopology& get_topology ( ) const
inline

Returns the topology information for this multibody tree.

Users should not need to call this method since MultibodyTreeTopology is an internal bookkeeping detail. Used at Finalize() stage by multibody elements to retrieve a local copy of their topology.

int get_tree_height ( ) const
inline

Returns the height of the tree data structure of this MultibodyTree.

That is, the number of bodies in the longest kinematic path between the world and any other leaf body. For a model that only contains the world body, the height of the tree is one. Kinematic paths are created by Mobilizer objects connecting a chain of frames. Therefore, this method does not count kinematic cycles, which could only be considered in the model using constraints.

std::enable_if_t<std::is_base_of<Frame<T>, MultibodyElement<T> >::value, const MultibodyElement<T>&> get_variant ( const MultibodyElement< Scalar > &  element) const
inline

SFINAE overload for Frame<T> elements.

Here is the caller graph for this function:

std::enable_if_t<std::is_base_of<Body<T>, MultibodyElement<T> >::value, const MultibodyElement<T>&> get_variant ( const MultibodyElement< Scalar > &  element) const
inline

SFINAE overload for Body<T> elements.

std::enable_if_t<std::is_base_of<Mobilizer<T>, MultibodyElement<T> >::value, const MultibodyElement<T>&> get_variant ( const MultibodyElement< Scalar > &  element) const
inline

SFINAE overload for Mobilizer<T> elements.

const Body<T>& get_world_body ( ) const
inline

Returns a constant reference to the world body.

const BodyFrame<T>& get_world_frame ( ) const
inline

Returns a constant reference to the world frame.

MultibodyTree& operator= ( MultibodyTree< T > &&  )
delete
MultibodyTree& operator= ( const MultibodyTree< T > &  )
delete
void SetDefaults ( systems::Context< T > *  context) const

Sets default values in the context.

For mobilizers, this method sets them to their zero configuration according to Mobilizer::set_zero_configuration().

Here is the caller graph for this function:

std::unique_ptr<MultibodyTree<AutoDiffXd> > ToAutoDiffXd ( ) const
inline

Creates a deep copy of this MultibodyTree templated on AutoDiffXd.

bool topology_is_valid ( ) const
inline

Returns true if this MultibodyTree was finalized with Finalize() after all multibody elements were added, and false otherwise.

When a MultibodyTree is instantiated, its topology remains invalid until Finalize() is called, which validates the topology.

See also
Finalize().

Here is the caller graph for this function:

Friends And Related Function Documentation

friend class MultibodyTree
friend

The documentation for this class was generated from the following files: