39 #ifndef PR2_MECHANISM_MODEL_TREE_H 40 #define PR2_MECHANISM_MODEL_TREE_H 43 #include <kdl/chain.hpp> 44 #include <kdl/tree.hpp> 45 #include <kdl/jntarray.hpp> 46 #include <kdl/jntarrayvel.hpp> 125 for (
unsigned int i = 0; i <
joints_.size(); ++i)
126 positions(i) =
joints_[i]->position_;
132 assert((
int)v.size() == (int)
joints_.size());
133 for (
size_t i = 0; i <
joints_.size(); ++i)
141 for (
unsigned int i = 0; i <
joints_.size(); ++i)
143 velocities.
q(i) =
joints_[i]->position_;
151 assert((
int)v.size() == (int)
joints_.size());
152 for (
size_t i = 0; i <
joints_.size(); ++i)
159 for (
unsigned int i = 0; i <
joints_.size(); ++i)
160 efforts(i) =
joints_[i]->measured_effort_;
166 assert((
int)v.size() == (int)
joints_.size());
167 for (
size_t i = 0; i <
joints_.size(); ++i)
168 v[i] =
joints_[i]->measured_effort_;
174 for (
unsigned int i = 0; i <
joints_.size(); ++i)
175 joints_[i]->commanded_effort_ = efforts(i);
181 assert((
int)v.size() == (int)
joints_.size());
182 for (
size_t i = 0; i <
joints_.size(); ++i)
183 joints_[i]->commanded_effort_ = v[i];
189 for (
unsigned int i = 0; i <
joints_.size(); ++i)
190 joints_[i]->commanded_effort_ += efforts(i);
196 assert((
int)v.size() == (int)
joints_.size());
197 for (
size_t i = 0; i <
joints_.size(); ++i)
198 joints_[i]->commanded_effort_ += v[i];
203 for (
unsigned int i = 0; i <
joints_.size(); ++i)
218 if (actuated_joint_i >=
joints_.size())
221 return joints_[actuated_joint_i];
void addEfforts(const KDL::JntArray &)
add to the commanded joint efforts of the tree's joints from a KDL::JntArray
bool init(RobotState *robot_state)
initializes the tree object The initializer's most important functionality is to create a vector of j...
void setEfforts(const KDL::JntArray &)
set the commanded joint efforts of the tree's joints from a KDL::JntArray
unsigned int rows() const
void getEfforts(KDL::JntArray &) const
get the measured joint efforts of the tree's joints as a KDL::JntArray
int size() const
returns the number of actuated joints in the tree
std::vector< JointState * > joints_
a vector of pointers to joint states; includes only the ones that can be actuated (not fixed joints) ...
This class provides the controllers with an interface to the robot state.
bool allCalibrated() const
returns true, if all the joints in the tree are calibrated
void getVelocities(KDL::JntArrayVel &) const
get the velocities of the joints of the tree as a KDL::JntArrayVel (fills in the positions, too)
void toKdl(KDL::Tree &) const
get a KDL::Tree object that respresents the tree
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 star...
void getPositions(KDL::JntArray &) const
get the position of the joints of the tree as a KDL::JntArray