Drake
RigidBody< T > Class Template Reference

#include <drake/attic/multibody/rigid_body_actuator.h>

Public Types

typedef CollisionElementsVector::iterator CollisionElementsIterator
 

Public Member Functions

 RigidBody ()
 
std::unique_ptr< RigidBody< T > > Clone () const
 Returns a clone of this RigidBody. More...
 
const std::string & get_name () const
 Returns the name of this rigid body. More...
 
void set_name (const std::string &name)
 Sets the name of this rigid body. More...
 
const std::string & get_model_name () const
 Returns the name of the model defining this rigid body. More...
 
void set_model_name (const std::string &name)
 Sets the name of the model defining this rigid body. More...
 
int get_model_instance_id () const
 Returns the ID of the model instance to which this rigid body belongs. More...
 
void set_model_instance_id (int model_instance_id)
 Sets the ID of the model instance to which this rigid body belongs. More...
 
void setJoint (std::unique_ptr< DrakeJoint > joint)
 Sets the parent joint through which this rigid body connects to its parent rigid body. More...
 
template<typename JointType >
JointType * add_joint (RigidBody *parent, std::unique_ptr< JointType > joint)
 Adds degrees of freedom to this body by connecting it to parent with joint. More...
 
const DrakeJointgetJoint () const
 An accessor to this rigid body's parent joint. More...
 
DrakeJointget_mutable_joint ()
 An accessor to this rigid body's mutable inboard joint. More...
 
bool has_joint () const
 Reports if the body has a parent joint. More...
 
void set_parent (RigidBody *parent)
 Sets the parent rigid body. More...
 
const RigidBodyget_parent () const
 Returns a const pointer to this rigid body's parent rigid body. More...
 
bool has_parent_body () const
 Returns whether this RigidBody has a "parent", which is a RigidBody that is connected to this RigidBody via a DrakeJoint and is closer to the root of the RigidBodyTree relative to this RigidBody. More...
 
bool hasParent () const
 
bool has_as_parent (const RigidBody &other) const
 Checks if a particular rigid body is the parent of this rigid body. More...
 
void set_body_index (int body_index)
 Sets the "body index" of this RigidBody. More...
 
int get_body_index () const
 Returns the "body index" of this RigidBody. More...
 
void set_position_start_index (int position_start_index)
 Sets the start index of this rigid body's mobilizer joint's contiguous generalized coordinates q (joint position state variables) within the full RigidBodyTree generalized coordinate vector. More...
 
int get_position_start_index () const
 Returns the start index of this body's parent jont's position states; see RigidBody::set_position_start_index() for more information. More...
 
void set_velocity_start_index (int velocity_start_index)
 Sets the start index of this rigid body's mobilizer joint's contiguous generalized velocity v (joint velocity state variables) within the full RigidBodyTree generalized velocity vector. More...
 
int get_velocity_start_index () const
 Returns the start index of this body's parent jont's velocity states; see RigidBody::set_velocity_start_index() for more information. More...
 
void AddVisualElement (const DrakeShapes::VisualElement &elements)
 
const DrakeShapes::VectorOfVisualElementsget_visual_elements () const
 
void AddCollisionElement (const std::string &group_name, drake::multibody::collision::Element *element)
 Adds the given collision element to the body with the given group name. More...
 
const std::vector< drake::multibody::collision::ElementId > & get_collision_element_ids () const
 
std::vector< drake::multibody::collision::ElementId > & get_mutable_collision_element_ids ()
 Returns a reference to an std::vector of collision elements that represent the collision geometry of this rigid body. More...
 
const std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_group_to_collision_ids_map () const
 Returns a map of collision element group names to vectors of collision element IDs. More...
 
std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_mutable_group_to_collision_ids_map ()
 Returns a map of collision element group names to vectors of collision element IDs. More...
 
bool IsRigidlyFixedToWorld () const
 Reports if there is a path in this tree from this body to the world where all joints are fixed. More...
 
Eigen::Isometry3d ComputeWorldFixedPose () const
 Reports X_WBₙ, the pose of this body, Bₙ, in the world frame based on the rigid kinematic path from Bₙ to W. More...
 
bool adjacentTo (const RigidBody &other) const
 Reports if this body is considered "adjacent" to the given body. More...
 
bool CanCollideWith (const RigidBody &other) const
 Returns true if this body should be checked for collisions with the other body. More...
 
bool appendCollisionElementIdsFromThisBody (const std::string &group_name, std::vector< drake::multibody::collision::ElementId > &ids) const
 
bool appendCollisionElementIdsFromThisBody (std::vector< drake::multibody::collision::ElementId > &ids) const
 
const Eigen::Matrix3Xd & get_contact_points () const
 Returns the points on this rigid body that should be checked for collision with the environment. More...
 
void set_contact_points (const Eigen::Matrix3Xd &contact_points)
 Saves the points on this rigid body that should be checked for collision between this rigid body and the environment. More...
 
void set_mass (double mass)
 Sets the mass of this rigid body. More...
 
double get_mass () const
 Returns the mass of this rigid body. More...
 
void set_center_of_mass (const Eigen::Vector3d &center_of_mass)
 Sets the center of mass of this rigid body. More...
 
const Eigen::Vector3d & get_center_of_mass () const
 Gets the center of mass of this rigid body. More...
 
void set_spatial_inertia (const drake::SquareTwistMatrix< double > &inertia_matrix)
 Sets the spatial inertia of this rigid body. More...
 
const drake::SquareTwistMatrix< double > & get_spatial_inertia () const
 Returns the spatial inertia of this rigid body. More...
 
void ApplyTransformToJointFrame (const Eigen::Isometry3d &transform_body_to_joint)
 Transforms all of the visual, collision, and inertial elements associated with this body to the proper joint frame. More...
 
CollisionElementsIterator collision_elements_begin ()
 
CollisionElementsIterator collision_elements_end ()
 
int get_num_collision_elements () const
 Reports the total number of registered collision elements attached to this body. More...
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef std::vector< drake::multibody::collision::Element * > CollisionElementsVector
 

Friends

std::ostream & operator<< (std::ostream &out, const RigidBody< double > &b)
 

Member Typedef Documentation

◆ CollisionElementsIterator

typedef CollisionElementsVector::iterator CollisionElementsIterator

Constructor & Destructor Documentation

◆ RigidBody()

RigidBody ( )

Member Function Documentation

◆ add_joint()

JointType* add_joint ( RigidBody< T > *  parent,
std::unique_ptr< JointType >  joint 
)
inline

Adds degrees of freedom to this body by connecting it to parent with joint.

The body takes ownership of the joint.

This method aborts with an error message if the user attempts to assign a joint to a body that already has one.

Note that this is specifically a tree joint and that by "parent" we mean a body that is closer to "world" in the tree topology.

The parent pointer is copied and stored meaning its lifetime must exceed the lifetime of this RigidBody.

Parameters
[in]parentThe RigidBody this body gets connected to.
[in]jointThe DrakeJoint connecting this body to parent and adding degrees of freedom to this body.
Returns
A pointer to the joint just added to the body.

◆ AddCollisionElement()

void AddCollisionElement ( const std::string &  group_name,
drake::multibody::collision::Element element 
)

Adds the given collision element to the body with the given group name.

Parameters
[in]group_nameThe collision element's group name.
[in]elementThe element to associate with the rigid body.
Precondition
element has not already been added to this body.

◆ AddVisualElement()

void AddVisualElement ( const DrakeShapes::VisualElement elements)

◆ adjacentTo()

bool adjacentTo ( const RigidBody< T > &  other) const

Reports if this body is considered "adjacent" to the given body.

"Adjacency" refers to the idea that the bodies are connected to each other in the rigid body tree by a non-floating joint. By this definition, a rigid body is not adjacent to itself.

In the degenerate case where one rigid body is a parent of the other, but with no joint assigned, the rigid bodies will be considered adjacent. Conversely, the degenerate case where a joint is assigned, but the parent relationship is not set, the rigid bodies will not be considered adjacent.

Parameters
[in]otherThe body to test against this body.
Returns
true if the bodies are "adjacent".

◆ appendCollisionElementIdsFromThisBody() [1/2]

bool appendCollisionElementIdsFromThisBody ( const std::string &  group_name,
std::vector< drake::multibody::collision::ElementId > &  ids 
) const

◆ appendCollisionElementIdsFromThisBody() [2/2]

bool appendCollisionElementIdsFromThisBody ( std::vector< drake::multibody::collision::ElementId > &  ids) const

◆ ApplyTransformToJointFrame()

void ApplyTransformToJointFrame ( const Eigen::Isometry3d &  transform_body_to_joint)

Transforms all of the visual, collision, and inertial elements associated with this body to the proper joint frame.

This is necessary, for instance, to support SDF loading where the child frame can be specified independently from the joint frame. In our RigidBodyTree classes, the body frame IS the joint frame.

Parameters
transform_body_to_jointThe transform from this body's frame to the joint's frame.

◆ CanCollideWith()

bool CanCollideWith ( const RigidBody< T > &  other) const

Returns true if this body should be checked for collisions with the other body.

CanCollideWith should be commutative: A can collide with B implies B can collide with A.

Parameters
[in]otherThe body to query against.
Returns
true if collision between this and other should be tested.

◆ Clone()

std::unique_ptr< RigidBody< T > > Clone ( ) const

Returns a clone of this RigidBody.

Attention
The following are not cloned:
  • the joint
  • the parent RigidBody
  • the visual elements
  • the collision elements

The parent is not cloned because the reference to it can only be determined by the RigidBodyTree (which owns both this body and the parent body). Both the parent and the joint are expected to be set by calling add_joint().

The visual and collision elements will be cloned pending identified need.

◆ collision_elements_begin()

CollisionElementsIterator collision_elements_begin ( )
inline

◆ collision_elements_end()

CollisionElementsIterator collision_elements_end ( )
inline

◆ ComputeWorldFixedPose()

Isometry3d ComputeWorldFixedPose ( ) const

Reports X_WBₙ, the pose of this body, Bₙ, in the world frame based on the rigid kinematic path from Bₙ to W.

As such, the world-fixed pose is only defined for bodies that are rigidly fixed to the world.

For this body, with depth n in the tree, Bₙ, X_WBₙ is defined as:

X_WBₙ ≡ X_WB₁ * X_B₁B₂ * ... * X_Bₙ₋₂Bₙ₋₁ * X_Bₙ₋₁Bₙ

X_Bₖ₋₁Bₖ represents the transform from one body's frame (Bₖ) to its parent's frame (Bₖ₋₁). By construction, body Bₖ has a single inboard joint. This joint defines several frames, discussed in rigid_body_tree_frames, including its parent frame: Pₖ ≡ Bₖ₋₁. This allows us to compute X_Bₖ₋₁Bₖ as follows:

  • X_Bₖ₋₁Bₖ = X_PₖBₖ because Pₖ ≡ Bₖ₋₁
  • X_PₖBₖ ≡ X_PₖFₖ * X_FₖMₖ(q) * X_MₖBₖ, where:
    • X_MₖBₖ = I in Drake's implementation.
    • X_FₖMₖ(q) = I because we only follow FixedJoint instances.

If the body is not rigidly fixed to the world, an exception will be thrown.

Returns
X_WBₙ.
See also
IsRigidlyFixedToWorld

◆ get_body_index()

int get_body_index ( ) const
inline

Returns the "body index" of this RigidBody.

This is the index within the vector of RigidBody objects within the RigidBodyTree.

◆ get_center_of_mass()

const Eigen::Vector3d & get_center_of_mass ( ) const

Gets the center of mass of this rigid body.

The center of mass is expressed in this body's frame.

◆ get_collision_element_ids()

const std::vector<drake::multibody::collision::ElementId>& get_collision_element_ids ( ) const
inline
Returns
A reference to an std::vector of collision elements that represent the collision geometry of this rigid body.

◆ get_contact_points()

const Eigen::Matrix3Xd & get_contact_points ( ) const

Returns the points on this rigid body that should be checked for collision with the environment.

These are the contact points that were saved by RigidBody::set_contact_points().

◆ get_group_to_collision_ids_map()

const std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_group_to_collision_ids_map ( ) const

Returns a map of collision element group names to vectors of collision element IDs.

These are the collision element groups created through calls to RigidBody::AddCollisionElementToGroup().

◆ get_mass()

double get_mass ( ) const

Returns the mass of this rigid body.

◆ get_model_instance_id()

int get_model_instance_id ( ) const

Returns the ID of the model instance to which this rigid body belongs.

◆ get_model_name()

const std::string & get_model_name ( ) const

Returns the name of the model defining this rigid body.

◆ get_mutable_collision_element_ids()

std::vector< drake::multibody::collision::ElementId > & get_mutable_collision_element_ids ( )

Returns a reference to an std::vector of collision elements that represent the collision geometry of this rigid body.

◆ get_mutable_group_to_collision_ids_map()

std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_mutable_group_to_collision_ids_map ( )

Returns a map of collision element group names to vectors of collision element IDs.

These are the collision element groups created through calls to RigidBody::AddCollisionElementToGroup().

◆ get_mutable_joint()

DrakeJoint& get_mutable_joint ( )
inline

An accessor to this rigid body's mutable inboard joint.

Also called "parent joint".

Exceptions
std::runtime_errorif there is no joint (joint == nullptr)
Returns
The mutable inboard joint of this rigid body.

◆ get_name()

const std::string & get_name ( ) const

Returns the name of this rigid body.

◆ get_num_collision_elements()

int get_num_collision_elements ( ) const
inline

Reports the total number of registered collision elements attached to this body.

See Model::AddElement() for definition of "registered".

◆ get_parent()

const RigidBody* get_parent ( ) const
inline

Returns a const pointer to this rigid body's parent rigid body.

◆ get_position_start_index()

int get_position_start_index ( ) const
inline

Returns the start index of this body's parent jont's position states; see RigidBody::set_position_start_index() for more information.

◆ get_spatial_inertia()

const drake::SquareTwistMatrix<double>& get_spatial_inertia ( ) const
inline

Returns the spatial inertia of this rigid body.

◆ get_velocity_start_index()

int get_velocity_start_index ( ) const
inline

Returns the start index of this body's parent jont's velocity states; see RigidBody::set_velocity_start_index() for more information.

◆ get_visual_elements()

const DrakeShapes::VectorOfVisualElements & get_visual_elements ( ) const

◆ getJoint()

const DrakeJoint& getJoint ( ) const
inline

An accessor to this rigid body's parent joint.

By "parent joint" we mean the joint through which this rigid body connects to its parent rigid body in the rigid body tree.

Returns
The parent joint of this rigid body.

◆ has_as_parent()

bool has_as_parent ( const RigidBody< T > &  other) const
inline

Checks if a particular rigid body is the parent of this rigid body.

Parameters
[in]otherThe potential parent of this rigid body.
Returns
true if the supplied rigid body parameter other is the parent of this rigid body.

◆ has_joint()

bool has_joint ( ) const
inline

Reports if the body has a parent joint.

◆ has_parent_body()

bool has_parent_body ( ) const
inline

Returns whether this RigidBody has a "parent", which is a RigidBody that is connected to this RigidBody via a DrakeJoint and is closer to the root of the RigidBodyTree relative to this RigidBody.

In other words, the parent RigidBody is part of a kinematic path from this RigidBody to the root of the RigidBodyTree. Thus, by definition, all RigidBody objects should have a parent RigidBody except for the RigidBodyTree's root, which is the world.

◆ hasParent()

bool hasParent ( ) const

◆ IsRigidlyFixedToWorld()

bool IsRigidlyFixedToWorld ( ) const

Reports if there is a path in this tree from this body to the world where all joints are fixed.

This method throws an exception if the RigidBodyTree is invalid in that:

  • This node is the descendant of a parentless node that is not the world node, or
  • This node does not have a valid DrakeJoint.

◆ set_body_index()

void set_body_index ( int  body_index)

Sets the "body index" of this RigidBody.

The "body index" is the index of this RigidBody within the vector of RigidBody objects within the RigidBodyTree. Users should NOT call this method. It is only here to be used internally by RigidBodyTree.

◆ set_center_of_mass()

void set_center_of_mass ( const Eigen::Vector3d &  center_of_mass)

Sets the center of mass of this rigid body.

The center of mass is expressed in this body's frame.

◆ set_contact_points()

void set_contact_points ( const Eigen::Matrix3Xd &  contact_points)

Saves the points on this rigid body that should be checked for collision between this rigid body and the environment.

These contact points can be obtained through RigidBody::get_contact_points().

◆ set_mass()

void set_mass ( double  mass)

Sets the mass of this rigid body.

◆ set_model_instance_id()

void set_model_instance_id ( int  model_instance_id)

Sets the ID of the model instance to which this rigid body belongs.

◆ set_model_name()

void set_model_name ( const std::string &  name)

Sets the name of the model defining this rigid body.

◆ set_name()

void set_name ( const std::string &  name)

Sets the name of this rigid body.

◆ set_parent()

void set_parent ( RigidBody< T > *  parent)

Sets the parent rigid body.

This is the rigid body that is connected to this rigid body's joint.

Parameters
[in]parentA pointer to this rigid body's parent rigid body.

◆ set_position_start_index()

void set_position_start_index ( int  position_start_index)

Sets the start index of this rigid body's mobilizer joint's contiguous generalized coordinates q (joint position state variables) within the full RigidBodyTree generalized coordinate vector.

For more details about the semantics of position_start_index, see the documentation for RigidBodyTree.

◆ set_spatial_inertia()

void set_spatial_inertia ( const drake::SquareTwistMatrix< double > &  inertia_matrix)

Sets the spatial inertia of this rigid body.

◆ set_velocity_start_index()

void set_velocity_start_index ( int  velocity_start_index)

Sets the start index of this rigid body's mobilizer joint's contiguous generalized velocity v (joint velocity state variables) within the full RigidBodyTree generalized velocity vector.

For more details about the semantics of velocity_start_index, see the documentation for RigidBodyTree.

◆ setJoint()

void setJoint ( std::unique_ptr< DrakeJoint joint)

Sets the parent joint through which this rigid body connects to its parent rigid body.

Parameters
[in]jointThe parent joint of this rigid body. Note that this rigid body assumes ownership of this joint.

Friends And Related Function Documentation

◆ operator<<

std::ostream& operator<< ( std::ostream &  out,
const RigidBody< double > &  b 
)
friend

Member Data Documentation

◆ CollisionElementsVector

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef std::vector<drake::multibody::collision::Element*> CollisionElementsVector

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