Drake
BodyNode< T > Class Template Reference

For internal use only of the MultibodyTree implementation. More...

#include <multibody/multibody_tree/body_node.h>

Inheritance diagram for BodyNode< T >:
[legend]
Collaboration diagram for BodyNode< T >:
[legend]

## 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 base-to-tip 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 base-to-tip 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 base-to-tip 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 BodyNodeTopologyget_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

BodyNodeoperator= (const BodyNode &)=delete

BodyNode (BodyNode &&)=delete

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

## Detailed Description

### template<typename T> class drake::multibody::internal::BodyNode< T >

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.

#### Tree Structure

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:

• a body B in a given MultibodyTree,
• the outboard frame M attached to this body B,
• the inboard frame F attached to the unique parent body P of body B,
• the mobilizer connecting the inboard frame F with the outboard frame M.

#### Associated State

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.

• [Jain 2010] Jain, A., 2010. Robot and multibody dynamics: analysis and algorithms. Springer Science & Business Media.
Template Parameters
 T The scalar type. Must be a valid Eigen scalar.

## Constructor & Destructor Documentation

 BodyNode ( const BodyNode< T > & )
delete
 BodyNode ( BodyNode< T > && )
delete
 BodyNode ( const BodyNode< T > * parent_node, const Body< T > * body, const Mobilizer< T > * mobilizer )
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.

Parameters
 [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.
Note
BodyNode keeps a reference to the parent body, body and mobilizer for this node, which must outlive this BodyNode.

Here is the call graph for this function:

## Member Function Documentation

 void add_child_node ( const BodyNode< T > * child )
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.

 void CalcAcrossNodeGeometricJacobianExpressedInWorld ( const MultibodyTreeContext< T > & context, const PositionKinematicsCache< T > & pc, EigenPtr< MatrixX< T >> H_PB_W ) const
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.

Parameters
 [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.
Precondition
The position kinematics cache pc was already updated to be in sync with context by MultibodyTree::CalcPositionKinematicsCache().

Here is the call graph for this function:

 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
inline

Computes the generalized forces tau for a single BodyNode.

This method is used by MultibodyTree within a tip-to-base 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.

Parameters
 [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 non-null 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.
Note
There is no mechanism to assert that either 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.
Precondition
The position kinematics cache pc was already updated to be in sync with context by MultibodyTree::CalcPositionKinematicsCache().
The velocity kinematics cache vc was already updated to be in sync with context by MultibodyTree::CalcVelocityKinematicsCache().
CalcInverseDynamics_TipToBase() must have already been called for all the child nodes of this node (and, by recursive precondition, all successor nodes in the tree.)

Here is the call graph for this function:

 void CalcPositionKinematicsCache_BaseToTip ( const MultibodyTreeContext< T > & context, PositionKinematicsCache< T > * pc ) const
inline

This method is used by MultibodyTree within a base-to-tip loop to compute this node's kinematics that only depend on generalized positions.

This method aborts in Debug builds when:

• Called on the root node.
• pc is nullptr.
Parameters
 [in] context The context with the state of the MultibodyTree model. [out] pc A pointer to a valid, non nullptr, kinematics cache.
Precondition
CalcPositionKinematicsCache_BaseToTip() must have already been called for the parent node (and, by recursive precondition, all predecessor nodes in the tree.)

Here is the call graph for this function:

 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
inline

This method is used by MultibodyTree within a base-to-tip 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:

• Called on the root node.
• ac is nullptr.
Parameters
 [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::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 pre-computed 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.
Precondition
The position kinematics cache pc was already updated to be in sync with context by MultibodyTree::CalcPositionKinematicsCache().
The velocity kinematics cache vc was already updated to be in sync with context by MultibodyTree::CalcVelocityKinematicsCache().
CalcAccelerationKinematicsCache_BaseToTip() must have already been called for the parent node (and, by recursive precondition, all predecessor nodes in the tree). Therefore, on input, the argument array A_WB_array_ptr must contain already pre-computed spatial accelerations for the inboard bodies to this node's body B.

Here is the call graph for this function:

 void CalcVelocityKinematicsCache_BaseToTip ( const MultibodyTreeContext< T > & context, const PositionKinematicsCache< T > & pc, const Eigen::Ref< const MatrixUpTo6< T >> & H_PB_W, VelocityKinematicsCache< T > * vc ) const
inline

This method is used by MultibodyTree within a base-to-tip loop to compute this node's kinematics that depend on the generalized velocities.

This method aborts in Debug builds when:

• Called on the root node.
• vc is nullptr.
Parameters
 [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 across-node 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.
Precondition
The position kinematics cache pc was already updated to be in sync with context by MultibodyTree::CalcPositionKinematicsCache().
CalcVelocityKinematicsCache_BaseToTip() must have already been called for the parent node (and, by recursive precondition, all predecessor nodes in the tree.)

Here is the call graph for this function:

 const Body& get_body ( ) const
inline

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

Here is the caller graph for this function:

 const Frame& get_inboard_frame ( ) const
inlineprotected

Returns the inboard frame F of this node's mobilizer.

Exceptions
 std::runtime_error if called on the root node corresponding to the world body.

Here is the call graph for this function:

Here is the caller graph for this function:

 const Mobilizer& get_mobilizer ( ) const
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.

Here is the caller graph for this function:

 int get_num_mobilizer_positions ( ) const
inline

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

 int get_num_mobilizer_velocites ( ) const
inline

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

Here is the caller graph for this function:

 const Frame& get_outboard_frame ( ) const
inlineprotected

Returns the outboard frame M of this node's mobilizer.

Exceptions
 std::runtime_error if called on the root node corresponding to the world body.

Here is the call graph for this function:

Here is the caller graph for this function:

 const Body& get_parent_body ( ) const
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.

Here is the caller graph for this function:

 const BodyNodeTopology& get_topology ( ) const
inline

Returns the topology information for this body node.

Here is the caller graph for this function:

 Eigen::Map > GetJacobianFromArray ( const std::vector< Vector6< T >> & H_array ) const
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 per-node Jacobian matrices from a std::vector that would usually live in the cache.

Parameters
 [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().
Return values
 H An Eigen::Map to a matrix of size 6 x nm corresponding to the Jacobian matrix for this node.

Here is the call graph for this function:

 Eigen::Map > GetMutableJacobianFromArray ( std::vector< Vector6< T >> * H_array ) const
inline

Mutable version of GetJacobianFromArray().

Here is the call graph for this function:

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

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