$search
00001 /* 00002 * Copyright (c) 2011, PAL Robotics S.L. 00003 * All rights reserved. 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Author: Marcus Liebhardt 00009 * 00010 * This class has been derived from the chain class in the 00011 * pr2_mechanism_model package in the pr2_mechanism stack for ROS 00012 * written by Stuart Glaser and Wim Meeussen. 00013 * 00014 * Redistribution and use in source and binary forms, with or without 00015 * modification, are permitted provided that the following conditions are met: 00016 * 00017 * * Redistributions of source code must retain the above copyright 00018 * notice, this list of conditions and the following disclaimer. 00019 * * Redistributions in binary form must reproduce the above copyright 00020 * notice, this list of conditions and the following disclaimer in the 00021 * documentation and/or other materials provided with the distribution. 00022 * * Neither the name of the Willow Garage, Inc. nor the names of its 00023 * contributors may be used to endorse or promote products derived from 00024 * this software without specific prior written permission. 00025 * 00026 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00027 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00028 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00029 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00030 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00031 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00032 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00033 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00034 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00035 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00036 * POSSIBILITY OF SUCH DAMAGE. 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 } // namespace 00230 00231 #endif /* PR2_MECHANISM_MODEL_TREE_H */