#include <drake/systems/plants/RigidBodyTree.h>

Collaboration diagram for RigidBodyTree:

[legend]

Public Member Functions
	RigidBodyTree (const std::string &urdf_filename, const DrakeJoint::FloatingBaseType floating_base_type=DrakeJoint::ROLLPITCHYAW)

	RigidBodyTree (void)

virtual	~RigidBodyTree (void)

void	addRobotFromURDFString (const std::string &xml_string, const std::string &root_dir=".", const DrakeJoint::FloatingBaseType floating_base_type=DrakeJoint::ROLLPITCHYAW, std::shared_ptr< RigidBodyFrame > weld_to_frame=nullptr)

void	addRobotFromURDFString (const std::string &xml_string, std::map< std::string, std::string > &package_map, const std::string &root_dir=".", const DrakeJoint::FloatingBaseType floating_base_type=DrakeJoint::ROLLPITCHYAW, std::shared_ptr< RigidBodyFrame > weld_to_frame=nullptr)

void	addRobotFromURDF (const std::string &urdf_filename, const DrakeJoint::FloatingBaseType floating_base_type=DrakeJoint::ROLLPITCHYAW, std::shared_ptr< RigidBodyFrame > weld_to_frame=nullptr)

void	addRobotFromURDF (const std::string &urdf_filename, std::map< std::string, std::string > &package_map, const DrakeJoint::FloatingBaseType floating_base_type=DrakeJoint::ROLLPITCHYAW, std::shared_ptr< RigidBodyFrame > weld_to_frame=nullptr)

void	addRobotFromSDF (const std::string &sdf_filename, const DrakeJoint::FloatingBaseType floating_base_type=DrakeJoint::QUATERNION, std::shared_ptr< RigidBodyFrame > weld_to_frame=nullptr)

int	get_next_model_id ()
	Returns an integer that can be used to uniquely identify a model within this rigid body tree. More...

int	get_current_model_id ()
	Returns an integer that will be used as a unique ID for the next model to be added within the rigid body tree. More...

void	addFrame (std::shared_ptr< RigidBodyFrame > frame)

std::map< std::string, int >	computePositionNameToIndexMap () const

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

bool	transformCollisionFrame (const DrakeCollision::ElementId &eid, const Eigen::Isometry3d &transform_body_to_joint)

void	compile (void)

Eigen::VectorXd	getZeroConfiguration () const

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

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 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	isBodyPartOfRobot (const RigidBody &body, const std::set< int > &robotnum) const

double	getMass (const std::set< int > &robotnum=default_robot_num_set) const

template<typename Scalar >
Eigen::Matrix< Scalar, drake::kSpaceDimension, 1 >	centerOfMass (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum=default_robot_num_set) const

template<typename Scalar >
drake::TwistMatrix< Scalar >	worldMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum=default_robot_num_set, bool in_terms_of_qdot=false) const

template<typename Scalar >
drake::TwistVector< Scalar >	worldMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum=default_robot_num_set) const

template<typename Scalar >
drake::TwistMatrix< Scalar >	centroidalMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum=default_robot_num_set, bool in_terms_of_qdot=false) const

template<typename Scalar >
drake::TwistVector< Scalar >	centroidalMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum=default_robot_num_set) const

template<typename Scalar >
Eigen::Matrix< Scalar, drake::kSpaceDimension, Eigen::Dynamic >	centerOfMassJacobian (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum=default_robot_num_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 > &robotnum=default_robot_num_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...

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 eigen_aligned_unordered_map< RigidBody const *, drake::TwistVector< Scalar >> &f_ext, 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 eigen_aligned_unordered_map< RigidBody const *, drake::TwistVector< Scalar >> &f_ext, 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
	computeContactJacobians More...

DrakeCollision::ElementId	addCollisionElement (const RigidBody::CollisionElement &element, RigidBody &body, const std::string &group_name)

template<class UnaryPredicate >
void	removeCollisionGroupsIf (UnaryPredicate test)

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

void	updateStaticCollisionElements ()

void	updateDynamicCollisionElements (const KinematicsCache< double > &kin_cache)

void	getTerrainContactPoints (const RigidBody &body, Eigen::Matrix3Xd &terrain_points) const

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)
	collisionDetectFromPoints 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 *	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...

RigidBody *	findLink (const std::string &link_name, const std::string &model_name="", int model_id=-1) const
	This is a deprecated version of `FindBody(...)`. More...

int	FindBodyIndex (const std::string &body_name, int model_id=-1) const
	Obtains the index of a rigid body within this rigid body tree. More...

int	findLinkId (const std::string &link_name, int model_id=-1) const
	This is a deprecated version of `FindBodyIndex(...)`. More...

RigidBody *	findJoint (const std::string &joint_name, int model_id=-1) const
	Obtains a pointer to the rigid body whose parent joint is named `joint_name` and is part of a model with ID `model_id`. More...

int	findJointId (const std::string &joint_name, int model_id=-1) const

std::shared_ptr< RigidBodyFrame >	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...

std::string	getBodyOrFrameName (int body_or_frame_id) const

const RigidBodyActuator &	GetActuator (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

void	add_rigid_body (std::unique_ptr< RigidBody > body)
	Adds and takes ownership of a rigid body. More...

int	AddFloatingJoint (DrakeJoint::FloatingBaseType floating_base_type, const std::vector< int > &link_indices, const std::shared_ptr< RigidBodyFrame > weld_to_frame=nullptr, const PoseMap *pose_map=nullptr)
	Adds one floating joint to each link specified in the list of link indicies that does not already have a parent. More...

RigidBody &	world ()
	Returns a mutable reference to the RigidBody associated with the world in the model. More...

const RigidBody &	world () const
	Returns a const reference to the RigidBody associated with the world in the model. More...

int	number_of_positions () const
	An accessor to the number of position states outputted by this rigid body system. More...

int	number_of_velocities () const
	An accessor to the number of velocity states outputted by this rigid body system. More...

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

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

template<typename Scalar >
TwistMatrix< Scalar >	centroidalMomentumMatrix (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum, bool in_terms_of_qdot) const

template<typename Scalar >
TwistVector< Scalar >	centroidalMomentumMatrixDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum) const

template<typename Scalar >
Eigen::Matrix< Scalar, SPACE_DIMENSION, 1 >	centerOfMass (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum) const

template<typename Scalar >
Matrix< Scalar, SPACE_DIMENSION, Eigen::Dynamic >	centerOfMassJacobian (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum, bool in_terms_of_qdot) const

template<typename Scalar >
Matrix< Scalar, SPACE_DIMENSION, 1 >	centerOfMassJacobianDotTimesV (KinematicsCache< Scalar > &cache, const std::set< int > &robotnum) 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 eigen_aligned_unordered_map< RigidBody const *, TwistVector< Scalar >> &f_ext, bool include_velocity_terms) const

template<typename Scalar >
Matrix< Scalar, Eigen::Dynamic, 1 >	inverseDynamics (KinematicsCache< Scalar > &cache, const eigen_aligned_unordered_map< RigidBody const *, TwistVector< Scalar >> &f_ext, 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, QUAT_SIZE, 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, RPY_SIZE, 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

Public Attributes
Eigen::VectorXd	joint_limit_min

Eigen::VectorXd	joint_limit_max

std::vector< std::unique_ptr< RigidBody > >	bodies

std::vector< std::shared_ptr< RigidBodyFrame > >	frames

std::vector< RigidBodyActuator, Eigen::aligned_allocator< RigidBodyActuator > >	actuators

std::vector< RigidBodyLoop, Eigen::aligned_allocator< RigidBodyLoop > >	loops

drake::TwistVector< double >	a_grav

Eigen::MatrixXd	B

Static Public Attributes
static const char *const	kWorldLinkName = "world"
	Defines the name of the rigid body within a rigid body tree that represents the world. More...

static const std::set< int >	default_robot_num_set = {0}

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

Constructor & Destructor Documentation

RigidBodyTree	(	const std::string &	urdf_filename,
		const DrakeJoint::FloatingBaseType	floating_base_type = `DrakeJoint::ROLLPITCHYAW`
	)

Here is the call graph for this function:

RigidBodyTree ( void )

~RigidBodyTree ( void )

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

void add_rigid_body ( std::unique_ptr< RigidBody > 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] body The rigid body to add to this rigid body tree.

DrakeCollision::ElementId addCollisionElement	(	const RigidBody::CollisionElement &	element,
		RigidBody &	body,
		const std::string &	group_name
	)

Here is the caller graph for this function:

int AddFloatingJoint	(	DrakeJoint::FloatingBaseType	floating_base_type,
		const std::vector< int > &	link_indices,
		const std::shared_ptr< RigidBodyFrame >	weld_to_frame = `nullptr`,
		const PoseMap *	pose_map = `nullptr`
	)

Adds one floating joint to each link specified in the list of link indicies that does not already have a parent.

Typically, the list of link indices is created while calling add_rigid_body(). The purpose of the floating joint is to connect the links and of their child branches to the rigid body tree.

Parameters

floating_base_type	The floating joint's type.
link_indices	A list of link indexes to check. A floating joint is added to any link in this list that does not have a parent joint.
weld_to_frame	The frame to which the floating joint should attach the parent-less non-world links. This parameter may be nullptr, in which case the link is welded to the world with zero offset.
pose_map	A mapping where the key is the link's name and the value is the transform from the frame of the link to the frame of the model to which the link belongs.

Returns: The number of floating joint added to this rigid body tree.

Exceptions

A	std::runtime_error if the floating_base_type is unrecognized or zero floating joints were added to the model.

void addFrame ( std::shared_ptr< RigidBodyFrame > frame )

void addRobotFromSDF	(	const std::string &	sdf_filename,
		const DrakeJoint::FloatingBaseType	floating_base_type = `DrakeJoint::QUATERNION`,
		std::shared_ptr< RigidBodyFrame >	weld_to_frame = `nullptr`
	)

Here is the call graph for this function:

void addRobotFromURDF	(	const std::string &	urdf_filename,
		const DrakeJoint::FloatingBaseType	floating_base_type = `DrakeJoint::ROLLPITCHYAW`,
		std::shared_ptr< RigidBodyFrame >	weld_to_frame = `nullptr`
	)

Here is the call graph for this function:

void addRobotFromURDF	(	const std::string &	urdf_filename,
		std::map< std::string, std::string > &	package_map,
		const DrakeJoint::FloatingBaseType	floating_base_type = `DrakeJoint::ROLLPITCHYAW`,
		std::shared_ptr< RigidBodyFrame >	weld_to_frame = `nullptr`
	)

Here is the call graph for this function:

void addRobotFromURDFString	(	const std::string &	xml_string,
		const std::string &	root_dir = `"."`,
		const DrakeJoint::FloatingBaseType	floating_base_type = `DrakeJoint::ROLLPITCHYAW`,
		std::shared_ptr< RigidBodyFrame >	weld_to_frame = `nullptr`
	)

Here is the call graph for this function:

void addRobotFromURDFString	(	const std::string &	xml_string,
		std::map< std::string, std::string > &	package_map,
		const std::string &	root_dir = `"."`,
		const DrakeJoint::FloatingBaseType	floating_base_type = `DrakeJoint::ROLLPITCHYAW`,
		std::shared_ptr< RigidBodyFrame >	weld_to_frame = `nullptr`
	)

Here is the call graph for this function:

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:

Eigen::Matrix<Scalar, drake::kSpaceDimension, 1> centerOfMass	(	KinematicsCache< Scalar > &	cache,
		const std::set< int > &	robotnum = `default_robot_num_set`
	)		const

Here is the caller graph for this function:

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

Here is the call graph for this function:

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

Here is the caller graph for this function:

Matrix<Scalar, SPACE_DIMENSION, Eigen::Dynamic> centerOfMassJacobian	(	KinematicsCache< Scalar > &	cache,
		const std::set< int > &	robotnum,
		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 > &	robotnum = `default_robot_num_set`
	)		const

Matrix<Scalar, SPACE_DIMENSION, 1> centerOfMassJacobianDotTimesV	(	KinematicsCache< Scalar > &	cache,
		const std::set< int > &	robotnum
	)		const

Here is the call graph for this function:

drake::TwistMatrix<Scalar> centroidalMomentumMatrix	(	KinematicsCache< Scalar > &	cache,
		const std::set< int > &	robotnum = `default_robot_num_set`,
		bool	in_terms_of_qdot = `false`
	)		const

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

Here is the call graph for this function:

drake::TwistVector<Scalar> centroidalMomentumMatrixDotTimesV	(	KinematicsCache< Scalar > &	cache,
		const std::set< int > &	robotnum = `default_robot_num_set`
	)		const

Here is the caller graph for this function:

TwistVector<Scalar> centroidalMomentumMatrixDotTimesV	(	KinematicsCache< Scalar > &	cache,
		const std::set< int > &	robotnum
	)		const

Here is the call 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 call graph for this function:

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 call graph for this function:

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
	)

collisionDetectFromPoints

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

Here is the call graph for this function:

Here is the caller graph for this function:

void compile ( void )

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

computeContactJacobians

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

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

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

This method is explicitly instantiated in RigidBodyTree.cpp 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 RigidBodyTree.cpp 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 RigidBodyTree.cpp for a small set of supported Scalar types.

Here is the call graph for this function:

void drawKinematicTree ( std::string graphviz_dotfile_filename ) const

Eigen::Matrix<Scalar, Eigen::Dynamic, 1> dynamicsBiasTerm	(	KinematicsCache< Scalar > &	cache,
		const eigen_aligned_unordered_map< RigidBody const *, drake::TwistVector< Scalar >> &	f_ext,
		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 eigen_aligned_unordered_map< RigidBody const *, TwistVector< Scalar >> &	f_ext,
		bool	include_velocity_terms
	)		const

Here is the call graph for this function:

void findAncestorBodies	(	std::vector< int > &	ancestor_bodies,
		int	body
	)		const

Here is the call graph for this function:

Here is the caller graph for this function:

RigidBody * 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_name	The name of the body to find.
[in]	model_name	The name of the model to which the body belongs. If this value is an empty string, every model is searched.
[in]	model_id	The ID of the model to which the body belongs. If this value is -1, every model is searched.

Exceptions

std::logic_error if 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:

int FindBodyIndex	(	const std::string &	body_name,
		int	model_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_name	The body whose index we want to find. It should be unique within the searched models, otherwise an exception will be thrown.
[in]	model_id	The ID of the model. This parameter is optional. If supplied, only the model with the specified ID is searched; otherwise, all models are searched.

Returns: The index of the specified rigid body.

Exceptions

std::logic_error if 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:

shared_ptr< RigidBodyFrame > 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_name	The name of the frame to find.
[in]	model_id	The ID of the model to which the frame belongs. If this value is -1, search all models.

Exceptions

std::logic_error if multiple matching frames are found.

Here is the call graph for this function:

Here is the caller graph for this function:

RigidBody * findJoint	(	const std::string &	joint_name,
		int	model_id = `-1`
	)		const

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

Parameters

[in]	joint_name	The name of the joint to find.
[in]	model_id	The ID of the model that contains the joint. This parameter is optional. If supplied, only that model is searched; otherwise, all models are searched.

Returns: A pointer to the rigid body whose joint is the one being searched for.

Exceptions

std::logic_error if no joint is found with the given name within the searched model(s).

Here is the call graph for this function:

Here is the caller graph for this function:

int findJointId	(	const std::string &	joint_name,
		int	model_id = `-1`
	)		const

Here is the call graph for this function:

Here is the caller graph for this function:

KinematicPath findKinematicPath	(	int	start_body_or_frame_idx,
		int	end_body_or_frame_idx
	)		const

Here is the call graph for this function:

Here is the caller graph for this function:

RigidBody * 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.

Here is the call graph for this function:

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

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

Please use FindBodyIndex(...) instead.

Here is the call graph for this function:

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

Here is the caller graph for this function:

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

Here is the caller graph for this function:

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:

int get_current_model_id ( )

inline

Returns an integer that will be used as a unique ID for the next model to be added within the rigid body tree.

Here is the call graph for this function:

int get_next_model_id ( )

inline

Returns an integer that can be used to uniquely identify a model within this rigid body tree.

Note that this method is not thread safe!

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

name	The name of the rigid body actuator to get.

Returns: A const reference to the rigid body actuator with name name.

Exceptions

std::invalid_argument if 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 > & robotnum = default_robot_num_set ) const

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

string getPositionName ( int position_num ) const

Here is the caller 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

Here is the call graph for this function:

void getTerrainContactPoints	(	const RigidBody &	body,
		Eigen::Matrix3Xd &	terrain_points
	)		const

Here is the caller graph for this function:

string getVelocityName ( int velocity_num ) const

Here is the caller graph for this function:

Eigen::VectorXd getZeroConfiguration ( ) const

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inverseDynamics	(	KinematicsCache< Scalar > &	cache,
		const eigen_aligned_unordered_map< RigidBody const *, drake::TwistVector< Scalar >> &	f_ext,
		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

cache	a KinematicsCache constructed given \( q \) and \( v \)
f_ext	external wrenches exerted upon bodies. Expressed in body frame.
vd	\( \dot{v} \)
include_velocity_terms	whether 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 eigen_aligned_unordered_map< RigidBody const *, TwistVector< Scalar >> &	f_ext,
		const Matrix< Scalar, Eigen::Dynamic, 1 > &	vd,
		bool	include_velocity_terms
	)		const

Here is the call graph for this function:

bool isBodyPartOfRobot	(	const RigidBody &	body,
		const std::set< int > &	robotnum
	)		const

Here is the caller 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:

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

cache a 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

inline

An accessor to the number of position states outputted by this rigid body system.

Here is the caller graph for this function:

int number_of_velocities ( ) const

inline

An accessor to the number of velocity states outputted by this rigid body system.

Here is the caller graph for this function:

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

Here is the call graph for this function:

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

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

Here is the call graph for this function:

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

Here is the call graph for this function:

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 call graph for this function:

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, QUAT_SIZE, 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:

template DRAKERBM_EXPORT VectorX< AutoDiffXd > relativeQuaternionJacobianDotTimesV< AutoDiffXd >	(	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 call graph for this function:

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, RPY_SIZE, 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:

template DRAKERBM_EXPORT VectorX< AutoDiffUpTo73d > relativeRollPitchYawJacobianDotTimesV< AutoDiffUpTo73d >	(	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

Here is the caller graph for this function:

template DRAKERBM_EXPORT pair< Vector3d, double > resolveCenterOfPressure< Vector3d, Vector3d >	(	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 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	(	const DrakeCollision::ElementId &	eid,
		const Eigen::Isometry3d &	transform_body_to_joint
	)

Updates the frame of collision elements to be equal to the joint's frame.

Parameters

eid	The ID of the collision element to update.
transform_body_to_joint	The transform from the model's body frame to the joint frame.

Returns: true if the collision element was successfully updated. False can be returned if a collision element with the specified eid cannot be found.

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:

template DRAKERBM_EXPORT MatrixXd transformPointsJacobian< double, Vector3d >	(	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 call graph for this function:

Here is the caller graph for this function:

template DRAKERBM_EXPORT VectorXd transformPointsJacobianDotTimesV< double, Vector3d >	(	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:

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

Here is the caller graph for this function:

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:

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

Here is the caller graph for this function:

void updateDynamicCollisionElements ( const KinematicsCache< double > & kin_cache )

Here is the call graph for this function:

Here is the caller graph for this function:

void updateStaticCollisionElements ( )

Here is the call graph for this function:

Here is the caller graph for this function:

RigidBody& 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& 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 > &	robotnum = `default_robot_num_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 > &	robotnum,
		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 > &	robotnum = `default_robot_num_set`
	)		const

Here is the caller graph for this function:

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

Here is the call graph for this function:

Friends And Related Function Documentation

DRAKERBM_EXPORT std::ostream& operator<<	(	std::ostream &	os,
		const RigidBodyTree &	tree
	)

friend

A toString method for this class.

Member Data Documentation

drake::TwistVector<double> a_grav

std::vector<RigidBodyActuator, Eigen::aligned_allocator<RigidBodyActuator> > actuators

Eigen::MatrixXd B

std::vector<std::unique_ptr<RigidBody> > bodies

const set< int > default_robot_num_set = {0}

static

std::vector<std::shared_ptr<RigidBodyFrame> > frames

Eigen::VectorXd joint_limit_max

Eigen::VectorXd joint_limit_min

const char *const kWorldLinkName = "world"

static

Defines the name of the rigid body within a rigid body tree that represents the world.

std::vector<RigidBodyLoop, Eigen::aligned_allocator<RigidBodyLoop> > loops

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

drake/systems/plants/RigidBodyTree.h
drake/systems/plants/RigidBodyTree.cpp
drake/systems/plants/RigidBodyTreeContact.cpp

Public Member Functions

Public Attributes

Static Public Attributes

Friends

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation