Go to the documentation of this file.
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>
108 void toKdl(KDL::Tree&)
const;
124 assert(positions.rows() ==
joints_.size());
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)
139 assert(velocities.q.rows() ==
joints_.size());
140 assert(velocities.qdot.rows() ==
joints_.size());
141 for (
unsigned int i = 0; i <
joints_.size(); ++i)
143 velocities.q(i) =
joints_[i]->position_;
144 velocities.qdot(i) =
joints_[i]->velocity_;
151 assert((
int)v.size() == (
int)
joints_.size());
152 for (
size_t i = 0; i <
joints_.size(); ++i)
158 assert(efforts.rows() ==
joints_.size());
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_;
173 assert(efforts.rows() ==
joints_.size());
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];
188 assert(efforts.rows() ==
joints_.size());
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 getEfforts(KDL::JntArray &) const
get the measured joint efforts of the tree's joints as a KDL::JntArray
void setEfforts(const KDL::JntArray &)
set 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 getPositions(KDL::JntArray &) const
get the position of the joints of the tree as a KDL::JntArray
void toKdl(KDL::Tree &) const
get a KDL::Tree object that respresents the tree
This class provides the controllers with an interface to the robot state.
std::vector< JointState * > joints_
a vector of pointers to joint states; includes only the ones that can be actuated (not fixed joints)
bool allCalibrated() const
returns true, if all the joints in the tree are calibrated
int size() const
returns the number of actuated joints in 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 getVelocities(KDL::JntArrayVel &) const
get the velocities of the joints of the tree as a KDL::JntArrayVel (fills in the positions,...
void addEfforts(const KDL::JntArray &)
add to the commanded joint efforts of the tree's joints from a KDL::JntArray
pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Mar 6 2023 03:49:17