Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef PR2_MECHANISM_MODEL_TREE_H
00040 #define PR2_MECHANISM_MODEL_TREE_H
00041
00042 #include <ros/ros.h>
00043 #include <kdl/chain.hpp>
00044 #include <kdl/tree.hpp>
00045 #include <kdl/jntarray.hpp>
00046 #include <kdl/jntarrayvel.hpp>
00047 #include "pr2_mechanism_model/robot.h"
00048
00049 namespace pr2_mechanism_model
00050 {
00051
00052 class Tree
00053 {
00054 public:
00055 Tree() : kdl_tree_() {};
00056 ~Tree() {};
00057
00067 bool init(RobotState *robot_state);
00068
00070 void getPositions(KDL::JntArray&) const;
00071
00073 template <class Vec>
00074 void getPositions(Vec&) const;
00075
00077 void getVelocities(KDL::JntArrayVel&) const;
00078
00080 template <class Vec>
00081 void getVelocities(Vec&) const;
00082
00084 void getEfforts(KDL::JntArray&) const;
00085
00087 template <class Vec>
00088 void getEfforts(Vec&) const;
00089
00091 void setEfforts(const KDL::JntArray&);
00092
00094 template <class Vec>
00095 void setEfforts(const Vec&);
00096
00098 void addEfforts(const KDL::JntArray&);
00099
00101 template <class Vec>
00102 void addEfforts(const Vec&);
00103
00105 bool allCalibrated() const;
00106
00108 void toKdl(KDL::Tree&) const;
00109
00111 JointState* getJoint(unsigned int) const;
00112
00114 int size() const;
00115
00116 private:
00117 KDL::Tree kdl_tree_;
00119 std::vector<JointState*> joints_;
00120 };
00121
00122 inline void Tree::getPositions(KDL::JntArray& positions) const
00123 {
00124 assert(positions.rows() == joints_.size());
00125 for (unsigned int i = 0; i < joints_.size(); ++i)
00126 positions(i) = joints_[i]->position_;
00127 }
00128
00129 template <class Vec>
00130 inline void Tree::getPositions(Vec &v) const
00131 {
00132 assert((int)v.size() == (int)joints_.size());
00133 for (size_t i = 0; i < joints_.size(); ++i)
00134 v[i] = joints_[i]->position_;
00135 }
00136
00137 inline void Tree::getVelocities(KDL::JntArrayVel& velocities) const
00138 {
00139 assert(velocities.q.rows() == joints_.size());
00140 assert(velocities.qdot.rows() == joints_.size());
00141 for (unsigned int i = 0; i < joints_.size(); ++i)
00142 {
00143 velocities.q(i) = joints_[i]->position_;
00144 velocities.qdot(i) = joints_[i]->velocity_;
00145 }
00146 }
00147
00148 template <class Vec>
00149 inline void Tree::getVelocities(Vec &v) const
00150 {
00151 assert((int)v.size() == (int)joints_.size());
00152 for (size_t i = 0; i < joints_.size(); ++i)
00153 v[i] = joints_[i]->velocity_;
00154 }
00155
00156 inline void Tree::getEfforts(KDL::JntArray& efforts) const
00157 {
00158 assert(efforts.rows() == joints_.size());
00159 for (unsigned int i = 0; i < joints_.size(); ++i)
00160 efforts(i) = joints_[i]->measured_effort_;
00161 }
00162
00163 template <class Vec>
00164 inline void Tree::getEfforts(Vec &v) const
00165 {
00166 assert((int)v.size() == (int)joints_.size());
00167 for (size_t i = 0; i < joints_.size(); ++i)
00168 v[i] = joints_[i]->measured_effort_;
00169 }
00170
00171 inline void Tree::setEfforts(const KDL::JntArray& efforts)
00172 {
00173 assert(efforts.rows() == joints_.size());
00174 for (unsigned int i = 0; i < joints_.size(); ++i)
00175 joints_[i]->commanded_effort_ = efforts(i);
00176 }
00177
00178 template <class Vec>
00179 inline void Tree::setEfforts(const Vec& v)
00180 {
00181 assert((int)v.size() == (int)joints_.size());
00182 for (size_t i = 0; i < joints_.size(); ++i)
00183 joints_[i]->commanded_effort_ = v[i];
00184 }
00185
00186 inline void Tree::addEfforts(const KDL::JntArray& efforts)
00187 {
00188 assert(efforts.rows() == joints_.size());
00189 for (unsigned int i = 0; i < joints_.size(); ++i)
00190 joints_[i]->commanded_effort_ += efforts(i);
00191 }
00192
00193 template <class Vec>
00194 inline void Tree::addEfforts(const Vec& v)
00195 {
00196 assert((int)v.size() == (int)joints_.size());
00197 for (size_t i = 0; i < joints_.size(); ++i)
00198 joints_[i]->commanded_effort_ += v[i];
00199 }
00200
00201 inline bool Tree::allCalibrated() const
00202 {
00203 for (unsigned int i = 0; i < joints_.size(); ++i)
00204 {
00205 if (!joints_[i]->calibrated_)
00206 return false;
00207 }
00208 return true;
00209 }
00210
00211 inline void Tree::toKdl(KDL::Tree &tree) const
00212 {
00213 tree = kdl_tree_;
00214 }
00215
00216 inline JointState* Tree::getJoint(unsigned int actuated_joint_i) const
00217 {
00218 if (actuated_joint_i >= joints_.size())
00219 return NULL;
00220 else
00221 return joints_[actuated_joint_i];
00222 }
00223
00224 inline int Tree::size() const
00225 {
00226 return joints_.size();
00227 }
00228
00229 }
00230
00231 #endif