Drake
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
RigidBodyTree< T > Class Template Reference

Maintains a vector of RigidBody objects that are arranged into a kinematic tree via DrakeJoint objects. More...

#include <drake/multibody/ik_options.h>

Collaboration diagram for RigidBodyTree< T >:

Public Types

using BodyToWrenchMap = drake::eigen_aligned_std_unordered_map< RigidBody< double > const *, drake::WrenchVector< T >>
 Convenience alias for rigid body to external wrench map, for use with inverseDynamics and dynamicsBiasTerm. More...
 

Public Member Functions

 RigidBodyTree ()
 A constructor that initializes the gravity vector to be [0, 0, -9.81] and a single RigidBody named "world". More...
 
virtual ~RigidBodyTree ()
 
std::unique_ptr< RigidBodyTree
< double > > 
Clone () const
 Returns a deep clone of this RigidBodyTree<double>. More...
 
int add_model_instance ()
 Adds a new model instance to this RigidBodyTree. More...
 
int get_next_clique_id ()
 
int get_num_model_instances () const
 Returns the number of model instances in the tree, not including the world. More...
 
int get_number_of_model_instances () const
 
void addFrame (std::shared_ptr< RigidBodyFrame< T >> frame)
 
std::map< std::string, intcomputePositionNameToIndexMap () const
 Returns a map from DOF position name to DOF index within the output vector of this RigidBodyTree. More...
 
void surfaceTangents (Eigen::Map< Eigen::Matrix3Xd > const &normals, std::vector< Eigen::Map< Eigen::Matrix3Xd >> &tangents) const
 
bool transformCollisionFrame (RigidBody< T > *body, const Eigen::Isometry3d &displace_transform)
 Applies the given transform to the given body's collision elements, displacing them from their current configurations. More...
 
void compile ()
 
Eigen::VectorXd getZeroConfiguration () const
 
Eigen::VectorXd getRandomConfiguration (std::default_random_engine &generator) const
 
std::string get_position_name (int position_num) const
 Returns the name of the position state at index position_num within this RigidBodyTree's state vector. More...
 
std::string get_velocity_name (int velocity_num) const
 Returns the name of the velocity state at index velocity_num within this RigidBodyTree's state vector. More...
 
std::string getPositionName (int position_num) const
 
std::string getVelocityName (int velocity_num) const
 
std::string getStateName (int state_num) const
 
void drawKinematicTree (std::string graphviz_dotfile_filename) const
 
template<typename Scalar >
void CheckCacheValidity (const KinematicsCache< Scalar > &cache) const
 Checks whether cache is valid for use with this RigidBodyTree. More...
 
KinematicsCache< T > CreateKinematicsCache () const
 Creates a KinematicsCache to perform computations with this RigidBodyTree. More...
 
template<typename CacheT >
KinematicsCache< CacheT > CreateKinematicsCacheWithType () const
 A helper template method used to create a KinematicsCache templated on CacheT from a RigidBodyTree templated on T, with CacheT and T not necessarily the same scalar type. More...
 
template<typename DerivedQ >
KinematicsCache< typename
DerivedQ::Scalar > 
doKinematics (const Eigen::MatrixBase< DerivedQ > &q) const
 Initializes a KinematicsCache with the given configuration q, computes the kinematics, and returns the cache. More...
 
template<typename DerivedQ , typename DerivedV >
KinematicsCache< typename
DerivedQ::Scalar > 
doKinematics (const Eigen::MatrixBase< DerivedQ > &q, const Eigen::MatrixBase< DerivedV > &v, bool compute_JdotV=true) const
 Initializes a KinematicsCache with the given configuration q and velocity v, computes the kinematics, and returns the cache. More...
 
template<typename Scalar >
void doKinematics (KinematicsCache< Scalar > &cache, bool compute_JdotV=false) const
 Computes the kinematics on the given cache. More...
 
bool is_part_of_model_instances (const RigidBody< T > &body, const std::set< int > &model_instance_id_set) const
 Returns true if body is part of a model instance whose ID is in model_instance_id_set. More...
 
double getMass (const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set) const
 Computes the total combined mass of a set of model instances. More...
 
template<typename Scalar >
Eigen::Matrix< Scalar,
drake::kSpaceDimension, 1 > 
centerOfMass (const KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set) const
 
drake::Matrix6< T > LumpedSpatialInertiaInWorldFrame (const KinematicsCache< T > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set) const
 Computes the summed spatial inertia in the world frame of all bodies that belong to model instances in model_instance_id_set. More...
 
drake::Matrix6< T > LumpedSpatialInertiaInWorldFrame (const KinematicsCache< T > &cache, const std::vector< const RigidBody< T > * > &bodies_of_interest) const
 Computes the summed spatial inertia in the world frame of all the bodies in bodies_of_interest. More...
 
drake::Isometry3< T > CalcBodyPoseInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body) const
 Computes the pose X_WB of body's frame B in the world frame W. More...
 
drake::Isometry3< T > CalcFramePoseInWorldFrame (const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F) const
 Computes the pose X_WF of frame_F in the world frame W. More...
 
drake::Isometry3< T > CalcFramePoseInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body, const drake::Isometry3< T > &X_BF) const
 Computes the pose X_WF of the rigid body frame F in the world frame W. More...
 
drake::Vector6< T > CalcBodySpatialVelocityInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body) const
 Computes the spatial velocity V_WB of body's frame B measured and expressed in the world frame W. More...
 
drake::Vector6< T > CalcFrameSpatialVelocityInWorldFrame (const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F) const
 Computes the spatial velocity V_WF of RigidBodyFrame frame_F measured and expressed in the world frame W. More...
 
drake::Vector6< T > CalcFrameSpatialVelocityInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body, const drake::Isometry3< T > &X_BF) const
 Computes the spatial velocity V_WF of the frame F measured and expressed in the world frame W. More...
 
drake::Matrix6X< T > CalcFrameSpatialVelocityJacobianInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body, const drake::Isometry3< T > &X_BF, bool in_terms_of_qdot=false) const
 Computes the Jacobian J_WF of the spatial velocity V_WF of frame F measured and expressed in the world frame W such that V_WF = J_WF * v, where v is the generalized velocity. More...
 
drake::Matrix6X< T > CalcFrameSpatialVelocityJacobianInWorldFrame (const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F, bool in_terms_of_qdot=false) const
 Computes the Jacobian J_WF of the spatial velocity V_WF of frame F measured and expressed in the world frame W such that V_WF = J_WF * v, where v is the generalized velocity. More...
 
drake::Matrix6X< T > CalcBodySpatialVelocityJacobianInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body, bool in_terms_of_qdot=false) const
 Computes the Jacobian J_WB of the spatial velocity V_WB of body frame B measured and expressed in the world frame W such that V_WB = J_WB * v, where v is the generalized velocity. More...
 
drake::Vector6< T > CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body, const drake::Isometry3< T > &X_BF) const
 Computes Jdot_WF * v, where J_WF is the Jacobian of spatial velocity, V_WF, of frame F measured and expressed in the world frame W, and v is the generalized velocity. More...
 
drake::Vector6< T > CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame (const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F) const
 Computes Jdot_WF * v, where J_WF is the Jacobian of spatial velocity V_WF of frame F measured and expressed in the world frame W, and v is the generalized velocity. More...
 
drake::Vector6< T > CalcBodySpatialVelocityJacobianDotTimesVInWorldFrame (const KinematicsCache< T > &cache, const RigidBody< T > &body) const
 Computes Jdot_WB * v, where J_WB is the Jacobian of the spatial velocity V_WB of body frame B measured and expressed in the world frame W, and v is the generalized velocity. More...
 
template<typename Scalar >
drake::TwistMatrix< Scalar > worldMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set, bool in_terms_of_qdot=false) const
 
template<typename Scalar >
drake::TwistVector< Scalar > worldMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set) const
 
template<typename Scalar >
drake::TwistMatrix< Scalar > centroidalMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set, bool in_terms_of_qdot=false) const
 
template<typename Scalar >
drake::TwistVector< Scalar > centroidalMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
drake::kSpaceDimension,
Eigen::Dynamic > 
centerOfMassJacobian (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set, bool in_terms_of_qdot=false) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
drake::kSpaceDimension, 1 > 
centerOfMassJacobianDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set=RigidBodyTreeConstants::default_model_instance_id_set) const
 
template<typename DerivedA , typename DerivedB , typename DerivedC >
void jointLimitConstraints (Eigen::MatrixBase< DerivedA > const &q, Eigen::MatrixBase< DerivedB > &phi, Eigen::MatrixBase< DerivedC > &J) const
 
size_t getNumJointLimitConstraints () const
 
int getNumContacts (const std::set< int > &body_idx) const
 
template<typename DerivedNormal , typename DerivedPoint >
std::pair< Eigen::Vector3d,
double
resolveCenterOfPressure (const KinematicsCache< double > &cache, const std::vector< ForceTorqueMeasurement > &force_torque_measurements, const Eigen::MatrixBase< DerivedNormal > &normal, const Eigen::MatrixBase< DerivedPoint > &point_on_contact_plane) const
 Computes CoP in world frame. More...
 
std::vector< intFindAncestorBodies (int body_index) const
 Finds the ancestors of a body. More...
 
void findAncestorBodies (std::vector< int > &ancestor_bodies, int body) const
 
KinematicPath findKinematicPath (int start_body_or_frame_idx, int end_body_or_frame_idx) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, Eigen::Dynamic > 
massMatrix (KinematicsCache< Scalar > &cache) const
 Compute the positive definite mass (configuration space) matrix \( *H(q) \), defined by \(T = \frac{1}{2} v^T H(q) v \), where \( T \) is kinetic energy. More...
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
dynamicsBiasTerm (KinematicsCache< Scalar > &cache, const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, drake::WrenchVector< Scalar >> &external_wrenches, bool include_velocity_terms=true) const
 Compute the term \( C(q, v, f_\text{ext}) \) in the manipulator equations

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u \]

. More...

 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
inverseDynamics (KinematicsCache< Scalar > &cache, const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, drake::WrenchVector< Scalar >> &external_wrenches, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &vd, bool include_velocity_terms=true) const
 Compute

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) \]

that is, the left hand side of the manipulator equations

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u \]

. More...

 
template<typename DerivedV >
Eigen::Matrix< typename
DerivedV::Scalar,
Eigen::Dynamic, 1 > 
frictionTorques (Eigen::MatrixBase< DerivedV > const &v) const
 
template<typename Scalar , typename DerivedPoints >
Eigen::Matrix< Scalar,
3, DerivedPoints::ColsAtCompileTime > 
transformPoints (const KinematicsCache< Scalar > &cache, const Eigen::MatrixBase< DerivedPoints > &points, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar >
Eigen::Matrix< Scalar, 4, 1 > relativeQuaternion (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar >
Eigen::Matrix< Scalar, 3, 1 > relativeRollPitchYaw (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar , typename DerivedPoints >
Eigen::Matrix< Scalar,
Eigen::Dynamic, Eigen::Dynamic > 
transformPointsJacobian (const KinematicsCache< Scalar > &cache, const Eigen::MatrixBase< DerivedPoints > &points, int from_body_or_frame_ind, int to_body_or_frame_ind, bool in_terms_of_qdot) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
drake::kQuaternionSize,
Eigen::Dynamic > 
relativeQuaternionJacobian (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind, bool in_terms_of_qdot) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
drake::kRpySize,
Eigen::Dynamic > 
relativeRollPitchYawJacobian (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind, bool in_terms_of_qdot) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, Eigen::Dynamic > 
forwardKinPositionGradient (const KinematicsCache< Scalar > &cache, int npoints, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar , typename DerivedPoints >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
transformPointsJacobianDotTimesV (const KinematicsCache< Scalar > &cache, const Eigen::MatrixBase< DerivedPoints > &points, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
relativeQuaternionJacobianDotTimesV (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
relativeRollPitchYawJacobianDotTimesV (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar >
drake::TwistMatrix< Scalar > geometricJacobian (const KinematicsCache< Scalar > &cache, int base_body_or_frame_ind, int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind, bool in_terms_of_qdot=false, std::vector< int > *v_indices=nullptr) const
 
template<typename Scalar >
drake::TwistVector< Scalar > geometricJacobianDotTimesV (const KinematicsCache< Scalar > &cache, int base_body_or_frame_ind, int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind) const
 
template<typename Scalar >
drake::TwistVector< Scalar > relativeTwist (const KinematicsCache< Scalar > &cache, int base_or_frame_ind, int body_or_frame_ind, int expressed_in_body_or_frame_ind) const
 
template<typename Scalar >
drake::TwistVector< Scalar > transformSpatialAcceleration (const KinematicsCache< Scalar > &cache, const drake::TwistVector< Scalar > &spatial_acceleration, int base_or_frame_ind, int body_or_frame_ind, int old_body_or_frame_ind, int new_body_or_frame_ind) const
 
template<typename Scalar >
Eigen::Transform< Scalar,
drake::kSpaceDimension,
Eigen::Isometry > 
relativeTransform (const KinematicsCache< Scalar > &cache, int base_or_frame_ind, int body_or_frame_ind) const
 
template<typename Scalar >
void computeContactJacobians (const KinematicsCache< Scalar > &cache, Eigen::Ref< const Eigen::VectorXi > const &idxA, Eigen::Ref< const Eigen::VectorXi > const &idxB, Eigen::Ref< const Eigen::Matrix3Xd > const &xA, Eigen::Ref< const Eigen::Matrix3Xd > const &xB, Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &J) const
 Computes the Jacobian for many points in the format currently used by MATLAB. More...
 
void addCollisionElement (const DrakeCollision::Element &element, RigidBody< T > &body, const std::string &group_name)
 Adds a new collision element to the tree. More...
 
const DrakeCollision::ElementFindCollisionElement (const DrakeCollision::ElementId &id) const
 Retrieve a const pointer to an element of the collision model. More...
 
template<class UnaryPredicate >
void removeCollisionGroupsIf (UnaryPredicate test)
 
void updateCollisionElements (const RigidBody< T > &body, const Eigen::Transform< double, 3, Eigen::Isometry > &transform_to_world)
 
void updateDynamicCollisionElements (const KinematicsCache< double > &kin_cache)
 
void getTerrainContactPoints (const RigidBody< T > &body, Eigen::Matrix3Xd *terrain_points, const std::string &group_name="") const
 Gets the contact points defined by a body's collision elements. More...
 
bool collisionRaycast (const KinematicsCache< double > &cache, const Eigen::Matrix3Xd &origins, const Eigen::Matrix3Xd &ray_endpoints, Eigen::VectorXd &distances, bool use_margins=false)
 
bool collisionRaycast (const KinematicsCache< double > &cache, const Eigen::Matrix3Xd &origins, const Eigen::Matrix3Xd &ray_endpoints, Eigen::VectorXd &distances, Eigen::Matrix3Xd &normals, bool use_margins=false)
 
void collisionDetectFromPoints (const KinematicsCache< double > &cache, const Eigen::Matrix3Xd &points, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &x, Eigen::Matrix3Xd &body_x, std::vector< int > &body_idx, bool use_margins)
 Computes the signed distance from the given points to the nearest body in the RigidBodyTree. More...
 
bool collisionDetect (const KinematicsCache< double > &cache, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &xA, Eigen::Matrix3Xd &xB, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, const std::vector< DrakeCollision::ElementId > &ids_to_check, bool use_margins)
 
bool collisionDetect (const KinematicsCache< double > &cache, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &xA, Eigen::Matrix3Xd &xB, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, const std::vector< int > &bodies_idx, const std::set< std::string > &active_element_groups, bool use_margins=true)
 
bool collisionDetect (const KinematicsCache< double > &cache, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &xA, Eigen::Matrix3Xd &xB, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, const std::vector< int > &bodies_idx, bool use_margins=true)
 
bool collisionDetect (const KinematicsCache< double > &cache, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &xA, Eigen::Matrix3Xd &xB, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, const std::set< std::string > &active_element_groups, bool use_margins=true)
 
bool collisionDetect (const KinematicsCache< double > &cache, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &xA, Eigen::Matrix3Xd &xB, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, bool use_margins=true)
 
bool allCollisions (const KinematicsCache< double > &cache, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, Eigen::Matrix3Xd &ptsA, Eigen::Matrix3Xd &ptsB, bool use_margins=true)
 
void potentialCollisions (const KinematicsCache< double > &cache, Eigen::VectorXd &phi, Eigen::Matrix3Xd &normal, Eigen::Matrix3Xd &xA, Eigen::Matrix3Xd &xB, std::vector< int > &bodyA_idx, std::vector< int > &bodyB_idx, bool use_margins=true)
 
std::vector
< DrakeCollision::PointPair
ComputeMaximumDepthCollisionPoints (const KinematicsCache< double > &cache, bool use_margins=true)
 Computes the point of closest approach between bodies in the RigidBodyTree that are in contact. More...
 
virtual bool collidingPointsCheckOnly (const KinematicsCache< double > &cache, const std::vector< Eigen::Vector3d > &points, double collision_threshold)
 
virtual std::vector< size_t > collidingPoints (const KinematicsCache< double > &cache, const std::vector< Eigen::Vector3d > &points, double collision_threshold)
 
RigidBody< T > * FindBody (const std::string &body_name, const std::string &model_name="", int model_id=-1) const
 Finds a body with the specified body_name belonging to a model with the specified model_name and model_id. More...
 
const RigidBody< double > * FindBody (DrakeCollision::ElementId element_id) const
 Reports the RigidBody that owns the collision element indicated by the id. More...
 
std::vector< const RigidBody
< T > * > 
FindModelInstanceBodies (int model_instance_id) const
 Returns a vector of pointers to all rigid bodies in this tree that belong to a particular model instance. More...
 
RigidBody< T > * findLink (const std::string &link_name, const std::string &model_name="", int model_id=-1) const
 This is a deprecated version of FindBody(...). More...
 
std::vector< intFindBaseBodies (int model_instance_id=-1) const
 Obtains a vector of indexes of the bodies that are directly attached to the world via any type of joint. More...
 
int FindBodyIndex (const std::string &body_name, int model_instance_id=-1) const
 Obtains the index of a rigid body within this rigid body tree. More...
 
std::vector< intFindChildrenOfBody (int parent_body_index, int model_instance_id=-1) const
 Returns a vector of indexes of bodies that are the children of the body at index parent_body_index. More...
 
int findLinkId (const std::string &link_name, int model_id=-1) const
 This is a deprecated version of FindBodyIndex(...). More...
 
RigidBody< T > * FindChildBodyOfJoint (const std::string &joint_name, int model_instance_id=-1) const
 Obtains a pointer to the rigid body whose parent joint is named joint_name and is part of a model instance with ID model_instance_id. More...
 
RigidBody< T > * findJoint (const std::string &joint_name, int model_id=-1) const
 
int FindIndexOfChildBodyOfJoint (const std::string &joint_name, int model_instance_id=-1) const
 Returns the index within the vector of rigid bodies of the rigid body whose parent joint is named joint_name and is part of a model instance with ID model_instance_id. More...
 
int findJointId (const std::string &joint_name, int model_id=-1) const
 
std::shared_ptr
< RigidBodyFrame< T > > 
findFrame (const std::string &frame_name, int model_id=-1) const
 Finds a frame of the specified frame_name belonging to a model with the specified model_id. More...
 
const RigidBody< T > & get_body (int body_index) const
 Returns the body at index body_index. More...
 
RigidBody< T > * get_mutable_body (int body_index)
 Returns the body at index body_index. More...
 
int get_num_bodies () const
 Returns the number of bodies in this tree. More...
 
int get_num_frames () const
 Returns the number of frames in this tree. More...
 
int get_number_of_bodies () const
 
std::string getBodyOrFrameName (int body_or_frame_id) const
 
const RigidBodyActuatorGetActuator (const std::string &name) const
 Obtains a rigid body actuator from this rigid body tree. More...
 
template<typename Scalar >
int parseBodyOrFrameID (const int body_or_frame_id, Eigen::Transform< Scalar, 3, Eigen::Isometry > *Tframe) const
 
int parseBodyOrFrameID (const int body_or_frame_id) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
positionConstraints (const KinematicsCache< Scalar > &cache) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, Eigen::Dynamic > 
positionConstraintsJacobian (const KinematicsCache< Scalar > &cache, bool in_terms_of_qdot=true) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
Eigen::Dynamic, 1 > 
positionConstraintsJacDotTimesV (const KinematicsCache< Scalar > &cache) const
 
size_t getNumPositionConstraints () const
 
template<typename Derived >
Eigen::Matrix< typename
Derived::Scalar,
Derived::RowsAtCompileTime,
Eigen::Dynamic > 
compactToFull (const Eigen::MatrixBase< Derived > &compact, const std::vector< int > &joint_path, bool in_terms_of_qdot) const
 
RigidBody< T > * add_rigid_body (std::unique_ptr< RigidBody< T >> body)
 Adds and takes ownership of a rigid body. More...
 
void DefineCollisionFilterGroup (const std::string &name)
 Attempts to define a new collision filter group. More...
 
void AddCollisionFilterGroupMember (const std::string &group_name, const std::string &body_name, int model_id)
 Adds a RigidBody to a collision filter group. More...
 
void AddCollisionFilterIgnoreTarget (const std::string &group_name, const std::string &target_group_name)
 Adds a collision group to the set of groups ignored by the specified collision filter group. More...
 
void SetBodyCollisionFilters (const RigidBody< T > &body, const DrakeCollision::bitmask &group, const DrakeCollision::bitmask &ignores)
 Directly set the masks for a body. More...
 
RigidBody< T > & world ()
 Returns a mutable reference to the RigidBody associated with the world in the model. More...
 
const RigidBody< T > & world () const
 Returns a const reference to the RigidBody associated with the world in the model. More...
 
int get_num_positions () const
 Returns the number of position states outputted by this RigidBodyTree. More...
 
int number_of_positions () const
 
int get_num_velocities () const
 Returns the number of velocity states outputted by this RigidBodyTree. More...
 
int number_of_velocities () const
 
int get_num_actuators () const
 Returns the number of actuators in this RigidBodyTree. More...
 
bool initialized () const
 Returns whether this RigidBodyTree is initialized. More...
 
template<typename Scalar >
TwistMatrix< Scalar > worldMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set, bool in_terms_of_qdot) const
 
template<typename Scalar >
TwistVector< Scalar > worldMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set) const
 
template<typename Scalar >
TwistMatrix< Scalar > centroidalMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set, bool in_terms_of_qdot) const
 
template<typename Scalar >
TwistVector< Scalar > centroidalMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
kSpaceDimension, 1 > 
centerOfMass (const KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set) const
 
template<typename Derived >
MatrixX< typename Derived::Scalar > transformVelocityMappingToQDotMapping (const KinematicsCache< typename Derived::Scalar > &cache, const Eigen::MatrixBase< Derived > &Av)
 
template<typename Derived >
MatrixX< typename Derived::Scalar > transformQDotMappingToVelocityMapping (const KinematicsCache< typename Derived::Scalar > &cache, const Eigen::MatrixBase< Derived > &Ap)
 
template<typename Scalar >
MatrixX< Scalar > GetVelocityToQDotMapping (const KinematicsCache< Scalar > &cache)
 
template<typename Scalar >
MatrixX< Scalar > GetQDotToVelocityMapping (const KinematicsCache< Scalar > &cache)
 
template<typename Scalar >
Matrix< Scalar,
kSpaceDimension,
Eigen::Dynamic > 
centerOfMassJacobian (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set, bool in_terms_of_qdot) const
 
template<typename Scalar >
Matrix< Scalar,
kSpaceDimension, 1 > 
centerOfMassJacobianDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &model_instance_id_set) const
 
template<typename Scalar >
int parseBodyOrFrameID (const int body_or_frame_id, Eigen::Transform< Scalar, 3, Isometry > *Tframe) const
 
template<typename Scalar >
TwistMatrix< Scalar > geometricJacobian (const KinematicsCache< Scalar > &cache, int base_body_or_frame_ind, int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind, bool in_terms_of_qdot, std::vector< int > *v_or_qdot_indices) const
 
template<typename Scalar >
TwistVector< Scalar > geometricJacobianDotTimesV (const KinematicsCache< Scalar > &cache, int base_body_or_frame_ind, int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind) const
 
template<typename Scalar >
TwistVector< Scalar > relativeTwist (const KinematicsCache< Scalar > &cache, int base_or_frame_ind, int body_or_frame_ind, int expressed_in_body_or_frame_ind) const
 
template<typename Scalar >
TwistVector< Scalar > transformSpatialAcceleration (const KinematicsCache< Scalar > &cache, const TwistVector< Scalar > &spatial_acceleration, int base_ind, int body_ind, int old_expressed_in_body_or_frame_ind, int new_expressed_in_body_or_frame_ind) const
 
template<typename Scalar >
Transform< Scalar, 3, Isometry > relativeTransform (const KinematicsCache< Scalar > &cache, int base_or_frame_ind, int body_or_frame_ind) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic,
Eigen::Dynamic > 
massMatrix (KinematicsCache< Scalar > &cache) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic, 1 > dynamicsBiasTerm (KinematicsCache< Scalar > &cache, const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, WrenchVector< Scalar >> &external_wrenches, bool include_velocity_terms) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic, 1 > inverseDynamics (KinematicsCache< Scalar > &cache, const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, WrenchVector< Scalar >> &external_wrenches, const Matrix< Scalar, Eigen::Dynamic, 1 > &vd, bool include_velocity_terms) const
 
template<typename DerivedV >
Matrix< typename
DerivedV::Scalar, Dynamic, 1 > 
frictionTorques (Eigen::MatrixBase< DerivedV > const &v) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
kQuaternionSize,
Eigen::Dynamic > 
relativeQuaternionJacobian (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind, bool in_terms_of_qdot) const
 
template<typename Scalar >
Eigen::Matrix< Scalar,
kRpySize, Eigen::Dynamic > 
relativeRollPitchYawJacobian (const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind, bool in_terms_of_qdot) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic,
Eigen::Dynamic > 
forwardKinPositionGradient (const KinematicsCache< Scalar > &cache, int npoints, int from_body_or_frame_ind, int to_body_or_frame_ind) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic, 1 > positionConstraints (const KinematicsCache< Scalar > &cache) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic,
Eigen::Dynamic > 
positionConstraintsJacobian (const KinematicsCache< Scalar > &cache, bool in_terms_of_qdot) const
 
template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic, 1 > positionConstraintsJacDotTimesV (const KinematicsCache< Scalar > &cache) const
 
template<typename DerivedA , typename DerivedB , typename DerivedC >
void jointLimitConstraints (MatrixBase< DerivedA > const &q, MatrixBase< DerivedB > &phi, MatrixBase< DerivedC > &J) const
 
template<typename Scalar >
void accumulateContactJacobian (const KinematicsCache< Scalar > &cache, const int bodyInd, Matrix3Xd const &bodyPoints, std::vector< size_t > const &cindA, std::vector< size_t > const &cindB, Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &J) const
 
template<typename Scalar >
void computeContactJacobians (const KinematicsCache< Scalar > &cache, Ref< const VectorXi > const &idxA, Ref< const VectorXi > const &idxB, Ref< const Matrix3Xd > const &xA, Ref< const Matrix3Xd > const &xB, Matrix< Scalar, Dynamic, Dynamic > &J) const
 

Static Public Member Functions

static KinematicsCache< T > CreateKinematicsCacheFromBodiesVector (const std::vector< std::unique_ptr< RigidBody< T >>> &bodies)
 Creates a KinematicsCache given a reference to a vector of rigid bodies contained within a RigidBodyTree. More...
 
static drake::VectorX< T > transformQDotToVelocity (const KinematicsCache< T > &cache, const drake::VectorX< T > &qdot)
 Converts a vector of the time derivative of generalized coordinates (qdot) to generalized velocity (v). More...
 
static drake::VectorX< T > transformVelocityToQDot (const KinematicsCache< T > &cache, const drake::VectorX< T > &v)
 Converts a vector of generalized velocities (v) to the time derivative of generalized coordinates (qdot). More...
 
template<typename Derived >
static drake::MatrixX
< typename Derived::Scalar > 
transformVelocityMappingToQDotMapping (const KinematicsCache< typename Derived::Scalar > &cache, const Eigen::MatrixBase< Derived > &Av)
 Converts a matrix B, which transforms generalized velocities (v) to an output space X, to a matrix A, which transforms the time derivative of generalized coordinates (qdot) to the same output X. More...
 
template<typename Derived >
static drake::MatrixX
< typename Derived::Scalar > 
transformQDotMappingToVelocityMapping (const KinematicsCache< typename Derived::Scalar > &cache, const Eigen::MatrixBase< Derived > &Ap)
 Converts a matrix A, which transforms the time derivative of generalized coordinates (qdot) to an output space X, to a matrix B, which transforms generalized velocities (v) to the same space X. More...
 
template<typename Scalar >
static drake::MatrixX< Scalar > GetVelocityToQDotMapping (const KinematicsCache< Scalar > &cache)
 
template<typename Scalar >
static drake::MatrixX< Scalar > GetQDotToVelocityMapping (const KinematicsCache< Scalar > &cache)
 

Public Attributes

Eigen::VectorXd joint_limit_min
 
Eigen::VectorXd joint_limit_max
 
std::vector< std::unique_ptr
< RigidBody< T > > > 
bodies
 
std::vector< std::shared_ptr
< RigidBodyFrame< T > > > 
frames
 
std::vector< RigidBodyActuator,
Eigen::aligned_allocator
< RigidBodyActuator > > 
actuators
 
std::vector< RigidBodyLoop< T >
, Eigen::aligned_allocator
< RigidBodyLoop< T > > > 
loops
 
drake::TwistVector< doublea_grav
 
Eigen::MatrixXd B
 

Friends

std::ostream & operator<< (std::ostream &, const RigidBodyTree< double > &)
 A toString method for this class. More...
 

Detailed Description

template<typename T>
class RigidBodyTree< T >

Maintains a vector of RigidBody objects that are arranged into a kinematic tree via DrakeJoint objects.

It provides various utility methods for computing kinematic and dynamics properties of the RigidBodyTree.

The internal organization of a RigidBodyTree's generalized state vector is as follows:

[model instance 1's generalized coordinate vector]
[model instance 2's generalized coordinate vector]
...
[model instance 1's generalized velocity vector]
[model instance 2's generalized velocity vector]
...

Each RigidBody maintains for its joint that connects to its parent the indices of the joint's generalized coordinate vector and generalized velocity vector in the RigidBodyTree's generalized state vector.

The starting index of the joint's generalized coordinate vector in the RigidBodyTree's generalized state vector can be obtained by executing RigidBody::get_position_start_index().

The starting index of the joint's generalized velocity vector in the RigidBodyTree's generalized state vector can be computed as follows: RigidBodyTree::get_num_positions() + RigidBody::get_velocity_start_index().

Note that the velocity index starts at the beginning of the velocity state variables and not at the beginning of the full state of the RigidBodyTree. This is why the total number of positions needs to be added to the velocity index to get its index in the RigidBodyTree's full state vector.

Template Parameters
TThe scalar type. Must be a valid Eigen scalar.

Member Typedef Documentation

Convenience alias for rigid body to external wrench map, for use with inverseDynamics and dynamicsBiasTerm.

Constructor & Destructor Documentation

A constructor that initializes the gravity vector to be [0, 0, -9.81] and a single RigidBody named "world".

This RigidBody can be accessed by calling RigidBodyTree::world().

~RigidBodyTree ( )
virtual

Member Function Documentation

void accumulateContactJacobian ( const KinematicsCache< Scalar > &  cache,
const int  bodyInd,
Matrix3Xd const &  bodyPoints,
std::vector< size_t > const &  cindA,
std::vector< size_t > const &  cindB,
Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &  J 
) const
int add_model_instance ( )

Adds a new model instance to this RigidBodyTree.

The model instance is identified by a unique model instance ID, which is the return value of this method.

RigidBody< T > * add_rigid_body ( std::unique_ptr< RigidBody< T >>  body)

Adds and takes ownership of a rigid body.

A RigidBodyTree is the sole owner and manager of the RigidBody's in it. A body is assigned a unique id (RigidBody::id()) when added to a RigidBodyTree. This unique id can be use to access a body with the accessor RigidBodyTree::body.

Parameters
[in]bodyThe rigid body to add to this rigid body tree.
Returns
A bare, unowned pointer to the body.

Here is the call graph for this function:

void addCollisionElement ( const DrakeCollision::Element element,
RigidBody< T > &  body,
const std::string &  group_name 
)

Adds a new collision element to the tree.

The input element will be copied and that copy will be stored in the tree, associated with the given body. This association is pending. It is necessary to call compile() in order for the element to be fully integrated into the RigidBodyTree.

Parameters
elementthe element to add.
bodythe body to associate the element with.
group_namea group name to tag the associated element with.

Here is the call graph for this function:

Here is the caller graph for this function:

void AddCollisionFilterGroupMember ( const std::string &  group_name,
const std::string &  body_name,
int  model_id 
)

Adds a RigidBody to a collision filter group.

The RigidBody is referenced by name and model instance id. The process will fail if the body cannot be found, if the group cannot be found, or if the indicated body already has registered collision elements (see Model::AddElement() for more details). An exception is thrown in the event of failure.

Parameters
group_nameThe collision filter group name to add the body to.
body_nameThe name of the body to add.
model_idThe id of the model instance to which this body belongs.

Here is the call graph for this function:

void AddCollisionFilterIgnoreTarget ( const std::string &  group_name,
const std::string &  target_group_name 
)

Adds a collision group to the set of groups ignored by the specified collision filter group.

Will fail if the specified group name does not refer to an existing collision filter group. (The target group name need not exist at this time.) An exception is thrown upon failure.

Parameters
group_name
target_group_name
void addFrame ( std::shared_ptr< RigidBodyFrame< T >>  frame)
bool allCollisions ( const KinematicsCache< double > &  cache,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
Eigen::Matrix3Xd &  ptsA,
Eigen::Matrix3Xd &  ptsB,
bool  use_margins = true 
)

Here is the call graph for this function:

Here is the caller graph for this function:

drake::Isometry3<T> CalcBodyPoseInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body 
) const
inline

Computes the pose X_WB of body's frame B in the world frame W.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
Return values
<tt>X_WB</tt>

Here is the caller graph for this function:

Vector6< T > CalcBodySpatialVelocityInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body 
) const

Computes the spatial velocity V_WB of body's frame B measured and expressed in the world frame W.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
Return values
<tt>V_WB</tt>

Here is the call graph for this function:

Here is the caller graph for this function:

drake::Vector6<T> CalcBodySpatialVelocityJacobianDotTimesVInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body 
) const
inline

Computes Jdot_WB * v, where J_WB is the Jacobian of the spatial velocity V_WB of body frame B measured and expressed in the world frame W, and v is the generalized velocity.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
Return values
<tt>Jdot_WB* v

Here is the caller graph for this function:

drake::Matrix6X<T> CalcBodySpatialVelocityJacobianInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body,
bool  in_terms_of_qdot = false 
) const
inline

Computes the Jacobian J_WB of the spatial velocity V_WB of body frame B measured and expressed in the world frame W such that V_WB = J_WB * v, where v is the generalized velocity.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
in_terms_of_qdottrue for J_WB computed with respect to the time derivative of the generalized position such that V_WB = J_WB * qdot. false for J_WB computed with respect to v.
Return values
<tt>J_WB</tt>

Here is the caller graph for this function:

drake::Isometry3<T> CalcFramePoseInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBodyFrame< T > &  frame_F 
) const
inline

Computes the pose X_WF of frame_F in the world frame W.

frame_F does not necessarily need to be owned by this RigidBodyTree. However, the RigidBody to which frame_F attaches to has to be owned by this RigidBodyTree.

Parameters
cacheReference to the KinematicsCache.
frame_FReference to the RigidBodyFrame.
Return values
<tt>X_WF</tt>

Here is the caller graph for this function:

Isometry3< T > CalcFramePoseInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body,
const drake::Isometry3< T > &  X_BF 
) const

Computes the pose X_WF of the rigid body frame F in the world frame W.

Frame F is rigidly attached to body.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
X_BFThe pose of frame F in body frame B.
Return values
<tt>X_WF</tt>

Here is the call graph for this function:

drake::Vector6<T> CalcFrameSpatialVelocityInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBodyFrame< T > &  frame_F 
) const
inline

Computes the spatial velocity V_WF of RigidBodyFrame frame_F measured and expressed in the world frame W.

frame_F does not necessarily need to be owned by this RigidBodyTree. However, the RigidBody to which frame_F attaches to has to be owned by this RigidBodyTree.

Parameters
cacheReference to the KinematicsCache.
frame_FReference to the RigidBodyFrame.
Return values
<tt>V_WF</tt>

Here is the caller graph for this function:

drake::Vector6< T > CalcFrameSpatialVelocityInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body,
const drake::Isometry3< T > &  X_BF 
) const

Computes the spatial velocity V_WF of the frame F measured and expressed in the world frame W.

Frame F is rigidly attached to body.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
X_BFThe pose of frame F in body frame B.
Return values
<tt>V_WF</tt>
drake::Vector6< T > CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body,
const drake::Isometry3< T > &  X_BF 
) const

Computes Jdot_WF * v, where J_WF is the Jacobian of spatial velocity, V_WF, of frame F measured and expressed in the world frame W, and v is the generalized velocity.

Frame F is rigidly attached to body.

Parameters
cacheReference to the KinematicsCache.
bodyReference to the RigidBody.
X_BFThe pose of frame F in body frame B.
Return values
<tt>Jdot_WF* v

Here is the call graph for this function:

Here is the caller graph for this function:

drake::Vector6<T> CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBodyFrame< T > &  frame_F 
) const
inline

Computes Jdot_WF * v, where J_WF is the Jacobian of spatial velocity V_WF of frame F measured and expressed in the world frame W, and v is the generalized velocity.

frame_F does not necessarily need to be owned by this RigidBodyTree. However, the RigidBody to which frame_F attaches to has to be owned by this RigidBodyTree.

Parameters
cacheReference to the KinematicsCache.
frame_FReference to the RigidBodyFrame.
Return values
<tt>Jdot_WF* v
drake::Matrix6X< T > CalcFrameSpatialVelocityJacobianInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBody< T > &  body,
const drake::Isometry3< T > &  X_BF,
bool  in_terms_of_qdot = false 
) const

Computes the Jacobian J_WF of the spatial velocity V_WF of frame F measured and expressed in the world frame W such that V_WF = J_WF * v, where v is the generalized velocity.

Frame F is rigidly attached to body.

Parameters
cacheReference to the KinematicsCache.
BReference to the RigidBody.
X_BFThe pose of frame F in body frame B.
in_terms_of_qdottrue for J_WF computed with respect to the time derivative of the generalized position such that V_WF = J_WF * qdot. false for J_WF computed with respect to v.
Return values
<tt>J_WF</tt>

Here is the call graph for this function:

Here is the caller graph for this function:

drake::Matrix6X<T> CalcFrameSpatialVelocityJacobianInWorldFrame ( const KinematicsCache< T > &  cache,
const RigidBodyFrame< T > &  frame_F,
bool  in_terms_of_qdot = false 
) const
inline

Computes the Jacobian J_WF of the spatial velocity V_WF of frame F measured and expressed in the world frame W such that V_WF = J_WF * v, where v is the generalized velocity.

frame_F does not necessarily need to be owned by this RigidBodyTree. However, the RigidBody to which frame_F attaches to has to be owned by this RigidBodyTree.

Parameters
cacheReference to the KinematicsCache.
frame_FReference to the RigidBodyFrame.
in_terms_of_qdottrue for J_WF computed with respect to the time derivative of the generalized position such that V_WF = J_WF * qdot. false for J_WF computed with respect to v.
Return values
<tt>J_WF</tt>
Eigen::Matrix<Scalar, drake::kSpaceDimension, 1> centerOfMass ( const KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set 
) const

Here is the caller graph for this function:

Eigen::Matrix<Scalar, kSpaceDimension, 1> centerOfMass ( const KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set 
) const

Here is the call graph for this function:

Eigen::Matrix<Scalar, drake::kSpaceDimension, Eigen::Dynamic> centerOfMassJacobian ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set,
bool  in_terms_of_qdot = false 
) const

Here is the caller graph for this function:

Matrix<Scalar, kSpaceDimension, Eigen::Dynamic> centerOfMassJacobian ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set,
bool  in_terms_of_qdot 
) const

Here is the call graph for this function:

Eigen::Matrix<Scalar, drake::kSpaceDimension, 1> centerOfMassJacobianDotTimesV ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set 
) const
Matrix<Scalar, kSpaceDimension, 1> centerOfMassJacobianDotTimesV ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set 
) const
drake::TwistMatrix<Scalar> centroidalMomentumMatrix ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set,
bool  in_terms_of_qdot = false 
) const
TwistMatrix<Scalar> centroidalMomentumMatrix ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set,
bool  in_terms_of_qdot 
) const
drake::TwistVector<Scalar> centroidalMomentumMatrixDotTimesV ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set 
) const
TwistVector<Scalar> centroidalMomentumMatrixDotTimesV ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set 
) const
void CheckCacheValidity ( const KinematicsCache< Scalar > &  cache) const

Checks whether cache is valid for use with this RigidBodyTree.

Throws a std::runtime_error exception if it is not valid.

Here is the call graph for this function:

std::unique_ptr<RigidBodyTree<double> > Clone ( ) const

Returns a deep clone of this RigidBodyTree<double>.

Currently, everything except for collision and visual elements are cloned.

Here is the caller graph for this function:

vector< size_t > collidingPoints ( const KinematicsCache< double > &  cache,
const std::vector< Eigen::Vector3d > &  points,
double  collision_threshold 
)
virtual

Here is the caller graph for this function:

bool collidingPointsCheckOnly ( const KinematicsCache< double > &  cache,
const std::vector< Eigen::Vector3d > &  points,
double  collision_threshold 
)
virtual

Here is the caller graph for this function:

bool collisionDetect ( const KinematicsCache< double > &  cache,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  xA,
Eigen::Matrix3Xd &  xB,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
const std::vector< DrakeCollision::ElementId > &  ids_to_check,
bool  use_margins 
)

Here is the caller graph for this function:

bool collisionDetect ( const KinematicsCache< double > &  cache,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  xA,
Eigen::Matrix3Xd &  xB,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
const std::vector< int > &  bodies_idx,
const std::set< std::string > &  active_element_groups,
bool  use_margins = true 
)
bool collisionDetect ( const KinematicsCache< double > &  cache,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  xA,
Eigen::Matrix3Xd &  xB,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
const std::vector< int > &  bodies_idx,
bool  use_margins = true 
)
bool collisionDetect ( const KinematicsCache< double > &  cache,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  xA,
Eigen::Matrix3Xd &  xB,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
const std::set< std::string > &  active_element_groups,
bool  use_margins = true 
)
bool collisionDetect ( const KinematicsCache< double > &  cache,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  xA,
Eigen::Matrix3Xd &  xB,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
bool  use_margins = true 
)
void collisionDetectFromPoints ( const KinematicsCache< double > &  cache,
const Eigen::Matrix3Xd &  points,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  x,
Eigen::Matrix3Xd &  body_x,
std::vector< int > &  body_idx,
bool  use_margins 
)

Computes the signed distance from the given points to the nearest body in the RigidBodyTree.

Here is the call graph for this function:

Here is the caller graph for this function:

bool collisionRaycast ( const KinematicsCache< double > &  cache,
const Eigen::Matrix3Xd &  origins,
const Eigen::Matrix3Xd &  ray_endpoints,
Eigen::VectorXd &  distances,
bool  use_margins = false 
)

Here is the caller graph for this function:

bool collisionRaycast ( const KinematicsCache< double > &  cache,
const Eigen::Matrix3Xd &  origins,
const Eigen::Matrix3Xd &  ray_endpoints,
Eigen::VectorXd &  distances,
Eigen::Matrix3Xd &  normals,
bool  use_margins = false 
)
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> compactToFull ( const Eigen::MatrixBase< Derived > &  compact,
const std::vector< int > &  joint_path,
bool  in_terms_of_qdot 
) const
inline
void compile ( )

Here is the call graph for this function:

Here is the caller graph for this function:

void computeContactJacobians ( const KinematicsCache< Scalar > &  cache,
Ref< const VectorXi > const &  idxA,
Ref< const VectorXi > const &  idxB,
Ref< const Matrix3Xd > const &  xA,
Ref< const Matrix3Xd > const &  xB,
Matrix< Scalar, Dynamic, Dynamic > &  J 
) const

Here is the call graph for this function:

void computeContactJacobians ( const KinematicsCache< Scalar > &  cache,
Eigen::Ref< const Eigen::VectorXi > const &  idxA,
Eigen::Ref< const Eigen::VectorXi > const &  idxB,
Eigen::Ref< const Eigen::Matrix3Xd > const &  xA,
Eigen::Ref< const Eigen::Matrix3Xd > const &  xB,
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &  J 
) const

Computes the Jacobian for many points in the format currently used by MATLAB.

(possibly should be scheduled for deletion, taking accumulateContactJacobians() with it)

Here is the caller graph for this function:

std::vector< DrakeCollision::PointPair > ComputeMaximumDepthCollisionPoints ( const KinematicsCache< double > &  cache,
bool  use_margins = true 
)

Computes the point of closest approach between bodies in the RigidBodyTree that are in contact.

Parameters
cache[in]a KinematicsCache constructed by RigidBodyTree::doKinematics given q and v.

Collision points are returned as a vector of PointPair's. See the documentation for PointPair for details. The collision point on the surface of each body is stored in the PointPair structure in the frame of the corresponding body.

Parameters
use_margins[in]If true the model uses the representation with margins. If false, the representation without margins is used instead.

Here is the call graph for this function:

map< string, int > computePositionNameToIndexMap ( ) const

Returns a map from DOF position name to DOF index within the output vector of this RigidBodyTree.

WARNING: There is a known bug in this method, see: #4697.

Here is the caller graph for this function:

KinematicsCache< T > CreateKinematicsCache ( ) const

Creates a KinematicsCache to perform computations with this RigidBodyTree.

The returned KinematicsCache is consistently templated on the scalar type for this RigidBodyTree instance. Aborts if this RigidBodyTree was not previously initialized with a call to RigidBodyTree::compile().

Returns
The created KinematicsCache.

Here is the caller graph for this function:

KinematicsCache< T > CreateKinematicsCacheFromBodiesVector ( const std::vector< std::unique_ptr< RigidBody< T >>> &  bodies)
static

Creates a KinematicsCache given a reference to a vector of rigid bodies contained within a RigidBodyTree.

This method is static since all the information to create the corresponding KinematicsCache resides in the input parameter vector bodies.

Parameters
bodiesA vector of unique pointers to the rigid bodies of a given RigidBodyTree for which a KinematicsCache needs to be created.
Returns
The created KinematicsCache.

Here is the call graph for this function:

KinematicsCache< CacheT > CreateKinematicsCacheWithType ( ) const

A helper template method used to create a KinematicsCache templated on CacheT from a RigidBodyTree templated on T, with CacheT and T not necessarily the same scalar type.

This method is particularly useful in mex files where only a reference to a RigidBodyTree<double> is available to create kinematics caches on different scalar types. Aborts if this RigidBodyTree was not previously initialized with a call to RigidBodyTree::compile().

Users should not call this method but instead create KinematicsCache objects with RigidBodyTree:CreateKinematicsCache().

Template Parameters
CacheTThe scalar type for the returned KinematicsCache.
Returns
A KinematicsCache templated on CacheT that can be used for computations on this RigidBodyTree with methods instantiated on CacheT.

Here is the call graph for this function:

Here is the caller graph for this function:

void DefineCollisionFilterGroup ( const std::string &  name)

Attempts to define a new collision filter group.

The given name must be unique in the current session (see CollisionFilterGroupManager for more detail). Duplicate names or attempting to add more collision filter groups than the system can handle will lead to failure. In the event of failure, an exception is thrown. kMaxNumCollisionFilterGroups defines the limit.

Parameters
nameThe unique name of the new group.
KinematicsCache< typename DerivedQ::Scalar > doKinematics ( const Eigen::MatrixBase< DerivedQ > &  q) const

Initializes a KinematicsCache with the given configuration q, computes the kinematics, and returns the cache.

This method is explicitly instantiated in rigid_body_tree.cc for a small set of supported DerivedQ.

Here is the call graph for this function:

Here is the caller graph for this function:

KinematicsCache< typename DerivedQ::Scalar > doKinematics ( const Eigen::MatrixBase< DerivedQ > &  q,
const Eigen::MatrixBase< DerivedV > &  v,
bool  compute_JdotV = true 
) const

Initializes a KinematicsCache with the given configuration q and velocity v, computes the kinematics, and returns the cache.

This method is explicitly instantiated in rigid_body_tree.cc for a small set of supported DerivedQ and DerivedV.

Here is the call graph for this function:

void doKinematics ( KinematicsCache< Scalar > &  cache,
bool  compute_JdotV = false 
) const

Computes the kinematics on the given cache.

This method is explicitly instantiated in rigid_body_tree.cc for a small set of supported Scalar types.

Here is the call graph for this function:

void drawKinematicTree ( std::string  graphviz_dotfile_filename) const

Here is the call graph for this function:

Eigen::Matrix<Scalar, Eigen::Dynamic, 1> dynamicsBiasTerm ( KinematicsCache< Scalar > &  cache,
const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, drake::WrenchVector< Scalar >> &  external_wrenches,
bool  include_velocity_terms = true 
) const

Compute the term \( C(q, v, f_\text{ext}) \) in the manipulator equations

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u \]

.

Convenience method that calls inverseDynamics with \( \dot{v} = 0 \). See inverseDynamics for argument descriptions.

See Also
inverseDynamics

Here is the caller graph for this function:

Matrix<Scalar, Eigen::Dynamic, 1> dynamicsBiasTerm ( KinematicsCache< Scalar > &  cache,
const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, WrenchVector< Scalar >> &  external_wrenches,
bool  include_velocity_terms 
) const
std::vector< int > FindAncestorBodies ( int  body_index) const

Finds the ancestors of a body.

The ancestors include the body's parent, the parent's parent, etc., all the way to the root of this RigidBodyTree, which represents the world.

Parameters
[in]body_indexThe index of the body in this RigidBodyTree for which the ancestors of the body are found. Ancestors are returned in a vector of body indexes.
Returns
A vector of body indexes of the ancestor bodies of the body with index body_index.

Here is the call graph for this function:

void findAncestorBodies ( std::vector< int > &  ancestor_bodies,
int  body 
) const
std::vector< int > FindBaseBodies ( int  model_instance_id = -1) const

Obtains a vector of indexes of the bodies that are directly attached to the world via any type of joint.

This method has a time complexity of O(N) where N is the number of bodies in the tree, which can be determined by calling RigidBodyTree::get_num_bodies().

RigidBody< T > * FindBody ( const std::string &  body_name,
const std::string &  model_name = "",
int  model_id = -1 
) const

Finds a body with the specified body_name belonging to a model with the specified model_name and model_id.

Note that if model_name is the empty string and model_id is -1, every model is searched. If model_name and model_id are inconsistent, no body will be found and an exception will be thrown.

Parameters
[in]body_nameThe name of the body to find.
[in]model_nameThe name of the model to which the body belongs. If this value is an empty string, every model is searched.
[in]model_idThe ID of the model to which the body belongs. If this value is -1, every model is searched.
Exceptions
std::logic_errorif multiple matching bodies are found or no matching bodies are found.

Here is the call graph for this function:

Here is the caller graph for this function:

const RigidBody< double > * FindBody ( DrakeCollision::ElementId  element_id) const

Reports the RigidBody that owns the collision element indicated by the id.

Parameters
element_idThe id to query.
Returns
A pointer to the owning RigidBody.
Exceptions
std::logic_errorif no body can be mapped to the element id.

Here is the call graph for this function:

int FindBodyIndex ( const std::string &  body_name,
int  model_instance_id = -1 
) const

Obtains the index of a rigid body within this rigid body tree.

The rigid body tree maintains a vector of pointers to all rigid bodies that are part of the rigid body tree. The index of a rigid body is the index within this vector at which a pointer to the rigid body is stored.

Parameters
[in]body_nameThe body whose index we want to find. It should be unique within the searched models, otherwise an exception will be thrown.
[in]model_instance_idThe ID of the model instance. This parameter is optional. If supplied, only the model instance with the specified instance ID is searched; otherwise, all model instances are searched.
Returns
The index of the specified rigid body.
Exceptions
std::logic_errorif no rigid body with the specified body_name and model_id was found or if multiple matching rigid bodies were found.

Here is the call graph for this function:

Here is the caller graph for this function:

RigidBody< T > * FindChildBodyOfJoint ( const std::string &  joint_name,
int  model_instance_id = -1 
) const

Obtains a pointer to the rigid body whose parent joint is named joint_name and is part of a model instance with ID model_instance_id.

Parameters
[in]joint_nameThe name of the parent joint of the rigid body to find.
[in]model_instance_idThe ID of the model instance that owns the rigid body to find. This parameter is optional. If supplied, the set of rigid bodies to search through is restricted to those that belong to the specified model instance. Otherwise, all rigid bodies in this tree are searched.
Returns
A pointer to the rigid body whose parent joint is named joint_name joint and, if model_instance_id is specified, is part of the specified model instance.
Exceptions
std::runtime_errorIf either no rigid body is found or multiple matching rigid bodies are found.

Here is the call graph for this function:

Here is the caller graph for this function:

std::vector< int > FindChildrenOfBody ( int  parent_body_index,
int  model_instance_id = -1 
) const

Returns a vector of indexes of bodies that are the children of the body at index parent_body_index.

The resulting list can be further filtered to be bodies that belong to model instance ID model_instance_id. This method has a time complexity of O(N) where N is the number of bodies in the tree, which can be determined by calling RigidBodyTree::get_num_bodies().

const DrakeCollision::Element* FindCollisionElement ( const DrakeCollision::ElementId id) const
inline

Retrieve a const pointer to an element of the collision model.

Note: The use of Find (instead of get) and the use of CamelCase both imply a potential runtime cost are carried over from the collision model accessor method.

Here is the caller graph for this function:

shared_ptr< RigidBodyFrame< T > > findFrame ( const std::string &  frame_name,
int  model_id = -1 
) const

Finds a frame of the specified frame_name belonging to a model with the specified model_id.

Parameters
[in]frame_nameThe name of the frame to find.
[in]model_idThe ID of the model to which the frame belongs. If this value is -1, search all models.
Returns
The frame with the specified name and model instance ID.
Exceptions
std::logic_errorif either multiple matching frames are found or no matching frame is found.

Here is the call graph for this function:

int FindIndexOfChildBodyOfJoint ( const std::string &  joint_name,
int  model_instance_id = -1 
) const

Returns the index within the vector of rigid bodies of the rigid body whose parent joint is named joint_name and is part of a model instance with ID model_instance_id.

Parameters
[in]joint_nameThe name of the parent joint of the rigid body whose index is being searched for.
[in]model_instance_idThe ID of the model instance that owns the rigid body to find. This parameter is optional. If supplied, the set of rigid bodies to search through is restricted to those that belong to the specified model instance. Otherwise, all rigid bodies in this tree are searched.
Returns
The index of the rigid body whose parent joint is named joint_name and, if model_instance_id is specified, is part of the specified model instance.
Exceptions
std::runtime_errorIf either no rigid body is found or multiple matching rigid bodies are found.

Here is the call graph for this function:

RigidBody< T > * findJoint ( const std::string &  joint_name,
int  model_id = -1 
) const
int findJointId ( const std::string &  joint_name,
int  model_id = -1 
) const
KinematicPath findKinematicPath ( int  start_body_or_frame_idx,
int  end_body_or_frame_idx 
) const

Here is the call graph for this function:

RigidBody< T > * findLink ( const std::string &  link_name,
const std::string &  model_name = "",
int  model_id = -1 
) const

This is a deprecated version of FindBody(...).

Please use FindBody(...) instead.

int findLinkId ( const std::string &  link_name,
int  model_id = -1 
) const

This is a deprecated version of FindBodyIndex(...).

Please use FindBodyIndex(...) instead.

std::vector< const RigidBody< T > * > FindModelInstanceBodies ( int  model_instance_id) const

Returns a vector of pointers to all rigid bodies in this tree that belong to a particular model instance.

Parameters
[in]model_instance_idThe ID of the model instance whose rigid bodies are being searched for.
Returns
A vector of pointers to every rigid body belonging to the specified model instance.
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> forwardKinPositionGradient ( const KinematicsCache< Scalar > &  cache,
int  npoints,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const
Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> forwardKinPositionGradient ( const KinematicsCache< Scalar > &  cache,
int  npoints,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const

Here is the call graph for this function:

Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> frictionTorques ( Eigen::MatrixBase< DerivedV > const &  v) const
Matrix<typename DerivedV::Scalar, Dynamic, 1> frictionTorques ( Eigen::MatrixBase< DerivedV > const &  v) const

Here is the call graph for this function:

drake::TwistMatrix<Scalar> geometricJacobian ( const KinematicsCache< Scalar > &  cache,
int  base_body_or_frame_ind,
int  end_effector_body_or_frame_ind,
int  expressed_in_body_or_frame_ind,
bool  in_terms_of_qdot = false,
std::vector< int > *  v_indices = nullptr 
) const

Here is the caller graph for this function:

TwistMatrix<Scalar> geometricJacobian ( const KinematicsCache< Scalar > &  cache,
int  base_body_or_frame_ind,
int  end_effector_body_or_frame_ind,
int  expressed_in_body_or_frame_ind,
bool  in_terms_of_qdot,
std::vector< int > *  v_or_qdot_indices 
) const

Here is the call graph for this function:

drake::TwistVector<Scalar> geometricJacobianDotTimesV ( const KinematicsCache< Scalar > &  cache,
int  base_body_or_frame_ind,
int  end_effector_body_or_frame_ind,
int  expressed_in_body_or_frame_ind 
) const
TwistVector<Scalar> geometricJacobianDotTimesV ( const KinematicsCache< Scalar > &  cache,
int  base_body_or_frame_ind,
int  end_effector_body_or_frame_ind,
int  expressed_in_body_or_frame_ind 
) const

Here is the call graph for this function:

const RigidBody< T > & get_body ( int  body_index) const

Returns the body at index body_index.

Parameter body_index must be between zero and the number of bodies in this tree, which can be determined by calling RigidBodyTree::get_num_bodies().

Here is the caller graph for this function:

RigidBody< T > * get_mutable_body ( int  body_index)

Returns the body at index body_index.

Parameter body_index must be between zero and the number of bodies in this tree, which can be determined by calling RigidBodyTree::get_num_bodies().

int get_next_clique_id ( )
inline
int get_num_actuators ( ) const

Returns the number of actuators in this RigidBodyTree.

Here is the caller graph for this function:

int get_num_bodies ( ) const

Returns the number of bodies in this tree.

This includes the one body that represents the world.

Here is the caller graph for this function:

int get_num_frames ( ) const

Returns the number of frames in this tree.

Here is the caller graph for this function:

int get_num_model_instances ( ) const

Returns the number of model instances in the tree, not including the world.

Here is the caller graph for this function:

int get_num_positions ( ) const

Returns the number of position states outputted by this RigidBodyTree.

Here is the caller graph for this function:

int get_num_velocities ( ) const

Returns the number of velocity states outputted by this RigidBodyTree.

Here is the caller graph for this function:

int get_number_of_bodies ( ) const
int get_number_of_model_instances ( ) const
string get_position_name ( int  position_num) const

Returns the name of the position state at index position_num within this RigidBodyTree's state vector.

Parameters
[in]position_numAn index value between zero and number_of_positions().
Returns
The name of the position value at index position_num.

Here is the caller graph for this function:

string get_velocity_name ( int  velocity_num) const

Returns the name of the velocity state at index velocity_num within this RigidBodyTree's state vector.

Parameters
[in]velocity_numAn index value between number_of_positions() and number_of_veocities().
Returns
The name of the velocity value at index velocity_num.

Here is the caller graph for this function:

const RigidBodyActuator & GetActuator ( const std::string &  name) const

Obtains a rigid body actuator from this rigid body tree.

The actuator is selected based on its name.

Parameters
nameThe name of the rigid body actuator to get.
Returns
A const reference to the rigid body actuator with name name.
Exceptions
std::invalid_argumentif no rigid body actuator with name name exists.
std::string getBodyOrFrameName ( int  body_or_frame_id) const

Here is the caller graph for this function:

double getMass ( const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set) const

Computes the total combined mass of a set of model instances.

Parameters
[in]model_instance_id_setA set of model instance ID values corresponding to the model instances whose masses should be included in the returned value.
Returns
The total combined mass of the model instances in model_instance_id_set.

Here is the call graph for this function:

Here is the caller graph for this function:

int getNumContacts ( const std::set< int > &  body_idx) const
size_t getNumJointLimitConstraints ( ) const

Here is the call graph for this function:

Here is the caller graph for this function:

size_t getNumPositionConstraints ( ) const

Here is the caller graph for this function:

std::string getPositionName ( int  position_num) const
static drake::MatrixX<Scalar> GetQDotToVelocityMapping ( const KinematicsCache< Scalar > &  cache)
static
MatrixX<Scalar> GetQDotToVelocityMapping ( const KinematicsCache< Scalar > &  cache)

Here is the call graph for this function:

Eigen::VectorXd getRandomConfiguration ( std::default_random_engine &  generator) const

Here is the call graph for this function:

Here is the caller graph for this function:

string getStateName ( int  state_num) const
void getTerrainContactPoints ( const RigidBody< T > &  body,
Eigen::Matrix3Xd *  terrain_points,
const std::string &  group_name = "" 
) const

Gets the contact points defined by a body's collision elements.

Parameters
[in]bodyThe body who's collision elements are searched.
[out]terrain_pointsContact points are added to this matrix.
[in]group_nameIf a group name was given, use it to look up the subset of collision elements that belong to that collision group. Otherwise, uses the full set of collision elements that belong to the body.
Exceptions
std::runtime_errorif an invalid group name is given.

Here is the call graph for this function:

std::string getVelocityName ( int  velocity_num) const
static drake::MatrixX<Scalar> GetVelocityToQDotMapping ( const KinematicsCache< Scalar > &  cache)
static
MatrixX<Scalar> GetVelocityToQDotMapping ( const KinematicsCache< Scalar > &  cache)

Here is the call graph for this function:

Eigen::VectorXd getZeroConfiguration ( ) const

Here is the call graph for this function:

Here is the caller graph for this function:

bool initialized ( ) const
inline

Returns whether this RigidBodyTree is initialized.

It is initialized after compile() is called.

Here is the caller graph for this function:

Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inverseDynamics ( KinematicsCache< Scalar > &  cache,
const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, drake::WrenchVector< Scalar >> &  external_wrenches,
const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &  vd,
bool  include_velocity_terms = true 
) const

Compute

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) \]

that is, the left hand side of the manipulator equations

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u \]

.

Note that the 'dynamics bias term' \( C(q, v, f_\text{ext}) \) can be computed by simply setting \( \dot{v} = 0\). Note also that if only the gravitational terms contained in \( C(q, v, *f_\text{ext}) \) are required, one can set include_velocity_terms to false. Alternatively, one can pass in a KinematicsCache created with \( v = 0\) or without specifying the velocity vector.

Algorithm: recursive Newton-Euler. Does not explicitly compute mass matrix.

Parameters
cachea KinematicsCache constructed given \( q \) and \( v \)
external_wrenchesexternal wrenches exerted upon bodies ( \( f_\text{ext} \)). Expressed in body frame.
vd\( \dot{v} \)
include_velocity_termswhether to include velocity-dependent terms in \( C(q, v, f_\text{ext}) \). Setting include_velocity_terms to false is Equivalent to setting \( v = 0 \)
Returns
\( H(q) \dot{v} + C(q, v, f_\text{ext}) \)

Here is the caller graph for this function:

Matrix<Scalar, Eigen::Dynamic, 1> inverseDynamics ( KinematicsCache< Scalar > &  cache,
const drake::eigen_aligned_std_unordered_map< RigidBody< T > const *, WrenchVector< Scalar >> &  external_wrenches,
const Matrix< Scalar, Eigen::Dynamic, 1 > &  vd,
bool  include_velocity_terms 
) const

Here is the call graph for this function:

bool is_part_of_model_instances ( const RigidBody< T > &  body,
const std::set< int > &  model_instance_id_set 
) const

Returns true if body is part of a model instance whose ID is in model_instance_id_set.

Here is the call graph for this function:

void jointLimitConstraints ( Eigen::MatrixBase< DerivedA > const &  q,
Eigen::MatrixBase< DerivedB > &  phi,
Eigen::MatrixBase< DerivedC > &  J 
) const

Here is the caller graph for this function:

void jointLimitConstraints ( MatrixBase< DerivedA > const &  q,
MatrixBase< DerivedB > &  phi,
MatrixBase< DerivedC > &  J 
) const

Here is the call graph for this function:

drake::Matrix6< T > LumpedSpatialInertiaInWorldFrame ( const KinematicsCache< T > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set 
) const

Computes the summed spatial inertia in the world frame of all bodies that belong to model instances in model_instance_id_set.

Parameters
cacheReference to the KinematicsCache.
model_instance_id_setA set of model instance ID values corresponding to the model instances whose spatial inertia should be included in the returned value.
Returns
The summed spatial inertia.

Here is the call graph for this function:

Here is the caller graph for this function:

drake::Matrix6< T > LumpedSpatialInertiaInWorldFrame ( const KinematicsCache< T > &  cache,
const std::vector< const RigidBody< T > * > &  bodies_of_interest 
) const

Computes the summed spatial inertia in the world frame of all the bodies in bodies_of_interest.

Parameters
cacheReference to the KinematicsCache.
bodies_of_interestVector of bodies, whose spatial inertia will be summed and returned.
Returns
The summed spatial inertia.

Here is the call graph for this function:

Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> massMatrix ( KinematicsCache< Scalar > &  cache) const

Compute the positive definite mass (configuration space) matrix \( *H(q) \), defined by \(T = \frac{1}{2} v^T H(q) v \), where \( T \) is kinetic energy.

The mass matrix also appears in the manipulator equations

\[ H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u \]

Parameters
cachea KinematicsCache constructed given \( q \)
Returns
the mass matrix \( H(q) \)

Here is the caller graph for this function:

Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> massMatrix ( KinematicsCache< Scalar > &  cache) const

Here is the call graph for this function:

int number_of_positions ( ) const
int number_of_velocities ( ) const
int parseBodyOrFrameID ( const int  body_or_frame_id,
Eigen::Transform< Scalar, 3, Eigen::Isometry > *  Tframe 
) const

Here is the caller graph for this function:

int parseBodyOrFrameID ( const int  body_or_frame_id) const
int parseBodyOrFrameID ( const int  body_or_frame_id,
Eigen::Transform< Scalar, 3, Isometry > *  Tframe 
) const
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> positionConstraints ( const KinematicsCache< Scalar > &  cache) const

Here is the caller graph for this function:

Matrix<Scalar, Eigen::Dynamic, 1> positionConstraints ( const KinematicsCache< Scalar > &  cache) const
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> positionConstraintsJacDotTimesV ( const KinematicsCache< Scalar > &  cache) const

Here is the caller graph for this function:

Matrix<Scalar, Eigen::Dynamic, 1> positionConstraintsJacDotTimesV ( const KinematicsCache< Scalar > &  cache) const
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> positionConstraintsJacobian ( const KinematicsCache< Scalar > &  cache,
bool  in_terms_of_qdot = true 
) const

Here is the caller graph for this function:

Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> positionConstraintsJacobian ( const KinematicsCache< Scalar > &  cache,
bool  in_terms_of_qdot 
) const
void potentialCollisions ( const KinematicsCache< double > &  cache,
Eigen::VectorXd &  phi,
Eigen::Matrix3Xd &  normal,
Eigen::Matrix3Xd &  xA,
Eigen::Matrix3Xd &  xB,
std::vector< int > &  bodyA_idx,
std::vector< int > &  bodyB_idx,
bool  use_margins = true 
)

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix<Scalar, 4, 1> relativeQuaternion ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const
inline

Here is the caller graph for this function:

Eigen::Matrix<Scalar, drake::kQuaternionSize, Eigen::Dynamic> relativeQuaternionJacobian ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind,
bool  in_terms_of_qdot 
) const

Here is the caller graph for this function:

Eigen::Matrix<Scalar, kQuaternionSize, Eigen::Dynamic> relativeQuaternionJacobian ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind,
bool  in_terms_of_qdot 
) const

Here is the call graph for this function:

Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > relativeQuaternionJacobianDotTimesV ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix<Scalar, 3, 1> relativeRollPitchYaw ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const
inline

Here is the caller graph for this function:

Eigen::Matrix<Scalar, drake::kRpySize, Eigen::Dynamic> relativeRollPitchYawJacobian ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind,
bool  in_terms_of_qdot 
) const

Here is the caller graph for this function:

Eigen::Matrix<Scalar, kRpySize, Eigen::Dynamic> relativeRollPitchYawJacobian ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind,
bool  in_terms_of_qdot 
) const

Here is the call graph for this function:

Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > relativeRollPitchYawJacobianDotTimesV ( const KinematicsCache< Scalar > &  cache,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Transform<Scalar, drake::kSpaceDimension, Eigen::Isometry> relativeTransform ( const KinematicsCache< Scalar > &  cache,
int  base_or_frame_ind,
int  body_or_frame_ind 
) const

Here is the caller graph for this function:

Transform<Scalar, 3, Isometry> relativeTransform ( const KinematicsCache< Scalar > &  cache,
int  base_or_frame_ind,
int  body_or_frame_ind 
) const

Here is the call graph for this function:

drake::TwistVector<Scalar> relativeTwist ( const KinematicsCache< Scalar > &  cache,
int  base_or_frame_ind,
int  body_or_frame_ind,
int  expressed_in_body_or_frame_ind 
) const

Here is the caller graph for this function:

TwistVector<Scalar> relativeTwist ( const KinematicsCache< Scalar > &  cache,
int  base_or_frame_ind,
int  body_or_frame_ind,
int  expressed_in_body_or_frame_ind 
) const

Here is the call graph for this function:

void removeCollisionGroupsIf ( UnaryPredicate  test)
inline
std::pair< Eigen::Vector3d, double > resolveCenterOfPressure ( const KinematicsCache< double > &  cache,
const std::vector< ForceTorqueMeasurement > &  force_torque_measurements,
const Eigen::MatrixBase< DerivedNormal > &  normal,
const Eigen::MatrixBase< DerivedPoint > &  point_on_contact_plane 
) const

Computes CoP in world frame.

Normal and point on contact plane should be in world frame too.

Here is the call graph for this function:

Here is the caller graph for this function:

void SetBodyCollisionFilters ( const RigidBody< T > &  body,
const DrakeCollision::bitmask group,
const DrakeCollision::bitmask ignores 
)

Directly set the masks for a body.

The values will remain in the current session (i.e., until CollisionFilterGroupManager::Clear() is called). This is a convenience function for Matlab integration. The Matlab parser handles the mapping of collision filter group names to ids and passes the mapped ids directly the manager for when the tree gets compiled. It relies on correct encoding of groups into bitmasks.

Here is the caller graph for this function:

void surfaceTangents ( Eigen::Map< Eigen::Matrix3Xd > const &  normals,
std::vector< Eigen::Map< Eigen::Matrix3Xd >> &  tangents 
) const

Here is the call graph for this function:

Here is the caller graph for this function:

bool transformCollisionFrame ( RigidBody< T > *  body,
const Eigen::Isometry3d &  displace_transform 
)

Applies the given transform to the given body's collision elements, displacing them from their current configurations.

These new poses will be considered the elements' pose with respect to the body.

This is important to the parsing code to maintain a Drake RigidBodyTree invariant. RigidBody instances do not maintain their own pose relative to their in-board joint. The joint's space is considered to be the body's space. So, if a URDF or SDF file defines the body with a non-identity pose relative to the parent, the parser uses this to move the collision elements relative to the effective body frame – that of the parent joint.

Parameters
bodyThe body whose collision elements will be moved.
displace_transformThe transform to apply to each collision element.
trueif the collision element was successfully updated.
Returns
true if the 's elements were successfully transformed.

Here is the call graph for this function:

Eigen::Matrix<Scalar, 3, DerivedPoints::ColsAtCompileTime> transformPoints ( const KinematicsCache< Scalar > &  cache,
const Eigen::MatrixBase< DerivedPoints > &  points,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const
inline

Here is the caller graph for this function:

Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > transformPointsJacobian ( const KinematicsCache< Scalar > &  cache,
const Eigen::MatrixBase< DerivedPoints > &  points,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind,
bool  in_terms_of_qdot 
) const

Here is the caller graph for this function:

Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > transformPointsJacobianDotTimesV ( const KinematicsCache< Scalar > &  cache,
const Eigen::MatrixBase< DerivedPoints > &  points,
int  from_body_or_frame_ind,
int  to_body_or_frame_ind 
) const

Here is the call graph for this function:

Here is the caller graph for this function:

static drake::MatrixX<typename Derived::Scalar> transformQDotMappingToVelocityMapping ( const KinematicsCache< typename Derived::Scalar > &  cache,
const Eigen::MatrixBase< Derived > &  Ap 
)
static

Converts a matrix A, which transforms the time derivative of generalized coordinates (qdot) to an output space X, to a matrix B, which transforms generalized velocities (v) to the same space X.

For example, A could be a Jacobian matrix that transforms qdot to spatial velocities at the end effector. Formally, this would be the matrix of partial derivatives of end-effector configuration computed with respect to the generalized coordinates (q). This function would allow the user to transform this Jacobian matrix to the more commonly used one: the matrix of partial derivatives of end-effector configuration computed with respect to quasi-coordinates (ꝗ).

Parameters
Apa m x nq sized matrix, where nq is the dimension of the generalized coordinates.
Return values
B,am x nv sized matrix, where nv is the dimension of the generalized velocities.
See Also
transformVelocityMappingToQDotMapping()

Here is the caller graph for this function:

MatrixX<typename Derived::Scalar> transformQDotMappingToVelocityMapping ( const KinematicsCache< typename Derived::Scalar > &  cache,
const Eigen::MatrixBase< Derived > &  Ap 
)

Here is the call graph for this function:

VectorX< T > transformQDotToVelocity ( const KinematicsCache< T > &  cache,
const drake::VectorX< T > &  qdot 
)
static

Converts a vector of the time derivative of generalized coordinates (qdot) to generalized velocity (v).

Parameters
cachethe kinematics cache, which is assumed to be up-to-date with respect to the state
qdota nq dimensional vector, where nq is the dimension of the generalized coordinates.
Returns
a nv dimensional vector, where nv is the dimension of the generalized velocities.
See Also
transformVelocityToQDot()

Here is the call graph for this function:

Here is the caller graph for this function:

drake::TwistVector<Scalar> transformSpatialAcceleration ( const KinematicsCache< Scalar > &  cache,
const drake::TwistVector< Scalar > &  spatial_acceleration,
int  base_or_frame_ind,
int  body_or_frame_ind,
int  old_body_or_frame_ind,
int  new_body_or_frame_ind 
) const
TwistVector<Scalar> transformSpatialAcceleration ( const KinematicsCache< Scalar > &  cache,
const TwistVector< Scalar > &  spatial_acceleration,
int  base_ind,
int  body_ind,
int  old_expressed_in_body_or_frame_ind,
int  new_expressed_in_body_or_frame_ind 
) const

Here is the call graph for this function:

static drake::MatrixX<typename Derived::Scalar> transformVelocityMappingToQDotMapping ( const KinematicsCache< typename Derived::Scalar > &  cache,
const Eigen::MatrixBase< Derived > &  Av 
)
static

Converts a matrix B, which transforms generalized velocities (v) to an output space X, to a matrix A, which transforms the time derivative of generalized coordinates (qdot) to the same output X.

For example, B could be a Jacobian matrix that transforms generalized velocities to spatial velocities at the end-effector. Formally, this would be the matrix of partial derivatives of end-effector configuration computed with respect to quasi-coordinates (ꝗ). This function would allow transforming that Jacobian so that all partial derivatives would be computed with respect to qdot.

Parameters
Av,am x nv sized matrix, where nv is the dimension of the generalized velocities.
Return values
Aa m x nq sized matrix, where nq is the dimension of the generalized coordinates.
See Also
transformQDotMappingToVelocityMapping()
MatrixX<typename Derived::Scalar> transformVelocityMappingToQDotMapping ( const KinematicsCache< typename Derived::Scalar > &  cache,
const Eigen::MatrixBase< Derived > &  Av 
)

Here is the call graph for this function:

VectorX< T > transformVelocityToQDot ( const KinematicsCache< T > &  cache,
const drake::VectorX< T > &  v 
)
static

Converts a vector of generalized velocities (v) to the time derivative of generalized coordinates (qdot).

Parameters
cachethe kinematics cache, which is assumed to be up-to-date with respect to the state
va nv dimensional vector, where nv is the dimension of the generalized velocities.
Return values
qdota nq dimensional vector, where nq is the dimension of the generalized coordinates.
See Also
transformQDotToVelocity()

Here is the call graph for this function:

Here is the caller graph for this function:

void updateCollisionElements ( const RigidBody< T > &  body,
const Eigen::Transform< double, 3, Eigen::Isometry > &  transform_to_world 
)

Here is the call graph for this function:

void updateDynamicCollisionElements ( const KinematicsCache< double > &  kin_cache)

Here is the call graph for this function:

RigidBody<T>& world ( )
inline

Returns a mutable reference to the RigidBody associated with the world in the model.

This is the root of the RigidBodyTree.

Here is the caller graph for this function:

const RigidBody<T>& world ( ) const
inline

Returns a const reference to the RigidBody associated with the world in the model.

This is the root of the RigidBodyTree.

drake::TwistMatrix<Scalar> worldMomentumMatrix ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set,
bool  in_terms_of_qdot = false 
) const

Here is the caller graph for this function:

TwistMatrix<Scalar> worldMomentumMatrix ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set,
bool  in_terms_of_qdot 
) const

Here is the call graph for this function:

drake::TwistVector<Scalar> worldMomentumMatrixDotTimesV ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set = RigidBodyTreeConstants::default_model_instance_id_set 
) const

Here is the caller graph for this function:

TwistVector<Scalar> worldMomentumMatrixDotTimesV ( KinematicsCache< Scalar > &  cache,
const std::set< int > &  model_instance_id_set 
) const

Here is the call graph for this function:

Friends And Related Function Documentation

std::ostream& operator<< ( std::ostream &  os,
const RigidBodyTree< double > &  tree 
)
friend

A toString method for this class.

Member Data Documentation

std::vector<RigidBodyActuator, Eigen::aligned_allocator<RigidBodyActuator> > actuators
Eigen::MatrixXd B
std::vector<std::unique_ptr<RigidBody<T> > > bodies
std::vector<std::shared_ptr<RigidBodyFrame<T> > > frames
Eigen::VectorXd joint_limit_max
Eigen::VectorXd joint_limit_min
std::vector<RigidBodyLoop<T>, Eigen::aligned_allocator<RigidBodyLoop<T> > > loops

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