#include <tree.h>
Public Member Functions | |
void | addEfforts (const KDL::JntArray &) |
add to the commanded joint efforts of the tree's joints from a KDL::JntArray | |
template<class Vec > | |
void | addEfforts (const Vec &) |
add to the commanded joint efforts of the tree's joints from any type that implements size() and [] lookup | |
bool | allCalibrated () const |
returns true, if all the joints in the tree are calibrated | |
void | getEfforts (KDL::JntArray &) const |
get the measured joint efforts of the tree's joints as a KDL::JntArray | |
template<class Vec > | |
void | getEfforts (Vec &) const |
get the measured joint efforts of the tree's joints as any type with size() and [] lookup | |
JointState * | getJoint (unsigned int) const |
returns a pointer to the joint state of a joint in the list of the tree's actuated joints (index starts at 0) | |
void | getPositions (KDL::JntArray &) const |
get the position of the joints of the tree as a KDL::JntArray | |
template<class Vec > | |
void | getPositions (Vec &) const |
get the position of the joints of the tree as any type with size() and [] lookup | |
void | getVelocities (KDL::JntArrayVel &) const |
get the velocities of the joints of the tree as a KDL::JntArrayVel (fills in the positions, too) | |
template<class Vec > | |
void | getVelocities (Vec &) const |
get the velocities of the joints of the tree as any type with size() and [] lookup | |
bool | init (RobotState *robot_state) |
initializes the tree object The initializer's most important functionality is to create a vector of joints. This vector is ordered according to the joint's number given by KDL's tree class, which is used by the kdl_parser to create a KDL::Tree from the robot description. This structure is what a KDL tree solver expects. The vector of joints can later be used to read the joints' positions or to send efforts to them. | |
void | setEfforts (const KDL::JntArray &) |
set the commanded joint efforts of the tree's joints from a KDL::JntArray | |
template<class Vec > | |
void | setEfforts (const Vec &) |
set the commanded joint efforts of the tree's joints from any type that implements size() and [] lookup | |
int | size () const |
returns the number of actuated joints in the tree | |
void | toKdl (KDL::Tree &) const |
get a KDL::Tree object that respresents the tree | |
Tree () | |
~Tree () | |
Private Attributes | |
std::vector< JointState * > | joints_ |
a vector of pointers to joint states; includes only the ones that can be actuated (not fixed joints) | |
KDL::Tree | kdl_tree_ |
pr2_mechanism_model::Tree::Tree | ( | ) | [inline] |
pr2_mechanism_model::Tree::~Tree | ( | ) | [inline] |
void pr2_mechanism_model::Tree::addEfforts | ( | const KDL::JntArray & | efforts | ) | [inline] |
add to the commanded joint efforts of the tree's joints from a KDL::JntArray
void pr2_mechanism_model::Tree::addEfforts | ( | const Vec & | v | ) | [inline] |
bool pr2_mechanism_model::Tree::allCalibrated | ( | ) | const [inline] |
void pr2_mechanism_model::Tree::getEfforts | ( | KDL::JntArray & | efforts | ) | const [inline] |
get the measured joint efforts of the tree's joints as a KDL::JntArray
void pr2_mechanism_model::Tree::getEfforts | ( | Vec & | v | ) | const [inline] |
JointState * pr2_mechanism_model::Tree::getJoint | ( | unsigned int | actuated_joint_i | ) | const [inline] |
void pr2_mechanism_model::Tree::getPositions | ( | KDL::JntArray & | positions | ) | const [inline] |
get the position of the joints of the tree as a KDL::JntArray
void pr2_mechanism_model::Tree::getPositions | ( | Vec & | v | ) | const [inline] |
void pr2_mechanism_model::Tree::getVelocities | ( | KDL::JntArrayVel & | velocities | ) | const [inline] |
get the velocities of the joints of the tree as a KDL::JntArrayVel (fills in the positions, too)
void pr2_mechanism_model::Tree::getVelocities | ( | Vec & | v | ) | const [inline] |
bool pr2_mechanism_model::Tree::init | ( | RobotState * | robot_state | ) |
initializes the tree object The initializer's most important functionality is to create a vector of joints. This vector is ordered according to the joint's number given by KDL's tree class, which is used by the kdl_parser to create a KDL::Tree from the robot description. This structure is what a KDL tree solver expects. The vector of joints can later be used to read the joints' positions or to send efforts to them.
robot_state | the robot state object containing the robot model and the state of each joint in the robot |
void pr2_mechanism_model::Tree::setEfforts | ( | const KDL::JntArray & | efforts | ) | [inline] |
set the commanded joint efforts of the tree's joints from a KDL::JntArray
void pr2_mechanism_model::Tree::setEfforts | ( | const Vec & | v | ) | [inline] |
int pr2_mechanism_model::Tree::size | ( | ) | const [inline] |
void pr2_mechanism_model::Tree::toKdl | ( | KDL::Tree & | tree | ) | const [inline] |
std::vector<JointState*> pr2_mechanism_model::Tree::joints_ [private] |