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/tree/rigid_body.h>

Public Member Functions

 MultibodyTree ()
 Creates a MultibodyTree containing only a world body. More...
 
int num_frames () const
 Returns the number of Frame objects in the MultibodyTree. More...
 
int num_bodies () const
 Returns the number of bodies in the MultibodyTree including the world body. More...
 
int num_joints () const
 Returns the number of joints added with AddJoint() to the MultibodyTree. More...
 
int num_actuators () const
 Returns the number of actuators in the model. More...
 
int num_mobilizers () const
 Returns the number of mobilizers in the MultibodyTree. More...
 
int num_force_elements () const
 Returns the number of ForceElement objects in the MultibodyTree. More...
 
int num_model_instances () const
 Returns the number of model instances in the MultibodyTree. More...
 
int num_positions () const
 Returns the number of generalized positions of the model. More...
 
int num_positions (ModelInstanceIndex model_instance) const
 Returns the number of generalized positions in a specific model instance. More...
 
int num_velocities () const
 Returns the number of generalized velocities of the model. More...
 
int num_velocities (ModelInstanceIndex model_instance) const
 Returns the number of generalized velocities in a specific model instance. More...
 
int num_states () const
 Returns the total size of the state vector in the model. More...
 
int num_states (ModelInstanceIndex model_instance) const
 Returns the total size of the state vector in a specific model instance. More...
 
int num_actuated_dofs () const
 Returns the total number of Joint degrees of freedom actuated by the set of JointActuator elements added to this model. More...
 
int num_actuated_dofs (ModelInstanceIndex model_instance) const
 Returns the total number of Joint degrees of freedom actuated by the set of JointActuator elements added to a specific model instance. More...
 
int tree_height () const
 Returns the height of the tree data structure of this MultibodyTree. More...
 
const RigidBody< T > & world_body () const
 Returns a constant reference to the world body. More...
 
const BodyFrame< T > & 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 Joint< T > & get_joint (JointIndex joint_index) const
 Returns a constant reference to the joint with unique index joint_index. More...
 
const JointActuator< T > & get_joint_actuator (JointActuatorIndex actuator_index) const
 Returns a constant reference to the joint actuator with unique index actuator_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...
 
const std::string & GetModelInstanceName (ModelInstanceIndex model_instance) const
 Returns the name of a model_instance. 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
 (Advanced) Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system. More...
 
void SetDefaultContext (systems::Context< T > *context) const
 Sets default values in the context. More...
 
void SetDefaultState (const systems::Context< T > &context, systems::State< T > *state) const
 Sets default values in the state. More...
 
Eigen::VectorBlock< const VectorX< T > > GetPositionsAndVelocities (const systems::Context< T > &context) const
 Returns a const Eigen vector reference containing the vector [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities. More...
 
VectorX< T > GetPositionsAndVelocities (const systems::Context< T > &context, ModelInstanceIndex model_instance) const
 Returns a Eigen vector containing the multibody state x = [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities for model instance model_instance. More...
 
Eigen::VectorBlock< VectorX< T > > GetMutablePositionsAndVelocities (systems::Context< T > *context) const
 Returns a mutable Eigen vector containing the vector [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities. More...
 
void SetPositionsAndVelocities (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &instance_state, systems::Context< T > *context) const
 Sets context to store the vector [q; v] with q the vector of generalized positions and v the vector of generalized velocities for model instance model_instance. More...
 
void SetFreeBodyPoseOrThrow (const Body< T > &body, const Isometry3< T > &X_WB, systems::Context< T > *context) const
 Sets context to store the pose X_WB of a given body B in the world frame W. More...
 
void SetFreeBodySpatialVelocityOrThrow (const Body< T > &body, const SpatialVelocity< T > &V_WB, systems::Context< T > *context) const
 Sets context to store the spatial velocity V_WB of a given body B in the world frame W. More...
 
void SetFreeBodyPoseOrThrow (const Body< T > &body, const Isometry3< T > &X_WB, const systems::Context< T > &context, systems::State< T > *state) const
 Sets sate to store the pose X_WB of a given body B in the world frame W, for a given context of this model. More...
 
void SetFreeBodySpatialVelocityOrThrow (const Body< T > &body, const SpatialVelocity< T > &V_WB, const systems::Context< T > &context, systems::State< T > *state) const
 Sets state to store the spatial velocity V_WB of a given body B in the world frame W, for a given context of this model. More...
 
MatrixX< doubleMakeStateSelectorMatrix (const std::vector< JointIndex > &user_to_joint_index_map) const
 This method allows users to map the state of this model, x, into a vector of selected state xₛ with a given preferred ordering. More...
 
MatrixX< doubleMakeStateSelectorMatrixFromJointNames (const std::vector< std::string > &selected_joints) const
 Alternative signature to build a state selector matrix from a std::vector of joint names. More...
 
MatrixX< doubleMakeActuatorSelectorMatrix (const std::vector< JointActuatorIndex > &user_to_actuator_index_map) const
 This method allows user to map a vector uₛ containing the actuation for a set of selected actuators into the vector u containing the actuation values for this full model. More...
 
MatrixX< doubleMakeActuatorSelectorMatrix (const std::vector< JointIndex > &user_to_joint_index_map) const
 Alternative signature to build an actuation selector matrix Su such that u = Su⋅uₛ, where u is the vector of actuation values for the full model (ordered by JointActuatorIndex) and uₛ is a vector of actuation values for the actuators acting on the joints listed by user_to_joint_index_map. 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...
 
const PositionKinematicsCache< T > & EvalPositionKinematics (const systems::Context< T > &context) const
 Evaluates position kinematics cached in context. More...
 
const VelocityKinematicsCache< T > & EvalVelocityKinematics (const systems::Context< T > &context) const
 Evaluates velocity kinematics cached in context. More...
 
void set_tree_system (MultibodyTreeSystem< T > *tree_system)
 (Internal use only) Informs the MultibodyTree how to access its resources within a Context. More...
 
void CalcAcrossNodeGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, std::vector< Vector6< T >> *H_PB_W_cache) const
 (Internal) Computes the cache entry associated with the geometric Jacobian H_PB_W for each node. 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...
 
const RigidBody< T > & AddRigidBody (const std::string &name, ModelInstanceIndex model_instance, const SpatialInertia< double > &M_BBo_B)
 Creates a rigid body with the provided name, model instance, and spatial inertia. More...
 
const RigidBody< T > & AddRigidBody (const std::string &name, const SpatialInertia< double > &M_BBo_B)
 Creates a rigid body with the provided name, model instance, and spatial inertia. 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...
 
template<template< typename Scalar > class ForceElementType>
const ForceElementType< T > & AddForceElement (std::unique_ptr< ForceElementType< T >> force_element)
 Creates and adds to this MultibodyTree (which retains ownership) a new ForceElement member with the specific type ForceElementType. More...
 
template<template< typename Scalar > class ForceElementType, typename... Args>
const ForceElementType< T > & AddForceElement (Args &&... args)
 Adds a new force element model of type ForceElementType to this MultibodyTree. More...
 
template<template< typename Scalar > class ForceElementType, typename... Args>
std::enable_if< std::is_same< ForceElementType< T >, UniformGravityFieldElement< T > >::value, const ForceElementType< T > & >::type AddForceElement (Args &&... args)
 
template<template< typename Scalar > class JointType>
const JointType< T > & AddJoint (std::unique_ptr< JointType< T >> joint)
 This method adds a Joint of type JointType between the frames specified by the joint. More...
 
template<template< typename > class JointType, typename... Args>
const JointType< T > & AddJoint (const std::string &name, const Body< T > &parent, const optional< Isometry3< double >> &X_PF, const Body< T > &child, const optional< Isometry3< double >> &X_BM, Args &&... args)
 This method helps to create a Joint of type JointType between two bodies. More...
 
const JointActuator< T > & AddJointActuator (const std::string &name, const Joint< T > &joint)
 Creates and adds a JointActuator model for an actuator acting on a given joint. More...
 
ModelInstanceIndex AddModelInstance (const std::string &name)
 Creates a new model instance. More...
 
Querying for multibody elements by name

These methods allow a user to query whether a given multibody element is part of this model.

These queries can be performed at any time during the lifetime of a MultibodyTree model, i.e. there is no restriction on whether they must be called before or after Finalize(). That is, these queries can be performed while new multibody elements are being added to the model.

bool HasBodyNamed (const std::string &name) const
 
bool HasBodyNamed (const std::string &name, ModelInstanceIndex model_instance) const
 
bool HasFrameNamed (const std::string &name) const
 
bool HasFrameNamed (const std::string &name, ModelInstanceIndex model_instance) const
 
bool HasJointNamed (const std::string &name) const
 
bool HasJointNamed (const std::string &name, ModelInstanceIndex model_instance) const
 
bool HasJointActuatorNamed (const std::string &name) const
 
bool HasJointActuatorNamed (const std::string &name, ModelInstanceIndex model_instance) const
 
bool HasModelInstanceNamed (const std::string &name) const
 
Retrieving multibody elements by name

These methods allow a user to retrieve a reference to a multibody element by its name.

A std::logic_error is thrown if there is no element with the requested name.

These queries can be performed at any time during the lifetime of a MultibodyTree model, i.e. there is no restriction on whether they must be called before or after Finalize(). This implies that these queries can be performed while new multibody elements are being added to the model.

If the named element is present in more than one model instance and a model instance is not explicitly specified, std::logic_error is thrown.

const Body< T > & GetBodyByName (const std::string &name) const
 Returns a constant reference to a body that is identified by the string name in this model. More...
 
const Body< T > & GetBodyByName (const std::string &name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the body that is uniquely identified by the string name in model_instance. More...
 
const Frame< T > & GetFrameByName (const std::string &name) const
 Returns a constant reference to a frame that is identified by the string name in this model. More...
 
const Frame< T > & GetFrameByName (const std::string &name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the frame that is uniquely identified by the string name in model_instance. More...
 
const RigidBody< T > & GetRigidBodyByName (const std::string &name) const
 Returns a constant reference to a rigid body that is identified by the string name in this model. More...
 
const RigidBody< T > & GetRigidBodyByName (const std::string &name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the rigid body that is uniquely identified by the string name in model_instance. More...
 
const Joint< T > & GetJointByName (const std::string &name) const
 Returns a constant reference to a joint that is identified by the string name in this model. More...
 
const Joint< T > & GetJointByName (const std::string &name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the joint that is uniquely identified by the string name in model_instance. More...
 
template<template< typename > class JointType>
const JointType< T > & GetJointByName (const std::string &name) const
 A templated version of GetJointByName() to return a constant reference of the specified type JointType in place of the base Joint class. More...
 
template<template< typename > class JointType>
const JointType< T > & GetJointByName (const std::string &name, ModelInstanceIndex model_instance) const
 A templated version of GetJointByName() to return a constant reference of the specified type JointType in place of the base Joint class. More...
 
const JointActuator< T > & GetJointActuatorByName (const std::string &name) const
 Returns a constant reference to an actuator that is identified by the string name in this model. More...
 
const JointActuator< T > & GetJointActuatorByName (const std::string &name, ModelInstanceIndex model_instance) const
 Returns a constant reference to the actuator that is uniquely identified by the string name in model_instance. More...
 
ModelInstanceIndex GetModelInstanceByName (const std::string &name) const
 Returns the index to the model instance that is uniquely identified by the string name in this model. More...
 
Model instance accessors

Many functions on MultibodyTree expect vectors of tree state or joint actuator inputs which encompass the entire tree.

Methods in this section are convenience accessors for the portion of those vectors which apply to a single model instance only.

void SetActuationInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &u_instance, EigenPtr< VectorX< T >> u) const
 Given the actuation values u_instance for all actuators in model_instance, this method sets the actuation vector u for the entire MultibodyTree model to which this actuator belongs to. More...
 
VectorX< T > GetPositionsFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q) const
 Returns a vector of generalized positions for model_instance from a vector q_array of generalized positions for the entire MultibodyTree model. More...
 
void SetPositionsInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q_instance, EigenPtr< VectorX< T >> q) const
 Sets the vector of generalized positions for model_instance in q using q_instance, leaving all other elements in the array untouched. More...
 
VectorX< T > GetVelocitiesFromArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &v_array) const
 Returns a vector of generalized velocities for model_instance from a vector v of generalized velocities for the entire MultibodyTree model. More...
 
void SetVelocitiesInArray (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &model_v, EigenPtr< VectorX< T >> v_array) const
 Sets the vector of generalized velocities for model_instance in v using v_instance, leaving all other elements in the array untouched. More...
 
Kinematic computations

Kinematics computations are concerned with the motion of bodies in the model without regard to their mass or the forces and moments that cause the motion.

Methods in this category include the computation of poses and spatial velocities.

void CalcAllBodyPosesInWorld (const systems::Context< T > &context, std::vector< Isometry3< T >> *X_WB) const
 Computes the world pose X_WB(q) of each body B in the model as a function of the generalized positions q stored in context. More...
 
void CalcAllBodySpatialVelocitiesInWorld (const systems::Context< T > &context, std::vector< SpatialVelocity< T >> *V_WB) const
 Computes the spatial velocity V_WB(q, v) of each body B in the model, measured and expressed in the world frame W. More...
 
Isometry3< T > CalcRelativeTransform (const systems::Context< T > &context, const Frame< T > &frame_A, const Frame< T > &frame_B) const
 Computes the relative transform X_AB(q) from a frame B to a frame A, as a function of the generalized positions q of the model. More...
 
void CalcPointsPositions (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const MatrixX< T >> &p_BQi, const Frame< T > &frame_A, EigenPtr< MatrixX< T >> p_AQi) const
 Given the positions p_BQi for a set of points Qi measured and expressed in a frame B, this method computes the positions p_AQi(q) of each point Qi in the set as measured and expressed in another frame A, as a function of the generalized positions q of the model. More...
 
const Isometry3< T > & EvalBodyPoseInWorld (const systems::Context< T > &context, const Body< T > &body_B) const
 Evaluate the pose X_WB of a body B in the world frame W. More...
 
const SpatialVelocity< T > & EvalBodySpatialVelocityInWorld (const systems::Context< T > &context, const Body< T > &body_B) const
 Evaluate the spatial velocity V_WB of a body B in the world frame W. More...
 
Methods to compute multibody Jacobians.
void CalcPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_FP_list, EigenPtr< MatrixX< T >> p_WP_list, EigenPtr< MatrixX< T >> Jv_WFp) const
 Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the geometric Jacobian Jv_WFp defined by: More...
 
void CalcPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_WP_list, EigenPtr< MatrixX< T >> Jv_WFp) const
 This is a variant to compute the geometric Jacobian Jv_WFp for a list of points P moving with frame_F, given that we know the position p_WP of each point in the list measured and expressed in the world frame W. More...
 
VectorX< T > CalcBiasForPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_FP_list) const
 Computes the bias term b_WFp associated with the translational acceleration a_WFp of a point P instantaneously moving with a frame F. More...
 
void CalcPointsAnalyticalJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const MatrixX< T >> &p_FP_list, EigenPtr< MatrixX< T >> p_WP_list, EigenPtr< MatrixX< T >> Jq_WFp) const
 Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the analytical Jacobian Jq_WFp(q). More...
 
void CalcFrameGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const Vector3< T >> &p_FP, EigenPtr< MatrixX< T >> Jv_WFp) const
 Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the geometric Jacobian Jv_WFp for frame Fp. More...
 
void CalcRelativeFrameGeometricJacobian (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BP, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Jv_ABp_E) const
 Computes the geometric Jacobian for a point moving with a given frame. More...
 
Vector6< T > CalcBiasForFrameGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_F, const Eigen::Ref< const Vector3< T >> &p_FP) const
 Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the bias term Ab_WFp associated with the spatial acceleration A_WFp a frame Fp instantaneously moving with a frame F at a fixed position p_FP. More...
 
void CalcJacobianSpatialVelocity (const systems::Context< T > &context, JacobianWrtVariable with_respect_to, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BP, const Frame< T > &frame_A, const Frame< T > &frame_E, EigenPtr< MatrixX< T >> Jw_ABp_E) const
 Computes the Jacobian of spatial velocity for a frame instantaneously moving with a specified frame in the model. More...
 
Computational methods

These methods expose the computational capabilities of MultibodyTree to compute kinematics, forward and inverse dynamics, and Jacobian matrices, among others.

These methods follow Drake's naming scheme for methods performing a computation and therefore are named CalcXXX(), where XXX corresponds to the quantity or object of interest to be computed. They all take a systems::Context as an input argument storing the state of the multibody system. A std::bad_cast exception is thrown if the passed context is not a MultibodyTreeContext.

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...
 
VectorX< T > CalcInverseDynamics (const systems::Context< T > &context, const VectorX< T > &known_vdot, const MultibodyForces< T > &external_forces) 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 in order to attain the specified generalized accelerations. 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
 (Advanced) 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 CalcForceElementsContribution (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, MultibodyForces< T > *forces) const
 Computes the combined force contribution of ForceElement objects in the model. More...
 
CalcPotentialEnergy (const systems::Context< T > &context) const
 Computes and returns the total potential energy stored in this multibody model for the configuration given by context. More...
 
CalcConservativePower (const systems::Context< T > &context) const
 Computes and returns the power generated by conservative forces in the multibody model. 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...
 
VectorX< T > CalcGravityGeneralizedForces (const systems::Context< T > &context) const
 Computes the generalized forces tau_g(q) due to gravity as a function of the generalized positions q stored in the input context. More...
 
void MapVelocityToQDot (const systems::Context< T > &context, const Eigen::Ref< const VectorX< T >> &v, EigenPtr< VectorX< T >> qdot) const
 Transforms generalized velocities v to time derivatives qdot of the generalized positions vector q (stored in context). More...
 
void MapQDotToVelocity (const systems::Context< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, EigenPtr< VectorX< T >> v) const
 Transforms the time derivative qdot of the generalized positions vector q (stored in context) to generalized velocities v. More...
 
void CalcArticulatedBodyInertiaCache (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, ArticulatedBodyInertiaCache< T > *abc) const
 Computes all the quantities that are required in the final pass of the articulated body algorithm and stores them in the articulated body cache abc. 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...
 
template<template< typename > class MultibodyElement, typename Scalar >
std::enable_if_t< std::is_base_of< Joint< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant (const MultibodyElement< Scalar > &element) const
 SFINAE overload for Joint<T> elements. More...
 

Friends

template<typename >
class MultibodyTree
 
class MultibodyTreeTester
 

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() [1/3]

MultibodyTree ( const MultibodyTree< T > &  )
delete

◆ MultibodyTree() [2/3]

MultibodyTree ( MultibodyTree< T > &&  )
delete

◆ MultibodyTree() [3/3]

Creates a MultibodyTree containing only a world body.

Member Function Documentation

◆ AddBody() [1/2]

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.

◆ AddBody() [2/2]

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.

◆ AddForceElement() [1/3]

const ForceElementType<T>& AddForceElement ( std::unique_ptr< ForceElementType< T >>  force_element)
inline

Creates and adds to this MultibodyTree (which retains ownership) a new ForceElement member with the specific type ForceElementType.

The arguments to this method args are forwarded to ForceElementType's constructor.

The newly created ForceElementType object will be specialized on the scalar type T of this MultibodyTree.

◆ AddForceElement() [2/3]

const ForceElementType<T>& AddForceElement ( Args &&...  args)
inline

Adds a new force element model of type ForceElementType to this MultibodyTree.

The arguments to this method args are forwarded to ForceElementType's constructor.

Parameters
[in]argsZero or more parameters provided to the constructor of the new force element. It must be the case that JointType<T>(args) is a valid constructor.
Template Parameters
ForceElementTypeThe type of the ForceElement to add. This method can only be called once for elements of type UniformGravityFieldElement. That is, gravity can only be specified once and std::runtime_error is thrown if the model already contains a gravity field element.
Returns
A constant reference to the new ForceElement just added, of type ForceElementType<T> specialized on the scalar type T of this MultibodyTree. It will remain valid for the lifetime of this MultibodyTree.
See also
The ForceElement class's documentation for further details on how a force element is defined.
Exceptions
std::exceptionif gravity was already added to the model.

◆ AddForceElement() [3/3]

std::enable_if<std::is_same< ForceElementType<T>, UniformGravityFieldElement<T> >::value, const ForceElementType<T>&>::type AddForceElement ( Args &&...  args)
inline

◆ AddFrame() [1/2]

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.

◆ AddFrame() [2/2]

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.

◆ AddJoint() [1/2]

const JointType<T>& AddJoint ( std::unique_ptr< JointType< T >>  joint)
inline

This method adds a Joint of type JointType between the frames specified by the joint.

Parameters
[in]jointJoint to be added.
Template Parameters
JointTypeThe type of the new joint to add, which must be a subclass of Joint<T>.
Returns
A const lvalue reference to the added joint.
See also
The Joint class's documentation for further details on how a Joint is defined, or the semi-emplace AddJoint<> overload below.

◆ AddJoint() [2/2]

const JointType<T>& AddJoint ( const std::string &  name,
const Body< T > &  parent,
const optional< Isometry3< double >> &  X_PF,
const Body< T > &  child,
const optional< Isometry3< double >> &  X_BM,
Args &&...  args 
)
inline

This method helps to create a Joint of type JointType between two bodies.

The two bodies connected by this Joint object are referred to as the parent and child bodies. Although the terms parent and child are sometimes used synonymously to describe the relationship between inboard and outboard bodies in multibody models, this usage is wholly unrelated and implies nothing about the inboard-outboard relationship between the bodies. As explained in the Joint class's documentation, in Drake we define a frame F attached to the parent body P with pose X_PF and a frame M attached to the child body B with pose X_BM. This method helps create a joint between two bodies with fixed poses X_PF and X_BM. Refer to the Joint class's documentation for more details.

The arguments to this method args are forwarded to JointType's constructor. The newly created JointType object will be specialized on the scalar type T of this MultibodyTree.

Parameters
[in]nameThe name of the joint.
[in]parentThe parent body connected by the new joint.
[in]X_PFThe fixed pose of frame F attached to the parent body, measured in the frame P of that body. X_PF is an optional parameter; empty curly braces {} imply that frame F is the same body frame P. If instead your intention is to make a frame F with pose X_PF, provide Isometry3<double>::Identity() as your input.
[in]childThe child body connected by the new joint.
[in]X_BMThe fixed pose of frame M attached to the child body, measured in the frame B of that body. X_BM is an optional parameter; empty curly braces {} imply that frame M is the same body frame B. If instead your intention is to make a frame F with pose X_PF, provide Isometry3<double>::Identity() as your input.
Template Parameters
JointTypeThe type of the new joint to add, which must be a subclass of Joint<T>.
Returns
A constant reference to the new joint just added, of type JointType<T> specialized on the scalar type T of this MultibodyTree. It will remain valid for the lifetime of this MultibodyTree.

Example of usage:

MultibodyTree<T> model;
// ... Code to define a parent body P and a child body B.
const Body<double>& parent_body =
model.AddBody<RigidBody>(SpatialInertia<double>(...));
const Body<double>& child_body =
model.AddBody<RigidBody>(SpatialInertia<double>(...));
// Define the pose X_BM of a frame M rigidly atached to child body B.
const RevoluteJoint<double>& elbow =
model.AddJoint<RevoluteJoint>(
"Elbow", /* joint name */
model.world_body(), /* parent body */
{}, /* frame F IS the parent body frame P */
pendulum, /* child body, the pendulum */
X_BM, /* pose of frame M in the body frame B */
Vector3d::UnitZ()); /* revolute axis in this case */
Exceptions
std::exceptionif this model already contains a joint with the given name. See HasJointNamed(), Joint::name().
See also
The Joint class's documentation for further details on how a Joint is defined.

◆ AddJointActuator()

const JointActuator<T>& AddJointActuator ( const std::string &  name,
const Joint< T > &  joint 
)
inline

Creates and adds a JointActuator model for an actuator acting on a given joint.

This method returns a constant reference to the actuator just added, which will remain valid for the lifetime of this MultibodyTree.

Parameters
[in]nameA string that identifies the new actuator to be added to this model. An exception is thrown if an actuator with the same name already exists in the same model instance as joint. See HasJointActuatorNamed().
[in]jointThe Joint to be actuated by the new JointActuator.
Returns
A constant reference to the new JointActuator just added, which will remain valid for the lifetime of this MultibodyTree.
Exceptions
std::exceptionif this model already contains a joint actuator with the given name. See HasJointActuatorNamed(), JointActuator::get_name().

◆ AddMobilizer() [1/2]

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, 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.
std::runtime_errorif 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.

◆ AddMobilizer() [2/2]

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.
std::runtime_errorif 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.

◆ AddModelInstance()

ModelInstanceIndex AddModelInstance ( const std::string &  name)
inline

Creates a new model instance.

Returns the index for a new model instance (as there is no concrete object beyond the index).

Parameters
[in]nameA string that uniquely identifies the new instance to be added to this model. An exception is thrown if an instance with the same name already exists in the model. See HasModelInstanceNamed().
Exceptions
std::logic_errorif Finalize() was already called on this tree.

◆ AddRigidBody() [1/2]

const RigidBody<T>& AddRigidBody ( const std::string &  name,
ModelInstanceIndex  model_instance,
const SpatialInertia< double > &  M_BBo_B 
)
inline

Creates a rigid body with the provided name, model instance, and spatial inertia.

This method 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 ...
ModelInstanceIndex model_instance = model.AddModelInstance("instance");
const RigidBody<T>& body =
model.AddRigidBody("BodyName", model_instance, spatial_inertia);
Parameters
[in]nameA string that identifies the new body to be added to this model. A std::runtime_error is thrown if a body named name already is part of model_instance. See HasBodyNamed(), Body::name().
[in]model_instanceA model instance index which this body is part of.
[in]M_BBo_BThe SpatialInertia of the new rigid body to be added to this model, computed about the body frame origin Bo and expressed in the body frame B.
Returns
A constant reference to the new RigidBody just added, which will remain valid for the lifetime of this MultibodyTree.
Exceptions
std::logic_errorif a body named name already exists in this model instance.
std::logic_errorif the model instance does not exist.

◆ AddRigidBody() [2/2]

const RigidBody<T>& AddRigidBody ( const std::string &  name,
const SpatialInertia< double > &  M_BBo_B 
)
inline

Creates a rigid body with the provided name, model instance, and spatial inertia.

The newly created body will be placed in the default model instance. This method 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.AddRigidBody("BodyName", spatial_inertia);
Parameters
[in]nameA string that identifies the new body to be added to this model. A std::runtime_error is thrown if a body named name already is part of the model in the default model instance. See HasBodyNamed(), Body::name().
[in]M_BBo_BThe SpatialInertia of the new rigid body to be added to this model, computed about the body frame origin Bo and expressed in the body frame B.
Returns
A constant reference to the new RigidBody just added, which will remain valid for the lifetime of this MultibodyTree.
Exceptions
std::logic_errorif a body named name already exists.
std::logic_errorif additional model instances have been created beyond the world and default instances.

◆ CalcAccelerationKinematicsCache()

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().

◆ CalcAcrossNodeGeometricJacobianExpressedInWorld()

void CalcAcrossNodeGeometricJacobianExpressedInWorld ( const systems::Context< T > &  context,
const PositionKinematicsCache< T > &  pc,
std::vector< Vector6< T >> *  H_PB_W_cache 
) const

(Internal) Computes the cache entry associated with the geometric Jacobian H_PB_W for each node.

The geometric Jacobian H_PB_W relates to the spatial velocity of B in P by V_PB_W = H_PB_W(q)⋅v_B, where v_B corresponds to the generalized velocities associated to body B. H_PB_W has size 6 x nm with nm the number of mobilities associated with body B. H_PB_W_cache stores the Jacobian matrices for all nodes in the tree as a vector of the columns of these matrices. Therefore H_PB_W_cache has as many entries as number of generalized velocities in the tree.

◆ CalcAllBodyPosesInWorld()

void CalcAllBodyPosesInWorld ( const systems::Context< T > &  context,
std::vector< Isometry3< T >> *  X_WB 
) const

Computes the world pose X_WB(q) of each body B in the model as a function of the generalized positions q stored in context.

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q of the model.
[out]X_WBOn output this vector will contain the pose of each body in the model ordered by BodyIndex. The index of a body in the model can be obtained with Body::index(). This method throws an exception if X_WB is nullptr. Vector X_WB is resized when needed to have size num_bodies().
Exceptions
std::exceptionif X_WB is nullptr.

◆ CalcAllBodySpatialVelocitiesInWorld()

void CalcAllBodySpatialVelocitiesInWorld ( const systems::Context< T > &  context,
std::vector< SpatialVelocity< T >> *  V_WB 
) const

Computes the spatial velocity V_WB(q, v) of each body B in the model, measured and expressed in the world frame W.

The body spatial velocities are a function of the generalized positions q and generalized velocities v, both stored in context.

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q and velocities v of the model.
[out]V_WBOn output this vector will contain the spatial velocity of each body in the model ordered by BodyIndex. The index of a body in the model can be obtained with Body::index(). This method throws an exception if V_WB is nullptr. Vector V_WB is resized when needed to have size num_bodies().
Exceptions
std::exceptionif V_WB is nullptr.

◆ CalcArticulatedBodyInertiaCache()

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

Computes all the quantities that are required in the final pass of the articulated body algorithm and stores them in the articulated body cache abc.

These include:

  • Articulated body inertia Pplus_PB_W, which can be thought of as the articulated body inertia of parent body P as though it were inertialess, but taken about Bo and expressed in 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.
[out]abcA pointer to a valid, non nullptr, articulated body cache. This method throws an exception if abc is a nullptr.
Precondition
The position kinematics pc must have been previously updated with a call to CalcPositionKinematicsCache() using the same context .

◆ CalcBiasForFrameGeometricJacobianExpressedInWorld()

Vector6< T > CalcBiasForFrameGeometricJacobianExpressedInWorld ( const systems::Context< T > &  context,
const Frame< T > &  frame_F,
const Eigen::Ref< const Vector3< T >> &  p_FP 
) const

Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the bias term Ab_WFp associated with the spatial acceleration A_WFp a frame Fp instantaneously moving with a frame F at a fixed position p_FP.

That is, the spatial acceleration of frame Fp can be computed as:

  A_WFp = Jv_WFp(q)⋅v̇ + Ab_WFp(q, v)

where Ab_WFp(q, v) = J̇v_WFp(q, v)⋅v.

See also
CalcFrameGeometricJacobianExpressedInWorld() to compute the geometric Jacobian Jv_WFp(q).
Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q and generalized velocities v.
[in]frame_FThe position p_FP of frame Fp is measured and expressed in this frame F.
[in]p_FPThe (fixed) position of the origin P of frame Fp as measured and expressed in frame F.
Returns
Ab_WFp The bias term, function of the generalized positions q and the generalized velocities v as stored in context. The returned vector is of size 6, with the first three elements related to the bias in angular acceleration and the with the last three elements related to the bias in translational acceleration.
Note
SpatialAcceleration(Ab_WFp) defines a valid SpatialAcceleration.

◆ CalcBiasForPointsGeometricJacobianExpressedInWorld()

VectorX< T > CalcBiasForPointsGeometricJacobianExpressedInWorld ( const systems::Context< T > &  context,
const Frame< T > &  frame_F,
const Eigen::Ref< const MatrixX< T >> &  p_FP_list 
) const

Computes the bias term b_WFp associated with the translational acceleration a_WFp of a point P instantaneously moving with a frame F.

That is, the translational acceleration of point P can be computed as:

  a_WFp = Jv_WFp(q)⋅v̇ + b_WFp(q, v)

where b_WFp = J̇v_WFp(q, v)⋅v.

This method computes b_WFp for each point P in p_FP_list defined by its position p_FP in frame_F.

See also
CalcPointsGeometricJacobianExpressedInWorld() to compute the geometric Jacobian Jv_WFp(q).
Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q and generalized velocities v.
[in]frame_FPoints P in the list instantaneously move with this frame.
[in]p_FP_listA matrix with the fixed position of a list of points P measured and expressed in frame_F. Each column of this matrix contains the position vector p_FP for a point P measured and expressed in frame F. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the list.
Returns
b_WFp The bias term, function of the generalized positions q and the generalized velocities v as stored in context. The returned vector has size 3⋅np, with np the number of points in p_FP_list, and concatenates the bias terms for each point P in the list in the same order they are specified on input.
Exceptions
std::exceptionif p_FP_list does not have 3 rows.

◆ CalcBiasTerm()

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 (num_velocities()) of the model. This method aborts if Cv is nullptr or if it does not have the proper size.

◆ CalcConservativePower()

T CalcConservativePower ( const systems::Context< T > &  context) const

Computes and returns the power generated by conservative forces in the multibody model.

This quantity is defined to be positive when the potential energy is decreasing. In other words, if U(q) is the potential energy as defined by CalcPotentialEnergy(), then the conservative power, Pc, is Pc = -U̇(q).

See also
CalcPotentialEnergy()

◆ CalcForceElementsContribution()

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

Computes the combined force contribution of ForceElement objects in the model.

A ForceElement can apply forces as a spatial force per body or as generalized forces, depending on the ForceElement model. Therefore this method provides outputs for both spatial forces per body (with F_Bo_W_array) and generalized forces (with tau_array). ForceElement contributions are a function of the state and time only. The output from this method can immediately be used as input to CalcInverseDynamics() to include the effect of applied forces by force elements.

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.
[out]forcesA pointer to a valid, non nullptr, multibody forces object. On output forces will store the forces exerted by all the ForceElement objects in the model. This method will abort if the forces pointer is null or if the forces object is not compatible with this MultibodyTree, see MultibodyForces::CheckInvariants().
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().

◆ CalcFrameGeometricJacobianExpressedInWorld()

void CalcFrameGeometricJacobianExpressedInWorld ( const systems::Context< T > &  context,
const Frame< T > &  frame_F,
const Eigen::Ref< const Vector3< T >> &  p_FP,
EigenPtr< MatrixX< T >>  Jv_WFp 
) const

Given a frame Fp defined by shifting a frame F from its origin Fo to a new origin P, this method computes the geometric Jacobian Jv_WFp for frame Fp.

The new origin P is specified by the position vector p_FP in frame F. The frame geometric Jacobian Jv_WFp is defined by:

  V_WFp(q, v) = Jv_WFp(q)⋅v

where V_WFp(q, v) is the spatial velocity of frame Fp measured and expressed in the world frame W and q and v are the vectors of generalized position and velocity, respectively. The geometric Jacobian Jv_WFp(q) is a function of the generalized coordinates q only.

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q.
[in]frame_FThe position p_FP of frame Fp is measured and expressed in this frame F.
[in]p_FPThe (fixed) position of the origin P of frame Fp as measured and expressed in frame F.
[out]Jv_WFpThe geometric Jacobian Jv_WFp(q), function of the generalized positions q only. This Jacobian relates to the spatial velocity V_WFp of frame Fp by:
    V_WFp(q, v) = Jv_WFp(q)⋅v
  
Therefore Jv_WFp is a matrix of size 6 x nv, with nv the number of generalized velocities. On input, matrix Jv_WFp must have size 6 x nv or this method throws an exception. The top rows of this matrix (which can be accessed with Jv_WFp.topRows<3>()) is the Jacobian Hw_WFp related to the angular velocity of Fp in W by w_WFp = Hw_WFp⋅v. The bottom rows of this matrix (which can be accessed with Jv_WFp.bottomRows<3>()) is the Jacobian Hv_WFp related to the translational velocity of the origin P of frame Fp in W by v_WFpo = Hv_WFp⋅v. This ordering is consistent with the internal storage of the SpatialVelocity class. Therefore the following operations results in a valid spatial velocity:
    SpatialVelocity<double> Jv_WFp_times_v(Jv_WFp * v);
  
Exceptions
std::exceptionif J_WFp is nullptr or if it is not of size 6 x nv.

◆ CalcGravityGeneralizedForces()

VectorX< T > CalcGravityGeneralizedForces ( const systems::Context< T > &  context) const

Computes the generalized forces tau_g(q) due to gravity as a function of the generalized positions q stored in the input context.

The vector of generalized forces due to gravity tau_g(q) is defined such that it appears on the right hand side of the equations of motion together with any other generalized forces, like so:

  Mv̇ + C(q, v)v = tau_g(q) + tau_app

where tau_app includes any other generalized forces applied on the system.

Parameters
[in]contextThe context storing the state of the multibody model.
Returns
tau_g A vector containing the generalized forces due to gravity. The generalized forces are consistent with the vector of generalized velocities v for this MultibodyTree model so that the inner product v⋅tau_g corresponds to the power applied by the gravity forces on the mechanical system. That is, v⋅tau_g > 0 corresponds to potential energy going into the system, as either mechanical kinetic energy, some other potential energy, or heat, and therefore to a decrease of the gravitational potential energy.

◆ CalcInverseDynamics() [1/2]

VectorX< T > CalcInverseDynamics ( const systems::Context< T > &  context,
const VectorX< T > &  known_vdot,
const MultibodyForces< T > &  external_forces 
) 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 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 model.
[in]known_vdotA vector with the known generalized accelerations vdot for the full MultibodyTree model. Use the provided Joint APIs in order to access entries into this array.
[in]external_forcesA set of forces to be applied to the system either as body spatial forces Fapp_Bo_W or generalized forces tau_app, see MultibodyForces for details.
Returns
the vector of generalized forces that would need to be applied to the mechanical system in order to achieve the desired acceleration given by known_vdot.

◆ CalcInverseDynamics() [2/2]

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

(Advanced) 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::node_index(). This method will abort if provided with an array that does not have a size of either 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::num_velocities(). This method will abort if provided with an array that does not have a size of either MultibodyTree::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 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::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 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::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::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::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().

◆ CalcJacobianSpatialVelocity()

void CalcJacobianSpatialVelocity ( const systems::Context< T > &  context,
JacobianWrtVariable  with_respect_to,
const Frame< T > &  frame_B,
const Eigen::Ref< const Vector3< T >> &  p_BP,
const Frame< T > &  frame_A,
const Frame< T > &  frame_E,
EigenPtr< MatrixX< T >>  Jw_ABp_E 
) const

Computes the Jacobian of spatial velocity for a frame instantaneously moving with a specified frame in the model.

Consider a point P instantaneously moving with a frame B with position p_BP in that frame. Frame Bp is the frame defined by shifting frame B with origin at Bo to a new origin at point P. The spatial velocity V_ABp_E of frame Bp measured in a frame A and expressed in a frame E can be expressed as:

  V_ABp_E(q, w) = Jw_ABp_E(q)⋅w

where w represents

This method computes Jw_ABp_E(q).

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q.
[in]with_respect_toEnum indicating whether Jw_ABp_E converts generalized velocities or time-derivatives of generalized positions to spatial velocities.
[in]frame_BThe position p_BP of point P is measured and expressed in this frame.
[in]p_BPThe (fixed) position of the origin P of frame Bp as measured and expressed in frame B.
[in]frame_AThe second frame in which the spatial velocity V_ABp is measured.
[in]frame_EFrame in which the velocity V_ABp_E, and therefore the Jacobian Jw_ABp_E is expressed.
[out]Jw_ABp_EThe Jacobian Jw_ABp_E(q), function of the generalized positions q only. This Jacobian relates to the spatial velocity V_ABp_E of frame Bp in A and expressed in E by:
    V_ABp_E(q, w) = Jw_ABp_E(q)⋅w 
Therefore Jw_ABp_E is a matrix of size 6 x nz, where nz is the number of elements in w. On input, matrix Jv_ABp_E must have size 6 x nz or this method throws an exception. Given a 6 x nz Jacobian J, let Jr be the 3 x nz rotational part (top 3 rows) and Jt be the translational part (bottom 3 rows). These can be obtained as follows:
Jr_ABp_E = Jw_ABp_E.topRows<3>();
Jt_ABp_E = Jw_ABp_E.bottomRows<3>();
This ordering is consistent with the internal storage of the SpatialVelocity class. Therefore the following operations results in a valid spatial velocity:
    SpatialVelocity<double> V_ABp(Jw_ABp * w); 
Exceptions
std::exceptionif Jw_ABp_E is nullptr or if it is not of size 6 x nz.

◆ CalcMassMatrixViaInverseDynamics()

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 (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.

◆ CalcPointsAnalyticalJacobianExpressedInWorld()

void CalcPointsAnalyticalJacobianExpressedInWorld ( const systems::Context< T > &  context,
const Frame< T > &  frame_F,
const Eigen::Ref< const MatrixX< T >> &  p_FP_list,
EigenPtr< MatrixX< T >>  p_WP_list,
EigenPtr< MatrixX< T >>  Jq_WFp 
) const

Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the analytical Jacobian Jq_WFp(q).

The analytical Jacobian Jq_WFp(q) is defined by:

  Jq_WFp(q) = d(p_WFp(q))/dq

where p_WFp(q) is the position of point P, which moves with frame F, in the world frame W.

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q.
[in]frame_FThe positions p_FP of each point in the input set are measured and expressed in this frame F and are constant (fixed) in this frame.
[in]p_FP_listA matrix with the fixed position of a set of points P measured and expressed in frame_F. Each column of this matrix contains the position vector p_FP for a point P measured and expressed in frame F. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the set.
[out]p_WP_listThe output positions of each point P now measured and expressed in computing the geometric Jacobian J_WP and therefore external storage must be provided. The output p_WP_list must have the same size as the input set p_FP_list or otherwise this method throws a std::runtime_error exception. That is p_WP_list must be in ℝ³ˣⁿᵖ.
[out]Jq_WFpThe analytical Jacobian Jq_WFp(q), function of the generalized positions q only. We stack the positions of each point P in the world frame W into a column vector p_WFp = [p_WFp1; p_WFp2; ...] of size 3⋅np, with np the number of points in p_FP_list. Then the analytical Jacobian is defined as:
    Jq_WFp(q) = ∇(p_WFp(q))
  
with ∇(⋅) the gradient operator with respect to the generalized positions q. Therefore Jq_WFp is a matrix of size 3⋅np x nq, with nq the number of generalized positions. On input, matrix Jq_WFp must have size 3⋅np x nq or this method throws a std::runtime_error exception.
Exceptions
std::exceptionif the output p_WP_list is nullptr or does not have the same size as the input array p_FP_list.
std::exceptionif Jq_WFp is nullptr or if it does not have the appropriate size, see documentation for Jq_WFp for details.

◆ CalcPointsGeometricJacobianExpressedInWorld() [1/2]

void CalcPointsGeometricJacobianExpressedInWorld ( const systems::Context< T > &  context,
const Frame< T > &  frame_F,
const Eigen::Ref< const MatrixX< T >> &  p_FP_list,
EigenPtr< MatrixX< T >>  p_WP_list,
EigenPtr< MatrixX< T >>  Jv_WFp 
) const

Given a list of points with fixed position vectors p_FP in a frame F, (that is, their time derivative DtF(p_FP) in frame F is zero), this method computes the geometric Jacobian Jv_WFp defined by:

  v_WP(q, v) = Jv_WFp(q)⋅v

where v_WP(q, v) is the translational velocity of point P in the world frame W and q and v are the vectors of generalized position and velocity, respectively.

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q.
[in]frame_FThe positions p_FP of each point in the input set are measured and expressed in this frame F and are constant (fixed) in this frame.
[in]p_FP_listA matrix with the fixed position of a set of points P measured and expressed in frame_F. Each column of this matrix contains the position vector p_FP for a point P measured and expressed in frame F. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the set.
[out]p_WP_listThe output positions of each point P now measured and expressed in computing the geometric Jacobian J_WP and therefore external storage must be provided. The output p_WP_list must have the same size as the input set p_FP_list or otherwise this method throws a std::runtime_error exception. That is p_WP_list must be in ℝ³ˣⁿᵖ.
[out]Jv_WFpThe geometric Jacobian Jv_WFp(q), function of the generalized positions q only. This Jacobian relates the translational velocity v_WP of each point P in the input set by:
    v_WP(q, v) = Jv_WFp(q)⋅v
  
so that v_WP is a column vector of size 3⋅np concatenating the velocity of all points P in the same order they were given in the input set. Therefore J_WFp is a matrix of size 3⋅np x nv, with nv the number of generalized velocities. On input, matrix J_WFp must have size 3⋅np x nv or this method throws a std::runtime_error exception.
Exceptions
std::exceptionif the output p_WP_list is nullptr or does not have the same size as the input array p_FP_list.
std::exceptionif Jv_WFp is nullptr or if it does not have the appropriate size, see documentation for Jv_WFp for details.

◆ CalcPointsGeometricJacobianExpressedInWorld() [2/2]

void CalcPointsGeometricJacobianExpressedInWorld ( const systems::Context< T > &  context,
const Frame< T > &  frame_F,
const Eigen::Ref< const MatrixX< T >> &  p_WP_list,
EigenPtr< MatrixX< T >>  Jv_WFp 
) const

This is a variant to compute the geometric Jacobian Jv_WFp for a list of points P moving with frame_F, given that we know the position p_WP of each point in the list measured and expressed in the world frame W.

The geometric Jacobian Jv_WFp is defined such that:

  v_WP(q, v) = Jv_WFp(q)⋅v

where v_WP(q, v) is the translational velocity of point P in the world frame W and q and v are the vectors of generalized position and velocity, respectively. Since the spatial velocity of each point P is linear in the generalized velocities, the geometric Jacobian Jv_WFp is a function of the generalized coordinates q only.

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q.
[in]frame_FPoints P in the list instantaneously move with this frame.
[in]p_WP_listA matrix with the fixed position of a list of points P measured and expressed in the world frame W. Each column of this matrix contains the position vector p_WP for a point P measured and expressed in the world frame W. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the list.
[out]Jv_WFpThe geometric Jacobian Jv_WFp(q), function of the generalized positions q only. This Jacobian relates the translational velocity v_WP of each point P in the input list by:
    `v_WP(q, v) = Jv_WFp(q)⋅v`
  
so that v_WP is a column vector of size 3⋅np concatenating the velocity of all points P in the same order they were given in the input list. Therefore J_WP is a matrix of size 3⋅np x nv, with nv the number of generalized velocities. On input, matrix J_WP must have size 3⋅np x nv or this method throws a std::runtime_error exception.
Exceptions
std::exceptionif Jv_WFp is nullptr or if it does not have the appropriate size, see documentation for Jv_WFp for details.

◆ CalcPointsPositions()

void CalcPointsPositions ( const systems::Context< T > &  context,
const Frame< T > &  frame_B,
const Eigen::Ref< const MatrixX< T >> &  p_BQi,
const Frame< T > &  frame_A,
EigenPtr< MatrixX< T >>  p_AQi 
) const

Given the positions p_BQi for a set of points Qi measured and expressed in a frame B, this method computes the positions p_AQi(q) of each point Qi in the set as measured and expressed in another frame A, as a function of the generalized positions q of the model.

Parameters
[in]contextThe context containing the state of the MultibodyTree model. It stores the generalized positions q of the model.
[in]frame_BThe frame B in which the positions p_BQi of a set of points Qi are given.
[in]p_BQiThe input positions of each point Qi in frame B. p_BQi ∈ ℝ³ˣⁿᵖ with np the number of points in the set. Each column of p_BQi corresponds to a vector in ℝ³ holding the position of one of the points in the set as measured and expressed in frame B.
[in]frame_AThe frame A in which it is desired to compute the positions p_AQi of each point Qi in the set.
[out]p_AQiThe output positions of each point Qi now computed as measured and expressed in frame A. The output p_AQi must have the same size as the input p_BQi or otherwise this method aborts. That is p_AQi must be in ℝ³ˣⁿᵖ.
Note
Both p_BQi and p_AQi must have three rows. Otherwise this method will throw a std::runtime_error exception. This method also throws a std::runtime_error exception if p_BQi and p_AQi differ in the number of columns.

◆ CalcPositionKinematicsCache()

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.

Aborts if pc is nullptr.

◆ CalcPotentialEnergy()

T CalcPotentialEnergy ( const systems::Context< T > &  context) const

Computes and returns the total potential energy stored in this multibody model for the configuration given by context.

Parameters
[in]contextThe context containing the state of the MultibodyTree model.
Returns
The total potential energy stored in this multibody model.

◆ CalcRelativeFrameGeometricJacobian()

void CalcRelativeFrameGeometricJacobian ( const systems::Context< T > &  context,
const Frame< T > &  frame_B,
const Eigen::Ref< const Vector3< T >> &  p_BP,
const Frame< T > &  frame_A,
const Frame< T > &  frame_E,
EigenPtr< MatrixX< T >>  Jv_ABp_E 
) const

Computes the geometric Jacobian for a point moving with a given frame.

Consider a point P instantaneously moving with a frame B with position p_BP in that frame. Frame Bp is the frame defined by shifting frame B with origin at Bo to a new origin at point P. The spatial velocity V_ABp_E of frame Bp measured in a frame A and expressed in a frame E relates to the generalized velocities of the system by the geometric Jacobian Jv_ABp_E(q) by:

  V_ABp_E(q, v) = Jv_ABp_E(q)⋅v

This method computes the geometric Jacobian Jv_ABp_E(q).

Parameters
[in]contextThe context containing the state of the model. It stores the generalized positions q.
[in]frame_BThe position p_BP of point P is measured and expressed in this frame.
[in]p_BPThe (fixed) position of the origin P of frame Bp as measured and expressed in frame B.
[in]frame_AThe second frame in which the spatial velocity V_ABp is measured and expressed.
[in]frame_EFrame in which the velocity V_ABp_E is expressed.
[out]Jv_ABp_EThe geometric Jacobian Jv_ABp_E(q), function of the generalized positions q only. This Jacobian relates to the spatial velocity V_ABp_E of frame Bp in A and expressed in E by:
    V_ABp_E(q, v) = Jv_ABp_E(q)⋅v
  
Therefore Jv_ABp_E is a matrix of size 6 x nv, with nv the number of generalized velocities. On input, matrix Jv_ABp_E must have size 6 x nv or this method throws an exception. Given a 6 x nv spatial Jacobian Jv, let Jvr be the 3 x nv rotational part (top 3 rows) and Jvt be the translational part (bottom 3 rows). These can be obtained as follows:
    Jvr_ABp = Jv_ABp.topRows<3>();
    Jvt_ABp = Jv_ABp.bottomRows<3>();
  
This ordering is consistent with the internal storage of the SpatialVelocity class. Therefore the following operations results in a valid spatial velocity:
    SpatialVelocity<double> V_ABp(Jv_ABp * v);
  
Exceptions
std::exceptionif J_ABp is nullptr or if it is not of size 6 x nv.

◆ CalcRelativeTransform()

Isometry3< T > CalcRelativeTransform ( const systems::Context< T > &  context,
const Frame< T > &  frame_A,
const Frame< T > &  frame_B 
) const

Computes the relative transform X_AB(q) from a frame B to a frame A, as a function of the generalized positions q of the model.

That is, the position p_AQ of a point Q measured and expressed in frame A can be computed from the position p_BQ of this point measured and expressed in frame B using the transformation p_AQ = X_AB⋅p_BQ.

Parameters
[in]contextThe context containing the state of the MultibodyTree model. It stores the generalized positions q of the model.
[in]frame_AThe target frame A in the computed relative transform X_AB.
[in]frame_BThe source frame B in the computed relative transform X_AB.
Return values
X_ABThe relative transform from frame B to frame A, such that p_AQ = X_AB⋅p_BQ.

◆ CalcSpatialAccelerationsFromVdot()

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 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().

◆ CalcVelocityKinematicsCache()

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().

Aborts if vc is nullptr.

◆ Clone()

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.

◆ CloneToScalar()

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.

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, where ToScalar = AutoDiffXd.
std::unique_ptr<MultibodyTree<AutoDiffXd>> model_autodiff =
model.CloneToScalar<AutoDiffXd>();
// User retrieves the AutoDiffXd variant corresponding to the original
// body added above.
body_autodiff = model_autodiff.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. In the example above, the user could have also invoked the method ToAutoDiffXd().

Precondition
Finalize() must have already been called on this MultibodyTree.

◆ CreateDefaultContext()

std::unique_ptr<systems::LeafContext<T> > CreateDefaultContext ( ) const
inline

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

Exceptions
std::runtime_errorif this is not owned by a MultibodyPlant / MultibodyTreeSystem.

◆ EvalBodyPoseInWorld()

const Isometry3< T > & EvalBodyPoseInWorld ( const systems::Context< T > &  context,
const Body< T > &  body_B 
) const

Evaluate the pose X_WB of a body B in the world frame W.

Parameters
[in]contextThe context storing the state of the MultibodyTree model.
[in]body_BThe body B for which the pose is requested.
Return values
X_WBThe pose of body frame B in the world frame W.
Exceptions
std::exceptionif Finalize() was not called on this model or if body_B does not belong to this model.

◆ EvalBodySpatialVelocityInWorld()

const SpatialVelocity< T > & EvalBodySpatialVelocityInWorld ( const systems::Context< T > &  context,
const Body< T > &  body_B 
) const

Evaluate the spatial velocity V_WB of a body B in the world frame W.

Parameters
[in]contextThe context storing the state of the MultibodyTree model.
[in]body_BThe body B for which the spatial velocity is requested.
Returns
V_WB The spatial velocity of body frame B in the world frame W.
Exceptions
std::exceptionif Finalize() was not called on this model or if body_B does not belong to this model.

◆ EvalPositionKinematics()

const PositionKinematicsCache<T>& EvalPositionKinematics ( const systems::Context< T > &  context) const
inline

Evaluates position kinematics cached in context.

Parameters
contextA Context whose position kinematics cache will be updated and returned.
Returns
Reference to the PositionKinematicsCache of context.

◆ EvalVelocityKinematics()

const VelocityKinematicsCache<T>& EvalVelocityKinematics ( const systems::Context< T > &  context) const
inline

Evaluates velocity kinematics cached in context.

This will also force position kinematics to be updated if it hasn't already.

Parameters
contextA Context whose velocity kinematics cache will be updated and returned.
Returns
Reference to the VelocityKinematicsCache of context.

◆ Finalize()

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::exceptionif called post-finalize.

◆ get_body()

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

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

Exceptions
std::exceptionif body_index does not correspond to a body in this multibody tree.

◆ get_frame()

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

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

Exceptions
std::exceptionif frame_index does not correspond to a frame in this multibody tree.

◆ get_joint()

const Joint<T>& get_joint ( JointIndex  joint_index) const
inline

Returns a constant reference to the joint with unique index joint_index.

Exceptions
std::runtime_errorwhen joint_index does not correspond to a joint in this multibody tree.

◆ get_joint_actuator()

const JointActuator<T>& get_joint_actuator ( JointActuatorIndex  actuator_index) const
inline

Returns a constant reference to the joint actuator with unique index actuator_index.

Exceptions
std::exceptionif actuator_index does not correspond to a joint actuator in this multibody tree.

◆ get_mobilizer()

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

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

Exceptions
std::runtime_errorwhen mobilizer_index does not correspond to a mobilizer in this multibody tree.

◆ get_topology()

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.

◆ get_variant() [1/4]

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.

◆ get_variant() [2/4]

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.

◆ get_variant() [3/4]

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.

◆ get_variant() [4/4]

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

SFINAE overload for Joint<T> elements.

◆ GetBodyByName() [1/2]

const Body<T>& GetBodyByName ( const std::string &  name) const
inline

Returns a constant reference to a body that is identified by the string name in this model.

Exceptions
std::logic_errorif there is no body with the requested name.
std::logic_errorif the body name occurs in multiple model instances.
See also
HasBodyNamed() to query if there exists a body in this model with a given specified name.

◆ GetBodyByName() [2/2]

const Body<T>& GetBodyByName ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline

Returns a constant reference to the body that is uniquely identified by the string name in model_instance.

Exceptions
std::logic_errorif there is no body with the requested name.
std::runtime_errorif model_instance is not valid for this model.
See also
HasBodyNamed() to query if there exists a body in this model with a given specified name.

◆ GetFrameByName() [1/2]

const Frame<T>& GetFrameByName ( const std::string &  name) const
inline

Returns a constant reference to a frame that is identified by the string name in this model.

Exceptions
std::logic_errorif there is no frame with the requested name.
std::logic_errorif the frame name occurs in multiple model instances.
See also
HasFrameNamed() to query if there exists a body in this model with a given specified name.

◆ GetFrameByName() [2/2]

const Frame<T>& GetFrameByName ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline

Returns a constant reference to the frame that is uniquely identified by the string name in model_instance.

Exceptions
std::logic_errorif there is no frame with the requested name.
std::runtime_errorif model_instance is not valid for this model.
See also
HasFrameNamed() to query if there exists a frame in this model with a given specified name.

◆ GetJointActuatorByName() [1/2]

const JointActuator<T>& GetJointActuatorByName ( const std::string &  name) const
inline

Returns a constant reference to an actuator that is identified by the string name in this model.

Exceptions
std::logic_errorif there is no actuator with the requested name.
std::logic_errorif the actuator name occurs in multiple model instances.
See also
HasJointActuatorNamed() to query if there exists an actuator in this model with a given specified name.

◆ GetJointActuatorByName() [2/2]

const JointActuator<T>& GetJointActuatorByName ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline

Returns a constant reference to the actuator that is uniquely identified by the string name in model_instance.

Exceptions
std::logic_errorif there is no actuator with the requested name.
std::runtime_errorif model_instance is not valid for this model.
See also
HasJointActuatorNamed() to query if there exists an actuator in this model with a given specified name.

◆ GetJointByName() [1/4]

const Joint<T>& GetJointByName ( const std::string &  name) const
inline

Returns a constant reference to a joint that is identified by the string name in this model.

Exceptions
std::logic_errorif there is no joint with the requested name.
std::logic_errorif the joint name occurs in multiple model instances.
See also
HasJointNamed() to query if there exists a joint in this model with a given specified name.

◆ GetJointByName() [2/4]

const Joint<T>& GetJointByName ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline

Returns a constant reference to the joint that is uniquely identified by the string name in model_instance.

Exceptions
std::logic_errorif there is no joint with the requested name.
std::runtime_errorif model_instance is not valid for this model.
See also
HasJointNamed() to query if there exists a joint in this model with a given specified name.

◆ GetJointByName() [3/4]

const JointType<T>& GetJointByName ( const std::string &  name) const
inline

A templated version of GetJointByName() to return a constant reference of the specified type JointType in place of the base Joint class.

See GetJointByName() for details.

Template Parameters
JointTypeThe specific type of the Joint to be retrieved. It must be a subclass of Joint.
Exceptions
std::logic_errorif the named joint is not of type JointType or if there is no Joint with that name.
std::logic_errorif the joint name occurs in multiple model instances.
See also
HasJointNamed() to query if there exists a joint in this model with a given specified name.

◆ GetJointByName() [4/4]

const JointType<T>& GetJointByName ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline

A templated version of GetJointByName() to return a constant reference of the specified type JointType in place of the base Joint class.

See GetJointByName() for details.

Template Parameters
JointTypeThe specific type of the Joint to be retrieved. It must be a subclass of Joint.
Exceptions
std::logic_errorif the named joint is not of type JointType or
std::runtime_errorif model_instance is not valid for this model. if there is no Joint with that name.
See also
HasJointNamed() to query if there exists a joint in this model with a given specified name.

◆ GetModelInstanceByName()

ModelInstanceIndex GetModelInstanceByName ( const std::string &  name) const
inline

Returns the index to the model instance that is uniquely identified by the string name in this model.

Exceptions
std::logic_errorif there is no instance with the requested name.
See also
HasModelInstanceNamed() to query if there exists an instance in this model with a given specified name.

◆ GetModelInstanceName()

const std::string& GetModelInstanceName ( ModelInstanceIndex  model_instance) const
inline

Returns the name of a model_instance.

Exceptions
std::logic_errorwhen model_instance does not correspond to a model in this multibody tree.

◆ GetMutablePositionsAndVelocities()

Eigen::VectorBlock< VectorX< T > > GetMutablePositionsAndVelocities ( systems::Context< T > *  context) const

Returns a mutable Eigen vector containing the vector [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities.

Exceptions
std::exceptionif the context is nullptr or if it does not correspond to the context for a multibody model.
Note
This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.

◆ GetPositionsAndVelocities() [1/2]

Eigen::VectorBlock< const VectorX< T > > GetPositionsAndVelocities ( const systems::Context< T > &  context) const

Returns a const Eigen vector reference containing the vector [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities.

Note
This method returns a reference to existing data, exhibits constant i.e., O(1) time complexity, and runs very quickly.
Exceptions
std::exceptionif the context does not correspond to the context for a multibody model.

◆ GetPositionsAndVelocities() [2/2]

VectorX< T > GetPositionsAndVelocities ( const systems::Context< T > &  context,
ModelInstanceIndex  model_instance 
) const

Returns a Eigen vector containing the multibody state x = [q; v] of the model with q the vector of generalized positions and v the vector of generalized velocities for model instance model_instance.

Exceptions
std::exceptionif the context does not correspond to the context for a multibody model or model_instance is invalid.
Note
returns a dense vector of dimension q.size() + v.size() associated with model_instance in O(q.size()) time.

◆ GetPositionsFromArray()

VectorX< T > GetPositionsFromArray ( ModelInstanceIndex  model_instance,
const Eigen::Ref< const VectorX< T >> &  q 
) const

Returns a vector of generalized positions for model_instance from a vector q_array of generalized positions for the entire MultibodyTree model.

This method throws an exception if q is not of size MultibodyTree::num_positions().

◆ GetRigidBodyByName() [1/2]

const RigidBody<T>& GetRigidBodyByName ( const std::string &  name) const
inline

Returns a constant reference to a rigid body that is identified by the string name in this model.

Exceptions
std::logic_errorif there is no body with the requested name.
std::logic_errorif the body name occurs in multiple model instances.
std::logic_errorif the requested body is not a RigidBody.
See also
HasBodyNamed() to query if there exists a body in this model with a given specified name.

◆ GetRigidBodyByName() [2/2]

const RigidBody<T>& GetRigidBodyByName ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline

Returns a constant reference to the rigid body that is uniquely identified by the string name in model_instance.

Exceptions
std::logic_errorif there is no body with the requested name.
std::logic_errorif the requested body is not a RigidBody.
std::runtime_errorif model_instance is not valid for this model.
See also
HasBodyNamed() to query if there exists a body in this model with a given specified name.

◆ GetVelocitiesFromArray()

VectorX< T > GetVelocitiesFromArray ( ModelInstanceIndex  model_instance,
const Eigen::Ref< const VectorX< T >> &  v_array 
) const

Returns a vector of generalized velocities for model_instance from a vector v of generalized velocities for the entire MultibodyTree model.

This method throws an exception if the input array is not of size MultibodyTree::num_velocities().

◆ HasBodyNamed() [1/2]

bool HasBodyNamed ( const std::string &  name) const
inline
Returns
true if a body named name was added to the model.
See also
AddRigidBody().
Exceptions
std::logic_errorif the body name occurs in multiple model instances.

◆ HasBodyNamed() [2/2]

bool HasBodyNamed ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline
Returns
true if a body named name was added to model_instance.
See also
AddRigidBody().
Exceptions
std::exceptionif model_instance is not valid for this model.

◆ HasFrameNamed() [1/2]

bool HasFrameNamed ( const std::string &  name) const
inline
Returns
true if a frame named name was added to the model.
See also
AddFrame().
Exceptions
std::logic_errorif the frame name occurs in multiple model instances.

◆ HasFrameNamed() [2/2]

bool HasFrameNamed ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline
Returns
true if a frame named name was added to model_instance.
See also
AddFrame().
Exceptions
std::exceptionif model_instance is not valid for this model.

◆ HasJointActuatorNamed() [1/2]

bool HasJointActuatorNamed ( const std::string &  name) const
inline
Returns
true if a joint actuator named name was added to the model.
See also
AddJointActuator().
Exceptions
std::logic_errorif the actuator name occurs in multiple model instances.

◆ HasJointActuatorNamed() [2/2]

bool HasJointActuatorNamed ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline
Returns
true if a joint actuator named name was added to model_instance.
See also
AddJointActuator().
Exceptions
std::exceptionif model_instance is not valid for this model.

◆ HasJointNamed() [1/2]

bool HasJointNamed ( const std::string &  name) const
inline
Returns
true if a joint named name was added to the model.
See also
AddJoint().
Exceptions
std::logic_errorif the joint name occurs in multiple model instances.

◆ HasJointNamed() [2/2]

bool HasJointNamed ( const std::string &  name,
ModelInstanceIndex  model_instance 
) const
inline
Returns
true if a joint named name was added to model_instance.
See also
AddJoint().
Exceptions
std::exceptionif model_instance is not valid for this model.

◆ HasModelInstanceNamed()

bool HasModelInstanceNamed ( const std::string &  name) const
inline
Returns
true if a model instance named name was added to the model.
See also
AddModelInstance().

◆ MakeActuatorSelectorMatrix() [1/2]

MatrixX< double > MakeActuatorSelectorMatrix ( const std::vector< JointActuatorIndex > &  user_to_actuator_index_map) const

This method allows user to map a vector uₛ containing the actuation for a set of selected actuators into the vector u containing the actuation values for this full model.

The mapping, or selection, is returned in the form of a selector matrix Su such that u = Su⋅uₛ. The size nₛ of uₛ is always smaller or equal than the size of the full vector of actuation values u. That is, a user might be interested in only a given subset of actuators in the model.

This selection matrix is particularly useful when adding PID control on a portion of the state, see systems::controllers::PidController.

A user specifies the preferred order in uₛ via user_to_actuator_index_map. The actuation values in uₛ are a concatenation of the values for each actuator in the order they appear in user_to_actuator_index_map. The full vector of actuation values u is ordered by JointActuatorIndex.

◆ MakeActuatorSelectorMatrix() [2/2]

MatrixX< double > MakeActuatorSelectorMatrix ( const std::vector< JointIndex > &  user_to_joint_index_map) const

Alternative signature to build an actuation selector matrix Su such that u = Su⋅uₛ, where u is the vector of actuation values for the full model (ordered by JointActuatorIndex) and uₛ is a vector of actuation values for the actuators acting on the joints listed by user_to_joint_index_map.

It is assumed that all joints referenced by user_to_joint_index_map are actuated. See MakeActuatorSelectorMatrix(const std::vector<JointActuatorIndex>&) for details.

Exceptions
std::logic_errorif any of the joints in user_to_joint_index_map does not have an actuator.

◆ MakeStateSelectorMatrix()

MatrixX< double > MakeStateSelectorMatrix ( const std::vector< JointIndex > &  user_to_joint_index_map) const

This method allows users to map the state of this model, x, into a vector of selected state xₛ with a given preferred ordering.

The mapping, or selection, is returned in the form of a selector matrix Sx such that xₛ = Sx⋅x. The size nₛ of xₛ is always smaller or equal than the size of the full state x. That is, a user might be interested in only a given portion of the full state x.

This selection matrix is particularly useful when adding PID control on a portion of the state, see systems::controllers::PidController.

A user specifies the preferred order in xₛ via user_to_joint_index_map. The selected state is built such that selected positions are followed by selected velocities, as in xₛ = [qₛ, vₛ]. The positions in qₛ are a concatenation of the positions for each joint in the order they appear in user_to_joint_index_map. That is, the positions for user_to_joint_index_map[0] are first, followed by the positions for user_to_joint_index_map[1], etc. Similarly for the selected velocities vₛ.

Exceptions
std::logic_errorif there are repeated indexes in user_to_joint_index_map.

◆ MakeStateSelectorMatrixFromJointNames()

MatrixX< double > MakeStateSelectorMatrixFromJointNames ( const std::vector< std::string > &  selected_joints) const

Alternative signature to build a state selector matrix from a std::vector of joint names.

See MakeStateSelectorMatrixFromJointNames(const std::vector<JointIndex>&) for details. selected_joints must not contain any duplicates.

A user specifies the preferred order in the selected states vector xₛ via selected_joints. The selected state is built such that selected positions are followed by selected velocities, as in xₛ = [qₛ, vₛ]. The positions in qₛ are a concatenation of the positions for each joint in the order they appear in selected_joints. That is, the positions for selected_joints[0] are first, followed by the positions for selected_joints[1], etc. Similarly for the selected velocities vₛ.

Exceptions
std::logic_errorif there are any duplicates in selected_joints.
std::logic_errorif there is no joint in the model with a name specified in selected_joints.

◆ MapQDotToVelocity()

void MapQDotToVelocity ( const systems::Context< T > &  context,
const Eigen::Ref< const VectorX< T >> &  qdot,
EigenPtr< VectorX< T >>  v 
) const

Transforms the time derivative qdot of the generalized positions vector q (stored in context) to generalized velocities v.

v and are related linearly by q̇ = N(q)⋅v. Although N(q) is not necessarily square, its left pseudo-inverse N⁺(q) can be used to invert that relationship without residual error, provided that qdot is in the range space of N(q) (that is, if it could have been produced as q̇ = N(q)⋅v for some v). Using the configuration q stored in the given context this method calculates v = N⁺(q)⋅q̇.

Parameters
[in]contextThe context containing the state of the MultibodyTree model.
[in]qdotA vector containing the time derivatives of the generalized positions. This method aborts if qdot is not of size num_positions().
[out]vA valid (non-null) pointer to a vector in ℛⁿ with n the number of generalized velocities. This method aborts if v is nullptr or if it is not of size num_velocities().
See also
MapVelocityToQDot()
Mobilizer::MapQDotToVelocity()

◆ MapVelocityToQDot()

void MapVelocityToQDot ( const systems::Context< T > &  context,
const Eigen::Ref< const VectorX< T >> &  v,
EigenPtr< VectorX< T >>  qdot 
) const

Transforms generalized velocities v to time derivatives qdot of the generalized positions vector q (stored in context).

v and qdot are related linearly by q̇ = N(q)⋅v. Using the configuration q stored in the given context this method calculates q̇ = N(q)⋅v.

Parameters
[in]contextThe context containing the state of the MultibodyTree model.
[in]vA vector of of generalized velocities for this MultibodyTree model. This method aborts if v is not of size num_velocities().
[out]qdotA valid (non-null) pointer to a vector in ℝⁿ with n being the number of generalized positions in this MultibodyTree model, given by num_positions(). This method aborts if qdot is nullptr or if it is not of size num_positions().
See also
MapQDotToVelocity()
Mobilizer::MapVelocityToQDot()

◆ num_actuated_dofs() [1/2]

int num_actuated_dofs ( ) const
inline

Returns the total number of Joint degrees of freedom actuated by the set of JointActuator elements added to this model.

◆ num_actuated_dofs() [2/2]

int num_actuated_dofs ( ModelInstanceIndex  model_instance) const
inline

Returns the total number of Joint degrees of freedom actuated by the set of JointActuator elements added to a specific model instance.

◆ num_actuators()

int num_actuators ( ) const
inline

Returns the number of actuators in the model.

See also
AddJointActuator().

◆ num_bodies()

int 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.

◆ num_force_elements()

int num_force_elements ( ) const
inline

Returns the number of ForceElement objects in the MultibodyTree.

◆ num_frames()

int 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.

◆ num_joints()

int num_joints ( ) const
inline

Returns the number of joints added with AddJoint() to the MultibodyTree.

◆ num_mobilizers()

int 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. num_mobilizers() returns num_bodies() - 1.

◆ num_model_instances()

int num_model_instances ( ) const
inline

Returns the number of model instances in the MultibodyTree.

◆ num_positions() [1/2]

int num_positions ( ) const
inline

Returns the number of generalized positions of the model.

◆ num_positions() [2/2]

int num_positions ( ModelInstanceIndex  model_instance) const
inline

Returns the number of generalized positions in a specific model instance.

◆ num_states() [1/2]

int num_states ( ) const
inline

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

◆ num_states() [2/2]

int num_states ( ModelInstanceIndex  model_instance) const
inline

Returns the total size of the state vector in a specific model instance.

◆ num_velocities() [1/2]

int num_velocities ( ) const
inline

Returns the number of generalized velocities of the model.

◆ num_velocities() [2/2]

int num_velocities ( ModelInstanceIndex  model_instance) const
inline

Returns the number of generalized velocities in a specific model instance.

◆ operator=() [1/2]

MultibodyTree& operator= ( MultibodyTree< T > &&  )
delete

◆ operator=() [2/2]

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

◆ set_tree_system()

void set_tree_system ( MultibodyTreeSystem< T > *  tree_system)
inline

(Internal use only) Informs the MultibodyTree how to access its resources within a Context.

◆ SetActuationInArray()

void SetActuationInArray ( ModelInstanceIndex  model_instance,
const Eigen::Ref< const VectorX< T >> &  u_instance,
EigenPtr< VectorX< T >>  u 
) const

Given the actuation values u_instance for all actuators in model_instance, this method sets the actuation vector u for the entire MultibodyTree model to which this actuator belongs to.

This method throws an exception if the size of u_instance is not equal to the number of degrees of freedom of all of the actuated joints in model_instance.

Parameters
[in]u_instanceActuation values for the actuators. It must be of size equal to the number of degrees of freedom of all of the actuated joints in model_instance.
[out]uThe vector containing the actuation values for the entire MultibodyTree.

◆ SetDefaultContext()

void SetDefaultContext ( 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().

◆ SetDefaultState()

void SetDefaultState ( const systems::Context< T > &  context,
systems::State< T > *  state 
) const

Sets default values in the state.

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

◆ SetFreeBodyPoseOrThrow() [1/2]

void SetFreeBodyPoseOrThrow ( const Body< T > &  body,
const Isometry3< T > &  X_WB,
systems::Context< T > *  context 
) const

Sets context to store the pose X_WB of a given body B in the world frame W.

Note
In general setting the pose and/or velocity of a body in the model would involve a complex inverse kinematics problem. This method allows us to simplify this process when we know the body is free in space.
Exceptions
std::exceptionif body is not a free body in the model.
std::exceptionif called pre-finalize.

◆ SetFreeBodyPoseOrThrow() [2/2]

void SetFreeBodyPoseOrThrow ( const Body< T > &  body,
const Isometry3< T > &  X_WB,
const systems::Context< T > &  context,
systems::State< T > *  state 
) const

Sets sate to store the pose X_WB of a given body B in the world frame W, for a given context of this model.

Note
In general setting the pose and/or velocity of a body in the model would involve a complex inverse kinematics problem. This method allows us to simplify this process when we know the body is free in space.
Exceptions
std::exceptionif body is not a free body in the model.
std::exceptionif called pre-finalize.

◆ SetFreeBodySpatialVelocityOrThrow() [1/2]

void SetFreeBodySpatialVelocityOrThrow ( const Body< T > &  body,
const SpatialVelocity< T > &  V_WB,
systems::Context< T > *  context 
) const

Sets context to store the spatial velocity V_WB of a given body B in the world frame W.

Note
In general setting the pose and/or velocity of a body in the model would involve a complex inverse kinematics problem. This method allows us to simplify this process when we know the body is free in space.
Exceptions
std::exceptionif body is not a free body in the model.
std::exceptionif called pre-finalize.

◆ SetFreeBodySpatialVelocityOrThrow() [2/2]

void SetFreeBodySpatialVelocityOrThrow ( const Body< T > &  body,
const SpatialVelocity< T > &  V_WB,
const systems::Context< T > &  context,
systems::State< T > *  state 
) const

Sets state to store the spatial velocity V_WB of a given body B in the world frame W, for a given context of this model.

Note
In general setting the pose and/or velocity of a body in the model would involve a complex inverse kinematics problem. This method allows us to simplify this process when we know the body is free in space.
Exceptions
std::exceptionif body is not a free body in the model.
std::exceptionif called pre-finalize.

◆ SetPositionsAndVelocities()

void SetPositionsAndVelocities ( ModelInstanceIndex  model_instance,
const Eigen::Ref< const VectorX< T >> &  instance_state,
systems::Context< T > *  context 
) const

Sets context to store the vector [q; v] with q the vector of generalized positions and v the vector of generalized velocities for model instance model_instance.

Exceptions
std::exceptionif the context does not correspond to the context for a multibody model, context is nullptr, model_instance is invalid, or instance_state.size() does not equal num_positions(model_instance)
  • num_velocities(model_instance).

◆ SetPositionsInArray()

void SetPositionsInArray ( ModelInstanceIndex  model_instance,
const Eigen::Ref< const VectorX< T >> &  q_instance,
EigenPtr< VectorX< T >>  q 
) const

Sets the vector of generalized positions for model_instance in q using q_instance, leaving all other elements in the array untouched.

This method throws an exception if q is not of size MultibodyTree::num_positions() or q_instance is not of size MultibodyTree::num_positions(model_instance).

◆ SetVelocitiesInArray()

void SetVelocitiesInArray ( ModelInstanceIndex  model_instance,
const Eigen::Ref< const VectorX< T >> &  model_v,
EigenPtr< VectorX< T >>  v_array 
) const

Sets the vector of generalized velocities for model_instance in v using v_instance, leaving all other elements in the array untouched.

This method throws an exception if v is not of size MultibodyTree::num_velocities() or v_instance is not of size MultibodyTree::num_positions(model_instance).

◆ ToAutoDiffXd()

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

Creates a deep copy of this MultibodyTree templated on AutoDiffXd.

◆ topology_is_valid()

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().

◆ tree_height()

int 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.

◆ world_body()

const RigidBody<T>& world_body ( ) const
inline

Returns a constant reference to the world body.

◆ world_frame()

const BodyFrame<T>& world_frame ( ) const
inline

Returns a constant reference to the world frame.

Friends And Related Function Documentation

◆ MultibodyTree

friend class MultibodyTree
friend

◆ MultibodyTreeTester

friend class MultibodyTreeTester
friend

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