Drake

For internal use only of the MultibodyTree implementation. More...
#include <multibody/multibody_tree/body_node.h>
Public Member Functions  
BodyNode (const BodyNode< T > *parent_node, const Body< T > *body, const Mobilizer< T > *mobilizer)  
A node encompasses a Body in a MultibodyTree and the inboard Mobilizer that connects this body to the rest of tree. More...  
void  add_child_node (const BodyNode< T > *child) 
Method to update the list of child body nodes maintained by this node, outboard to this node. More...  
const Body< T > &  get_body () const 
Returns a constant reference to the body B associated with this node. More...  
const Body< T > &  get_parent_body () const 
Returns a constant reference to the unique parent body P of the body B associated with this node. More...  
const Mobilizer< T > &  get_mobilizer () const 
Returns a constant reference to the mobilizer associated with this node. More...  
void  CalcPositionKinematicsCache_BaseToTip (const MultibodyTreeContext< T > &context, PositionKinematicsCache< T > *pc) const 
This method is used by MultibodyTree within a basetotip loop to compute this node's kinematics that only depend on generalized positions. More...  
void  CalcVelocityKinematicsCache_BaseToTip (const MultibodyTreeContext< T > &context, const PositionKinematicsCache< T > &pc, const Eigen::Ref< const MatrixUpTo6< T >> &H_PB_W, VelocityKinematicsCache< T > *vc) const 
This method is used by MultibodyTree within a basetotip loop to compute this node's kinematics that depend on the generalized velocities. More...  
void  CalcSpatialAcceleration_BaseToTip (const MultibodyTreeContext< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &mbt_vdot, std::vector< SpatialAcceleration< T >> *A_WB_array_ptr) const 
This method is used by MultibodyTree within a basetotip loop to compute this node's kinematics that depend on the generalized accelerations, i.e. More...  
void  CalcInverseDynamics_TipToBase (const MultibodyTreeContext< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const std::vector< SpatialAcceleration< T >> &A_WB_array, const SpatialForce< T > &Fapplied_Bo_W, const Eigen::Ref< const VectorX< T >> &tau_applied, std::vector< SpatialForce< T >> *F_BMo_W_array_ptr, EigenPtr< VectorX< T >> tau_array) const 
Computes the generalized forces tau for a single BodyNode. More...  
const BodyNodeTopology &  get_topology () const 
Returns the topology information for this body node. More...  
void  CalcAcrossNodeGeometricJacobianExpressedInWorld (const MultibodyTreeContext< T > &context, const PositionKinematicsCache< T > &pc, EigenPtr< MatrixX< T >> H_PB_W) const 
Computes the geometric Jacobian H_PB_W which relates to the spatial velocity of a body B in its parent body P by V_PB_W = H_PB_W(q)⋅v_B , where v_B denotes the generalized velocities associated with body B's node. More...  
Eigen::Map< const MatrixUpTo6< T > >  GetJacobianFromArray (const std::vector< Vector6< T >> &H_array) const 
Helper method to retrieve a Jacobian matrix for this node from an array storing the columns of a set of Jacobian matrices for each node. More...  
Eigen::Map< MatrixUpTo6< T > >  GetMutableJacobianFromArray (std::vector< Vector6< T >> *H_array) const 
Mutable version of GetJacobianFromArray(). More...  
Does not allow copy, move, or assignment  
BodyNode (const BodyNode &)=delete  
BodyNode &  operator= (const BodyNode &)=delete 
BodyNode (BodyNode &&)=delete  
BodyNode &  operator= (BodyNode &&)=delete 
Methods to retrieve BodyNode sizes  
int  get_num_mobilizer_positions () const 
Returns the number of generalized positions for the Mobilizer in this node. More...  
int  get_num_mobilizer_velocites () const 
Returns the number of generalized velocities for the Mobilizer in this node. More...  
Protected Member Functions  
const Frame< T > &  get_inboard_frame () const 
Returns the inboard frame F of this node's mobilizer. More...  
const Frame< T > &  get_outboard_frame () const 
Returns the outboard frame M of this node's mobilizer. More...  
For internal use only of the MultibodyTree implementation.
This is a base class representing a node in the tree structure of a MultibodyTree. BodyNode provides implementations for convenience methods to be used in MultibodyTree recursive algorithms but that however should not leak into the public API for the Mobilizer class. In this regard, BodyNode provides an additional separation layer between implementation internals and user facing API.
As a tree data structure, a MultibodyTree can be thought of as collection of BodyNode objects where each body node has a number of BodyNode children and a unique parent BodyNode object. Each BodyNode is associated with a given body B and an inboard mobilizer that connects this body B to the rest of the tree. The unique parent body of body B is denoted by P, which in turn has its own BodyNode associated with it. Associated with each BodyNode is an inboard frame F attached on body P and an outboard frame M attached to body B. The relationship between frames F and M is dictated by the body B's inboard mobilizer providing the pose X_FM
as a function of the generalized coordinates associated with that mobilizer.
In addition, body B could be a flexible body, in which case the pose of each frame attached to B would in general be a function of a number of generalized positions associated with body B. In particular, the pose X_BM
of the outboard frame M will be a function of body B's generalized positions while the pose X_PF
of the inboard frame F will be a function of parent body P's generalized positions. A RigidBody has no generalized positions associated with it (see RigidBody::get_num_flexible_positions()).
In summary, there will a BodyNode for each Body in the MultibodyTree which encompasses:
In the same way a Mobilizer and a Body have a number of generalized positions associated with them, a BodyNode is associated with the generalized positions of body B and of its inboard mobilizer.
The relationship between frames F and M is dictated by the body B's inboard mobilizer providing the pose X_FM(qm_B)
as a function of the generalized coordinates qm_B
(where m
refers to "mobilizer" and _B
refers to the fact this is the unique inboard mobilizer of body B.)
In addition, body B could be a flexible body, in which case the pose of each frame attached to B would in general be a function of the generalized positions qb_B
for body B (where b
refers to "body" and _B
refers to body B in particular.) In particular, the pose X_BM(qb_B)
of the outboard frame M will be a function of body B's generalized positions qb_B
while the pose X_PF(qb_P)
of the inboard frame F will be a function of parent body P's generalized positions qb_P
.
Therefore, the generalized positions associated with a given body node correspond to the concatenation qn_B = [qm_B, qb_B]
. Similarly, mobilizer's generalized velocities vm_B
and body generalized velocities vb_B
are grouped into vn_B = [vm_B, vb_B]
. [Jain 2010] uses a similar grouping of generalized coordinates when flexible bodies are considered, see Chapter 13.
T  The scalar type. Must be a valid Eigen scalar. 

inline 
A node encompasses a Body in a MultibodyTree and the inboard Mobilizer that connects this body to the rest of tree.
Given a body and its inboard mobilizer in a MultibodyTree this constructor creates the corresponding BodyNode. See this class' documentation for details on how a BodyNode is defined.
[in]  parent_node  A const pointer to the parent BodyNode object in the tree structure of the owning MultibodyTree. It can be a nullptr only when body is the world body, otherwise this constructor will abort. 
[in]  body  The body B associated with this node. 
[in]  mobilizer  The mobilizer associated with this node . It can be a nullptr only when body is the world body, otherwise this method will abort. 
this
BodyNode.

inline 
Method to update the list of child body nodes maintained by this node, outboard to this node.
Recall a BodyNode is a tree node within the tree structure of MultibodyTree. Therefore each BodyNode has a unique parent BodyNode, supplied at construction, and a set of child nodes, specified via calls to this method. Used by MultibodyTree at creation of a BodyNode during the MultibodyTree::Finalize() method call.

inline 
Computes the geometric Jacobian H_PB_W
which relates to the spatial velocity of a body B in its parent body P by V_PB_W = H_PB_W(q)⋅v_B
, where v_B denotes the generalized velocities associated with body B's node.
H_PB_W ∈ ℝ⁶ˣⁿᵐ
where nm
is the number of mobilities associated with body B's node. H_PB_W(q)
is a function of the model's generalized positions q only.
[in]  context  The context with the state of the MultibodyTree model. 
[in]  pc  An already updated position kinematics cache in sync with context . 
[out]  H_PB_W  The geometric Jacobian which relates the velocity V_PB_W of this node's body B in its parent body P, expressed in W, by V_PB_W = H_PB_W⋅v_B . 
pc
was already updated to be in sync with context
by MultibodyTree::CalcPositionKinematicsCache().

inline 
Computes the generalized forces tau
for a single BodyNode.
This method is used by MultibodyTree within a tiptobase loop to compute the vector of generalized forces tau
that would correspond with a known set of spatial accelerations A_WB
for each body in the MultibodyTree.
This method aborts in Debug builds when F_BMo_W_array
is nullptr.
[in]  context  The context with the state of the MultibodyTree model. 
[in]  pc  An already updated position kinematics cache in sync with context . 
[in]  vc  An already updated velocity kinematics cache in sync with context . 
[in]  A_WB_array  A vector of known spatial accelerations containing the spatial acceleration A_WB for each body in the MultibodyTree model. It must be of size equal to the number of bodies in the MultibodyTree and ordered by BodyNodeIndex. The calling MultibodyTree method must guarantee these conditions are satisfied. 
[in]  Fapplied_Bo_W  Externally applied spatial force on this node's body B at the body's frame origin Bo , expressed in the world frame. Fapplied_Bo_W must not be an entry into F_BMo_W_array_ptr , which would result in undefined results. 
[in]  tau_applied  Externally applied generalized force at this node's mobilizer. It can have zero size, implying no generalized forces are applied. Otherwise it must have a size equal to the number of generalized velocities for this node's mobilizer, see get_num_mobilizer_velocites(). tau_applied must not be an entry into tau_array , which would result in undefined results. 
[out]  F_BMo_W_array_ptr  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 and ordered by BodyNodeIndex. The calling MultibodyTree method must guarantee these conditions are satisfied. This method will abort if the the pointer is null. To access a mobilizer's reaction force on a given body B, access this array with the index returned by Body::get_node_index(). 
[out]  tau_array  A nonnull pointer to the output vector of generalized forces that would result in body B having spatial acceleration A_WB . This method will abort if the the pointer is null. The calling MultibodyTree method must guarantee the size of the array is the number of generalized velocities in the model. 
A_WB_array
nor F_BMo_W_array_ptr
are ordered by BodyNodeIndex and the correctness of MultibodyTree methods, properly unit tested, should guarantee this condition.pc
was already updated to be in sync with context
by MultibodyTree::CalcPositionKinematicsCache(). vc
was already updated to be in sync with context
by MultibodyTree::CalcVelocityKinematicsCache(). this
node (and, by recursive precondition, all successor nodes in the tree.)

inline 
This method is used by MultibodyTree within a basetotip loop to compute this node's kinematics that only depend on generalized positions.
This method aborts in Debug builds when:
pc
is nullptr. [in]  context  The context with the state of the MultibodyTree model. 
[out]  pc  A pointer to a valid, non nullptr, kinematics cache. 

inline 
This method is used by MultibodyTree within a basetotip loop to compute this node's kinematics that depend on the generalized accelerations, i.e.
the generalized velocities' time derivatives. This method aborts in Debug builds when:
ac
is nullptr. [in]  context  The context with the state of the MultibodyTree model. 
[in]  pc  An already updated position kinematics cache in sync with context . 
[in]  vc  An already updated velocity kinematics cache in sync with context . 
[in]  mbt_vdot  The entire vector of generalized accelerations for the full MultibodyTree model. It must have a size equal to the number of generalized velocities in the model. This method assumes the caller, MultibodyTree<T>::CalcAccelerationKinematicsCache(), provides a vector of the right size. 
[in,out]  A_WB_array_ptr  A pointer to a valid, non nullptr, vector of spatial accelerations containing the spatial acceleration A_WB for each body. On input, it must contain already precomputed spatial accelerations for the inboard bodies to this node's body B, see precondition below. It must be of size equal to the number of bodies in the MultibodyTree and ordered by BodyNodeIndex. The calling MultibodyTree method must guarantee these conditions are satisfied. This method will abort if the the pointer is null. There is no mechanism to assert that A_WB_array_ptr is ordered by BodyNodeIndex and the correctness of MultibodyTree methods, properly unit tested, should guarantee this condition. 
pc
was already updated to be in sync with context
by MultibodyTree::CalcPositionKinematicsCache(). vc
was already updated to be in sync with context
by MultibodyTree::CalcVelocityKinematicsCache(). A_WB_array_ptr
must contain already precomputed spatial accelerations for the inboard bodies to this node's body B.

inline 
This method is used by MultibodyTree within a basetotip loop to compute this node's kinematics that depend on the generalized velocities.
This method aborts in Debug builds when:
vc
is nullptr. [in]  context  The context with the state of the MultibodyTree model. 
[in]  pc  An already updated position kinematics cache in sync with context . 
[in]  H_PB_W  The acrossnode Jacobian matrix that relates to the spatial velocity V_PB_W of this node's body B in its parent node body P, expressed in the world frame W, with this node's generalized velocities (or mobilities) v_B by V_PB_W = H_PB_W⋅v_B . 
[out]  vc  A pointer to a valid, non nullptr, velocity kinematics cache. 
pc
was already updated to be in sync with context
by MultibodyTree::CalcPositionKinematicsCache().

inline 
Returns a constant reference to the body B associated with this node.

inlineprotected 
Returns the inboard frame F of this node's mobilizer.
std::runtime_error  if called on the root node corresponding to the world body. 

inline 
Returns a constant reference to the mobilizer associated with this node.
Aborts if called on the root node corresponding to the world body, for which there is no mobilizer.

inline 
Returns the number of generalized positions for the Mobilizer in this
node.

inline 
Returns the number of generalized velocities for the Mobilizer in this
node.

inlineprotected 
Returns the outboard frame M of this node's mobilizer.
std::runtime_error  if called on the root node corresponding to the world body. 

inline 
Returns a constant reference to the unique parent body P of the body B associated with this node.
This method aborts in Debug builds if called on the root node corresponding to the world body.

inline 
Returns the topology information for this body node.

inline 
Helper method to retrieve a Jacobian matrix for this
node from an array storing the columns of a set of Jacobian matrices for each node.
This method is used by MultibodyTree implementations to retrieve pernode Jacobian matrices from a std::vector
that would usually live in the cache.
[in]  H_array  This array stores a Jacobian matrix H for each node in the tree. Each matrix has size 6 x nm with nm the number of mobilities of the node. H_array stores the columns of these matrices and therefore it consists of a std::vector of vectors in ℝ⁶ with as many entries as the number of generalized velocities in the model. H_array must be of size MultibodyTree::get_num_velocities(). 
H  An Eigen::Map to a matrix of size 6 x nm corresponding to the Jacobian matrix for this node. 

inline 