Drake

MultibodyTree provides a representation for a physical system consisting of a collection of interconnected rigid and deformable bodies. More...
#include <drake/multibody/multibody_tree/body.h>
Public Member Functions  
MultibodyTree ()  
Creates a MultibodyTree containing only a world body. More...  
int  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...  
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 MultibodyTreeTopology &  get_topology () const 
Returns the topology information for this multibody tree. More...  
void  Finalize () 
This method must be called after all elements in the tree (joints, bodies, force elements, constraints) were added and before any computations are performed. More...  
std::unique_ptr< systems::LeafContext< T > >  CreateDefaultContext () const 
Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system. More...  
void  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...  
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...  
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...  
Does not allow copy, move, or assignment  
MultibodyTree (const MultibodyTree &)=delete  
MultibodyTree &  operator= (const MultibodyTree &)=delete 
MultibodyTree (MultibodyTree &&)=delete  
MultibodyTree &  operator= (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) 
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 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  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 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  set_actuation_vector (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 >  get_positions_from_array (ModelInstanceIndex model_instance, const Eigen::Ref< const VectorX< T >> &q_array) const 
Returns a vector of generalized positions for model_instance from a vector q_array of generalized positions for the entire MultibodyTree model. More...  
VectorX< T >  get_velocities_from_array (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_array of generalized velocities for the entire MultibodyTree model. 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_B, const Eigen::Ref< const MatrixX< T >> &p_BQi_set, EigenPtr< MatrixX< T >> p_WQi_set, EigenPtr< MatrixX< T >> Jv_WQi) const 
Given a set of points Qi with fixed position vectors p_BQi in a frame B, (that is, their time derivative ᴮd/dt(p_BQi) in frame B is zero), this method computes the geometric Jacobian Jv_WQi defined by: More...  
void  CalcPointsGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const MatrixX< T >> &p_WQi_set, EigenPtr< MatrixX< T >> Jv_WQi) const 
This is a variant to compute the geometric Jacobian Jv_WQi for a set of points Qi moving with frame_B , given that we know the position p_WQi of each point in the set measured and expressed in the world frame W. More...  
void  CalcFrameGeometricJacobianExpressedInWorld (const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const Vector3< T >> &p_BoFo_B, EigenPtr< MatrixX< T >> Jv_WF) const 
Given a frame F with fixed position p_BoFo_B in a frame B, this method computes the geometric Jacobian Jv_WF defined by: 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  
void  CalcPositionKinematicsCache (const systems::Context< T > &context, PositionKinematicsCache< T > *pc) const 
Computes into the position kinematics pc all the kinematic quantities that depend on the generalized positions only. More...  
void  CalcVelocityKinematicsCache (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, VelocityKinematicsCache< T > *vc) const 
Computes all the kinematic quantities that depend on the generalized velocities and stores them in the velocity kinematics cache vc . More...  
void  CalcAccelerationKinematicsCache (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, AccelerationKinematicsCache< T > *ac) const 
Computes all the kinematic quantities that depend on the generalized accelerations that is, the generalized velocities' time derivatives, and stores them in the acceleration kinematics cache ac . More...  
void  CalcSpatialAccelerationsFromVdot (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, std::vector< SpatialAcceleration< T >> *A_WB_array) const 
Given the state of this MultibodyTree in context and a known vector of generalized accelerations known_vdot , this method computes the spatial acceleration A_WB for each body as measured and expressed in the world frame W. More...  
void  CalcInverseDynamics (const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, const std::vector< SpatialForce< T >> &Fapplied_Bo_W_array, const Eigen::Ref< const VectorX< T >> &tau_applied_array, std::vector< SpatialAcceleration< T >> *A_WB_array, std::vector< SpatialForce< T >> *F_BMo_W_array, EigenPtr< VectorX< T >> tau_array) const 
Given the state of this MultibodyTree in context and a known vector of generalized accelerations vdot , this method computes the set of generalized forces tau that would need to be applied at each Mobilizer in order to attain the specified generalized accelerations. More...  
void  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...  
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 . More...  
T  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...  
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 Consider the following code example: // The user creates a model. MultibodyTree<double> model; // User adds a body and keeps a reference to it. // 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. const RigidBody<AutoDiffXd>& 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 
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.
T  The scalar type. Must be a valid Eigen scalar. 
Instantiated templates for the following kinds of T's are provided:
They are already available to link against in the containing library. No other values for T are currently supported.

delete 

delete 
MultibodyTree  (  ) 

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:
std::logic_error  if body is a nullptr. 
std::logic_error  if Finalize() was already called on this tree. 
[in]  body  A 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. 
BodyType
to the created body. This reference which will remain valid for the lifetime of this
MultibodyTree.BodyType  The type of the specific subclass of Body to add. The template needs to be specialized on the same scalar type T of this MultibodyTree. 

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:
Note that for dependent names you must use the template keyword (say for instance you have a MultibodyTree<T> member within your custom class):
std::logic_error  if Finalize() was already called on this tree. 
[in]  args  The arguments needed to construct a valid Body of type BodyType . BodyType must provide a public constructor that takes these arguments. 
BodyType
to the created body. This reference which will remain valid for the lifetime of this
MultibodyTree.BodyType  A template for the type of Body to construct. The template will be specialized on the scalar type T of this MultibodyTree. 

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.

inline 

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:
std::logic_error  if frame is a nullptr. 
std::logic_error  if Finalize() was already called on this tree. 
[in]  frame  A 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. 
FrameType
to the created frame. This reference which will remain valid for the lifetime of this
MultibodyTree.FrameType  The type of the specific subclass of Frame to add. The template needs to be specialized on the same scalar type T of this MultibodyTree. 

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:
Note that for dependent names you must use the template keyword (say for instance you have a MultibodyTree<T> member within your custom class):
std::logic_error  if Finalize() was already called on this tree. 
[in]  args  The arguments needed to construct a valid Frame of type FrameType . FrameType must provide a public constructor that takes these arguments. 
FrameType
to the created frame. This reference which will remain valid for the lifetime of this
MultibodyTree.FrameType  A template for the type of Frame to construct. The template will be specialized on the scalar type T of this MultibodyTree. 

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 inboardoutboard 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.
[in]  name  The name of the joint. 
[in]  parent  The parent body connected by the new joint. 
[in]  X_PF  The 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]  child  The child body connected by the new joint. 
[in]  X_BM  The 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. 
JointType  The type of the new joint to add, which must be a subclass of Joint. 
JointType<T>
specialized on the scalar type T of this
MultibodyTree. It will remain valid for the lifetime of this
MultibodyTree.Example of usage:
if  this model already contains a joint with the given name . See HasJointNamed(), Joint::name(). 

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.
[in]  name  A 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]  joint  The Joint to be actuated by the new JointActuator. 
this
MultibodyTree. if  this model already contains a joint actuator with the given name . See HasJointActuatorNamed(), JointActuator::get_name(). 

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:
A Mobilizer effectively connects the two bodies to which the inboard and outboard frames belong.
std::logic_error  if mobilizer is a nullptr. 
std::logic_error  if Finalize() was already called on this tree. 
a  std::runtime_error if the new mobilizer attempts to connect a frame with itself. 
std::runtime_error  if attempting to connect two bodies with more than one mobilizer between them. 
[in]  mobilizer  A 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. 
MobilizerType
to the created mobilizer. This reference which will remain valid for the lifetime of this
MultibodyTree.MobilizerType  The type of the specific subclass of Mobilizer to add. The template needs to be specialized on the same scalar type T of this MultibodyTree. 

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:
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).
std::logic_error  if Finalize() was already called on this tree. 
a  std::runtime_error if the new mobilizer attempts to connect a frame with itself. 
std::runtime_error  if attempting to connect two bodies with more than one mobilizer between them. 
[in]  args  The arguments needed to construct a valid Mobilizer of type MobilizerType . MobilizerType must provide a public constructor that takes these arguments. 
MobilizerType
to the created mobilizer. This reference which will remain valid for the lifetime of this
MultibodyTree.MobilizerType  A template for the type of Mobilizer to construct. The template will be specialized on the scalar type T of this MultibodyTree. 

inline 
Creates a new model instance.
Returns the index for a new model instance (as there is no concrete object beyond the index).
[in]  name  A 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(). 
std::logic_error  if Finalize() was already called on this tree. 

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:
[in]  name  A 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_instance  A model instance index which this body is part of. 
[in]  M_BBo_B  The 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. 
this
MultibodyTree. std::logic_error  if a body named name already exists in this model instance. 
std::logic_error  if the model instance does not exist. 

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:
[in]  name  A 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_B  The 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. 
this
MultibodyTree. std::logic_error  if a body named name already exists. 
std::logic_error  if additional model instances have been created beyond the world and default instances. 
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:
A_WB
for each body B in the model as measured and expressed in the world frame W.[in]  context  The context containing the state of the MultibodyTree model. 
[in]  pc  A position kinematics cache object already updated to be in sync with context . 
[in]  vc  A velocity kinematics cache object already updated to be in sync with context . 
[in]  known_vdot  A vector with the generalized accelerations for the full MultibodyTree model. 
[out]  ac  A pointer to a valid, non nullptr, acceleration kinematics cache. This method aborts if ac is nullptr. 
pc
must have been previously updated with a call to CalcPositionKinematicsCache(). vc
must have been previously updated with a call to CalcVelocityKinematicsCache(). 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
.
[in]  context  The context containing the state of the model. It stores the generalized positions q of the model. 
[out]  X_WB  On 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(). 
if  X_WB is nullptr. 
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
.
[in]  context  The context containing the state of the model. It stores the generalized positions q and velocities v of the model. 
[out]  V_WB  On 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(). 
///
if  V_WB is nullptr. 
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:
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.[in]  context  The context containing the state of the MultibodyTree model. 
[in]  pc  A position kinematics cache object already updated to be in sync with context . 
[out]  abc  A pointer to a valid, non nullptr, articulated body cache. This method throws an exception if abc is a nullptr. 
pc
must have been previously updated with a call to CalcPositionKinematicsCache() using the same context
. 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
.
[in]  context  The context containing the state of the MultibodyTree model. It stores the generalized positions q and the generalized velocities v. 
[out]  Cv  On output, Cv will contain the product C(q, v)v . It must be a valid (nonnull) 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. 
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)
.
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.
[in]  context  The context containing the state of the MultibodyTree model. 
[in]  pc  A position kinematics cache object already updated to be in sync with context . 
[in]  vc  A velocity kinematics cache object already updated to be in sync with context . 
[out]  forces  A 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(). 
pc
must have been previously updated with a call to CalcPositionKinematicsCache(). vc
must have been previously updated with a call to CalcVelocityKinematicsCache(). void CalcFrameGeometricJacobianExpressedInWorld  (  const systems::Context< T > &  context, 
const Frame< T > &  frame_B,  
const Eigen::Ref< const Vector3< T >> &  p_BoFo_B,  
EigenPtr< MatrixX< T >>  Jv_WF  
)  const 
Given a frame F with fixed position p_BoFo_B
in a frame B, this method computes the geometric Jacobian Jv_WF
defined by:
V_WF(q, v) = Jv_WF(q)⋅v
where V_WF(q, v)
is the spatial velocity of frame F measured and expressed in the world frame W and q and v are the vectors of generalized position and velocity, respectively. Since the spatial velocity of frame F is linear in the generalized velocities, the geometric Jacobian Jv_WF
is a function of the generalized coordinates q only.
[in]  context  The context containing the state of the model. It stores the generalized positions q. 
[in]  frame_B  The position p_BoFo_B of frame F is measured and expressed in this frame B. 
[in]  p_BoFo_B  The (fixed) position of frame F as measured and expressed in frame B. 
[out]  Jv_WF  The geometric Jacobian Jv_WF(q) , function of the generalized positions q only. This Jacobian relates to the spatial velocity V_WF of frame F by: V_WF(q, v) = Jv_WF(q)⋅vTherefore Jv_WF is a matrix of size 6 x nv , with nv the number of generalized velocities. On input, matrix Jv_WF must have size 6 x nv or this method throws an exception. The top rows of this matrix (which can be accessed with Jv_WF.topRows<3>()) is the Jacobian Hw_WF related to the angular velocity of F in W by w_WF = Hw_WF⋅v . The bottom rows of this matrix (which can be accessed with Jv_WF.bottomRows<3>()) is the Jacobian Hv_WF related to the translational velocity of the origin of frame F in W by v_WFo = Hw_WF⋅v . This ordering is consistent with the internal storage of the SpatialVector class. Therefore the following operations results in a valid spatial velocity: SpatialVelocity<double> Jv_WF_times_v(Jv_WF * v); 
if  J_WF is nullptr or if it is not of size 6 x nv . 
void CalcInverseDynamics  (  const systems::Context< T > &  context, 
const PositionKinematicsCache< T > &  pc,  
const VelocityKinematicsCache< T > &  vc,  
const VectorX< T > &  known_vdot,  
const std::vector< SpatialForce< T >> &  Fapplied_Bo_W_array,  
const Eigen::Ref< const VectorX< T >> &  tau_applied_array,  
std::vector< SpatialAcceleration< T >> *  A_WB_array,  
std::vector< SpatialForce< T >> *  F_BMo_W_array,  
EigenPtr< VectorX< T >>  tau_array  
)  const 
Given the state of this
MultibodyTree in context
and a known vector of generalized accelerations vdot
, this method computes the set of generalized forces tau
that would need to be applied at each Mobilizer in order to attain the specified generalized accelerations.
Mathematically, this method computes:
tau = M(q)v̇ + C(q, v)v  tau_app  ∑ J_WBᵀ(q) Fapp_Bo_W
where M(q)
is the MultibodyTree mass matrix, C(q, v)v
is the bias term containing Coriolis and gyroscopic effects and tau_app
consists of a vector applied generalized forces. The last term is a summation over all bodies in the model where Fapp_Bo_W
is an applied spatial force on body B at Bo
which gets projected into the space of generalized forces with the geometric Jacobian J_WB(q)
which maps generalized velocities into body B spatial velocity as V_WB = J_WB(q)v
. This method does not compute explicit expressions for the mass matrix nor for the bias term, which would be of at least O(n²)
complexity, but it implements an O(n)
NewtonEuler 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 NewtonEuler algorithm is the most efficient currently known general method for solving inverse dynamics [Featherstone 2008].
[in]  context  The context containing the state of the MultibodyTree model. 
[in]  pc  A position kinematics cache object already updated to be in sync with context . 
[in]  vc  A velocity kinematics cache object already updated to be in sync with context . 
[in]  known_vdot  A 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_array  A 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 nonzero 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_array  An 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 nonzero 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_array  A 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_array  A 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_array  On 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(). 
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.F_BMo_W_array
and tau_array
as the only local temporaries and therefore no additional dynamic memory allocation is performed.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.pc
must have been previously updated with a call to CalcPositionKinematicsCache(). vc
must have been previously updated with a call to CalcVelocityKinematicsCache(). 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
.
[in]  context  The context containing the state of the MultibodyTree model. 
[out]  H  A valid (nonnull) 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 ith
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 ith
column in M(q).
void CalcPointsGeometricJacobianExpressedInWorld  (  const systems::Context< T > &  context, 
const Frame< T > &  frame_B,  
const Eigen::Ref< const MatrixX< T >> &  p_BQi_set,  
EigenPtr< MatrixX< T >>  p_WQi_set,  
EigenPtr< MatrixX< T >>  Jv_WQi  
)  const 
Given a set of points Qi
with fixed position vectors p_BQi
in a frame B, (that is, their time derivative ᴮd/dt(p_BQi)
in frame B is zero), this method computes the geometric Jacobian Jv_WQi
defined by:
v_WQi(q, v) = Jv_WQi(q)⋅v
where p_WQi
is the position vector in the world frame for each point Qi
in the input set, v_WQi(q, v)
is the translational velocity of point Qi
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 Qi
is linear in the generalized velocities, the geometric Jacobian Jv_WQi
is a function of the generalized coordinates q only.
[in]  context  The context containing the state of the model. It stores the generalized positions q. 
[in]  frame_B  The positions p_BQi of each point in the input set are measured and expressed in this frame B and are constant (fixed) in this frame. 
[in]  p_BQi_set  A matrix with the fixed position of a set of points Qi measured and expressed in frame_B . Each column of this matrix contains the position vector p_BQi for a point Qi measured and expressed in frame B. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the set. 
[out]  p_WQi_set  The output positions of each point Qi now computed as measured and expressed in frame W. These positions are computed in the process of computing the geometric Jacobian J_WQi and therefore external storage must be provided. The output p_WQi_set must have the same size as the input set p_BQi_set or otherwise this method throws a std::runtime_error exception. That is p_WQi_set must be in ℝ³ˣⁿᵖ . 
[out]  Jv_WQi  The geometric Jacobian Jv_WQi(q) , function of the generalized positions q only. This Jacobian relates the translational velocity v_WQi of each point Qi in the input set by: `v_WQi(q, v) = Jv_WQi(q)⋅v`so that v_WQi is a column vector of size 3⋅np concatenating the velocity of all points Qi in the same order they were given in the input set. Therefore J_WQi is a matrix of size 3⋅np x nv , with nv the number of generalized velocities. On input, matrix J_WQi must have size 3⋅np x nv or this method throws a std::runtime_error exception. 
an  exception if the output p_WQi_set is nullptr or does not have the same size as the input array p_BQi_set . 
an  exception if Jv_WQi is nullptr or if it does not have the appropriate size, see documentation for Jv_WQi for details. 
void CalcPointsGeometricJacobianExpressedInWorld  (  const systems::Context< T > &  context, 
const Frame< T > &  frame_B,  
const Eigen::Ref< const MatrixX< T >> &  p_WQi_set,  
EigenPtr< MatrixX< T >>  Jv_WQi  
)  const 
This is a variant to compute the geometric Jacobian Jv_WQi
for a set of points Qi
moving with frame_B
, given that we know the position p_WQi
of each point in the set measured and expressed in the world frame W.
The geometric Jacobian Jv_WQi
is defined such that:
v_WQi(q, v) = Jv_WQi(q)⋅v
where v_WQi(q, v)
is the translational velocity of point Qi
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 Qi
is linear in the generalized velocities, the geometric Jacobian Jv_WQi
is a function of the generalized coordinates q only.
[in]  context  The context containing the state of the model. It stores the generalized positions q. 
[in]  frame_B  Points Qi in the set instantaneously move with this frame. 
[in]  p_WQi_set  A matrix with the fixed position of a set of points Qi measured and expressed in the world frame W. Each column of this matrix contains the position vector p_WQi for a point Qi measured and expressed in the world frame W. Therefore this input matrix lives in ℝ³ˣⁿᵖ with np the number of points in the set. 
[out]  Jv_WQi  The geometric Jacobian Jv_WQi(q) , function of the generalized positions q only. This Jacobian relates the translational velocity v_WQi of each point Qi in the input set by: `v_WQi(q, v) = Jv_WQi(q)⋅v`so that v_WQi is a column vector of size 3⋅np concatenating the velocity of all points Qi in the same order they were given in the input set. Therefore J_WQi is a matrix of size 3⋅np x nv , with nv the number of generalized velocities. On input, matrix J_WQi must have size 3⋅np x nv or this method throws a std::runtime_error exception. 
an  exception if Jv_WQi is nullptr or if it does not have the appropriate size, see documentation for Jv_WQi for details. 
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.
[in]  context  The context containing the state of the MultibodyTree model. It stores the generalized positions q of the model. 
[in]  frame_B  The frame B in which the positions p_BQi of a set of points Qi are given. 
[in]  p_BQi  The 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_A  The frame A in which it is desired to compute the positions p_AQi of each point Qi in the set. 
[out]  p_AQi  The 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 ℝ³ˣⁿᵖ . 
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. 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:
X_BF
of each of the frames F attached to body B.X_WB
of each body B in the model as measured and expressed in the world frame W.H_FM
and H_PB_W
.com_W
and M_Bo_W
.Aborts if pc
is nullptr.
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
.
[in]  context  The context containing the state of the MultibodyTree model. 
this
multibody model. 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
.
[in]  context  The context containing the state of the MultibodyTree model. It stores the generalized positions q of the model. 
[in]  frame_A  The target frame A in the computed relative transform X_AB . 
[in]  frame_B  The source frame B in the computed relative transform X_AB . 
X_AB  The relative transform from frame B to frame A, such that p_AQ = X_AB⋅p_BQ . 
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.
[in]  context  The context containing the state of the MultibodyTree model. 
[in]  pc  A position kinematics cache object already updated to be in sync with context . 
[in]  vc  A velocity kinematics cache object already updated to be in sync with context . 
[in]  known_vdot  A vector with the generalized accelerations for the full MultibodyTree model. 
[out]  A_WB_array  A 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(). 
pc
must have been previously updated with a call to CalcPositionKinematicsCache(). vc
must have been previously updated with a call to 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:
V_WB
for each body B in the model as measured and expressed in the world frame W.V_PB
for each body B in the model as measured and expressed in the inboard (or parent) body frame P.pc
must have been previously updated with a call to CalcPositionKinematicsCache().Aborts if vc
is nullptr.

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

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:
Consider the following code example:
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().
std::unique_ptr< systems::LeafContext< T > > CreateDefaultContext  (  )  const 
Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system.
std::logic_error  If users attempt to call this method on a MultibodyTree with an invalid topology. 
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.
[in]  context  The context storing the state of the MultibodyTree model. 
[in]  body_B  The body B for which the pose is requested. 
X_WB  The pose of body frame B in the world frame W. 
if  Finalize() was not called on this model or if body_B does not belong to this model. 
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.
[in]  context  The context storing the state of the MultibodyTree model. 
[in]  body_B  The body B for which the spatial velocity is requested. 
if  Finalize() was not called on this model or if body_B does not belong to this model. 
const PositionKinematicsCache< T > & EvalPositionKinematics  (  const systems::Context< T > &  context  )  const 
Evaluates position kinematics cached in context.
context  A MultibodyTreeContext on which to update position kinematics. 
const VelocityKinematicsCache< T > & EvalVelocityKinematics  (  const systems::Context< T > &  context  )  const 
Evaluates velocity kinematics cached in context.
context  A MultibodyTreeContext on which to update velocity kinematics. 
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 preprocessing 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 uptodate after this call. No more multibody tree elements can be added after a call to Finalize().
std::exception  if called postfinalize. 
Returns a constant reference to the body with unique index body_index
.
if  body_index does not correspond to a body in this multibody tree. 

inline 
Returns a constant reference to the frame with unique index frame_index
.
if  frame_index does not correspond to a frame in this multibody tree. 

inline 
Returns a constant reference to the joint with unique index joint_index
.
std::runtime_error  when joint_index does not correspond to a joint in this multibody tree. 

inline 
Returns a constant reference to the joint actuator with unique index actuator_index
.
if  actuator_index does not correspond to a joint actuator in this multibody tree. 

inline 
Returns a constant reference to the mobilizer with unique index mobilizer_index
.
std::runtime_error  when mobilizer_index does not correspond to a mobilizer in this multibody tree. 
VectorX< T > get_positions_from_array  (  ModelInstanceIndex  model_instance, 
const Eigen::Ref< const VectorX< T >> &  q_array  
)  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 aborts if q_array
is not of size MultibodyTree::num_positions().

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.

inline 
SFINAE overload for Frame<T> elements.

inline 
SFINAE overload for Body<T> elements.

inline 
SFINAE overload for Mobilizer<T> elements.

inline 
SFINAE overload for Joint<T> elements.
VectorX< T > get_velocities_from_array  (  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_array
of generalized velocities for the entire MultibodyTree model.
This method aborts if the input array is not of size MultibodyTree::num_velocities().

inline 
Returns a constant reference to a body that is identified by the string name
in this
model.
std::logic_error  if there is no body with the requested name. 
std::logic_error  if the body name occurs in multiple model instances. 
this
model with a given specified name.

inline 
Returns a constant reference to the body that is uniquely identified by the string name
in model_instance
.
std::logic_error  if there is no body with the requested name. 
std::runtime_error  if model_instance is not valid for this model. 
this
model with a given specified name.

inline 
Returns a constant reference to an actuator that is identified by the string name
in this
model.
std::logic_error  if there is no actuator with the requested name. 
std::logic_error  if the actuator name occurs in multiple model instances. 
this
model with a given specified name.

inline 
Returns a constant reference to the actuator that is uniquely identified by the string name
in model_instance
.
std::logic_error  if there is no actuator with the requested name. 
std::runtime_error  if model_instance is not valid for this model. 
this
model with a given specified name.

inline 
Returns a constant reference to a joint that is identified by the string name
in this
model.
std::logic_error  if there is no joint with the requested name. 
std::logic_error  if the joint name occurs in multiple model instances. 
this
model with a given specified name.

inline 
Returns a constant reference to the joint that is uniquely identified by the string name
in model_instance
.
std::logic_error  if there is no joint with the requested name. 
std::runtime_error  if model_instance is not valid for this model. 
this
model with a given specified name.

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.
JointType  The specific type of the Joint to be retrieved. It must be a subclass of Joint. 
std::logic_error  if the named joint is not of type JointType or if there is no Joint with that name. 
std::logic_error  if the joint name occurs in multiple model instances. 
this
model with a given specified name.

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.
JointType  The specific type of the Joint to be retrieved. It must be a subclass of Joint. 
std::logic_error  if the named joint is not of type JointType or 
std::runtime_error  if model_instance is not valid for this model. if there is no Joint with that name. 
this
model with a given specified name.

inline 
Returns the index to the model instance that is uniquely identified by the string name
in this
model.
std::logic_error  if there is no instance with the requested name. 
this
model with a given specified name.

inline 
Returns a constant reference to a rigid body that is identified by the string name
in this
model.
std::logic_error  if there is no body with the requested name. 
std::logic_error  if the body name occurs in multiple model instances. 
std::logic_error  if the requested body is not a RigidBody. 
this
model with a given specified name.

inline 
Returns a constant reference to the rigid body that is uniquely identified by the string name
in model_instance
.
std::logic_error  if there is no body with the requested name. 
std::logic_error  if the requested body is not a RigidBody. 
std::runtime_error  if model_instance is not valid for this model. 
this
model with a given specified name.

inline 
true
if a body named name
was added to the model. std::logic_error  if the body name occurs in multiple model instances. 

inline 
true
if a body named name
was added to model_instance
. if  model_instance is not valid for this model. 

inline 
true
if a joint actuator named name
was added to the model. std::logic_error  if the actuator name occurs in multiple model instances. 

inline 
true
if a joint actuator named name
was added to model_instance
. if  model_instance is not valid for this model. 

inline 
true
if a joint named name
was added to the model. std::logic_error  if the joint name occurs in multiple model instances. 

inline 
true
if a joint named name
was added to model_instance
. if  model_instance is not valid for this model. 

inline 
true
if a model instance named name
was added to the model. 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 q̇
are related linearly by q̇ = N(q)⋅v
. Although N(q)
is not necessarily square, its left pseudoinverse 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̇
.
[in]  context  The context containing the state of the MultibodyTree model. 
[in]  qdot  A vector containing the time derivatives of the generalized positions. This method aborts if qdot is not of size num_positions(). 
[out]  v  A valid (nonnull) 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(). 
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
.
[in]  context  The context containing the state of the MultibodyTree model. 
[in]  v  A vector of of generalized velocities for this MultibodyTree model. This method aborts if v is not of size num_velocities(). 
[out]  qdot  A valid (nonnull) 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(). 

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

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

inline 
Returns the number of actuators in the model.

inline 
Returns the number of bodies in the MultibodyTree including the world body.
Therefore the minimum number of bodies in a MultibodyTree is one.

inline 
Returns the number of ForceElement objects in the MultibodyTree.

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.

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

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.

inline 
Returns the number of model instances in the MultibodyTree.

inline 
Returns the number of generalized positions of the model.

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

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

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

inline 
Returns the number of generalized velocities of the model.

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

delete 

delete 
void set_actuation_vector  (  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.
[in]  u_instance  Actuation 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]  u  The vector containing the actuation values for the entire MultibodyTree. 
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().
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().
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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 
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.
std::exception  if body is not a free body in the model. 
std::exception  if called prefinalize. 

inline 
Creates a deep copy of this
MultibodyTree templated on AutoDiffXd.

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.

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.

inline 
Returns a constant reference to the world body.

inline 
Returns a constant reference to the world frame.

friend 

friend 