tree.hpp
Go to the documentation of this file.
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  * ros_ethercat_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 ROS_ETHERCAT_MODEL_TREE_H
00040 #define ROS_ETHERCAT_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 "ros_ethercat_model/robot_state.hpp"
00048 #include <kdl_parser/kdl_parser.hpp>
00049 
00050 namespace ros_ethercat_model
00051 {
00052 
00053 class Tree
00054 {
00055 public:
00056   Tree() :
00057     kdl_tree_()
00058   {
00059   }
00069   bool init(RobotState *robot_state)
00070   {
00071     KDL::SegmentMap segmentMap;
00072 
00073     // construct the kdl tree
00074     if (!kdl_parser::treeFromUrdfModel(robot_state->robot_model_, kdl_tree_))
00075     {
00076       ROS_ERROR("Failed to construct KDL:Tree from robot_state's URDF model! Aborting ...");
00077     }
00078     else
00079     {
00080       ROS_INFO("KDL::Tree successful created.");
00081 
00082       segmentMap = kdl_tree_.getSegments();
00083     }
00084 
00085     // the first step of extracting the joints from the tree is to go through all tree_elements, check for a joint,
00086     // and check in case a joint is found, if it is not of not of type KDL::Joint::None
00087 
00088     // map for saving the temporary result of the joint extraction from the tree
00089     std::map<unsigned int, std::string> jointMap;
00090 
00091     ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
00092     for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end();
00093          ++seg_it)
00094     {
00095       if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
00096         jointMap[seg_it->second.q_nr] = seg_it->second.segment.getJoint().getName().c_str();
00097     }
00098 
00099     // in the second step the joints found get checked, if they appear in the JointState vector of the robot
00100     ROS_DEBUG("Checking, if extracted joints can be found in the JointState vector of the robot.");
00101     joints_.clear();
00102     for (std::map<unsigned int, std::string>::const_iterator jnt_it = jointMap.begin();
00103          jnt_it != jointMap.end(); ++jnt_it)
00104     {
00105       JointState* jnt = robot_state->getJointState(jnt_it->second.c_str());
00106       if (!jnt)
00107       {
00108         ROS_ERROR("Joint '%s' has not been found in the robot's joint state vector! Aborting ...",
00109                   jnt_it->second.c_str());
00110         return false;
00111       }
00112       joints_.push_back(jnt);
00113     }
00114 
00115     ROS_DEBUG("The result after joint extraction and checking:");
00116     for (unsigned int i = 0; i < joints_.size(); ++i)
00117     {
00118       ROS_DEBUG("joints_[%d]: joint_.name = %s", i, joints_[i]->joint_->name.c_str());
00119     }
00120 
00121     ROS_INFO("Added %i joints", int(joints_.size()));
00122 
00123     return true;
00124   }
00125 
00127   void getPositions(KDL::JntArray& positions) const
00128   {
00129     assert(positions.rows() == joints_.size());
00130     for (unsigned int i = 0; i < joints_.size(); ++i)
00131       positions(i) = joints_[i]->position_;
00132   }
00133 
00135   template<class Vec>
00136   void getPositions(Vec &v) const
00137   {
00138     assert((int) v.size() == (int) joints_.size());
00139     for (size_t i = 0; i < joints_.size(); ++i)
00140       v[i] = joints_[i]->position_;
00141   }
00142 
00144   void getVelocities(KDL::JntArrayVel &velocities) const
00145   {
00146     assert(velocities.q.rows() == joints_.size());
00147     assert(velocities.qdot.rows() == joints_.size());
00148     for (unsigned int i = 0; i < joints_.size(); ++i)
00149     {
00150       velocities.q(i) = joints_[i]->position_;
00151       velocities.qdot(i) = joints_[i]->velocity_;
00152     }
00153   }
00154 
00156   template<class Vec>
00157   void getVelocities(Vec &v) const
00158   {
00159     assert((int) v.size() == (int) joints_.size());
00160     for (size_t i = 0; i < joints_.size(); ++i)
00161       v[i] = joints_[i]->velocity_;
00162   }
00163 
00165   void getEfforts(KDL::JntArray &efforts) const
00166   {
00167     assert(efforts.rows() == joints_.size());
00168     for (unsigned int i = 0; i < joints_.size(); ++i)
00169       efforts(i) = joints_[i]->measured_effort_;
00170   }
00171 
00173   template<class Vec>
00174   void getEfforts(Vec &v) const
00175   {
00176     assert((int) v.size() == (int) joints_.size());
00177     for (size_t i = 0; i < joints_.size(); ++i)
00178       v[i] = joints_[i]->measured_effort_;
00179   }
00180 
00182   void setEfforts(const KDL::JntArray &efforts)
00183   {
00184     assert(efforts.rows() == joints_.size());
00185     for (unsigned int i = 0; i < joints_.size(); ++i)
00186       joints_[i]->commanded_effort_ = efforts(i);
00187   }
00188 
00190   template<class Vec>
00191   void setEfforts(const Vec &v)
00192   {
00193     assert((int) v.size() == (int) joints_.size());
00194     for (size_t i = 0; i < joints_.size(); ++i)
00195       joints_[i]->commanded_effort_ = v[i];
00196   }
00197 
00199   void addEfforts(const KDL::JntArray &efforts)
00200   {
00201     assert(efforts.rows() == joints_.size());
00202     for (unsigned int i = 0; i < joints_.size(); ++i)
00203       joints_[i]->commanded_effort_ += efforts(i);
00204   }
00205 
00207   template<class Vec>
00208   void addEfforts(const Vec &v)
00209   {
00210     assert((int) v.size() == (int) joints_.size());
00211     for (size_t i = 0; i < joints_.size(); ++i)
00212       joints_[i]->commanded_effort_ += v[i];
00213   }
00214 
00216   bool allCalibrated() const
00217   {
00218     for (unsigned int i = 0; i < joints_.size(); ++i)
00219       if (!joints_[i]->calibrated_)
00220         return false;
00221     return true;
00222   }
00223 
00225   void toKdl(KDL::Tree &tree) const
00226   {
00227     tree = kdl_tree_;
00228   }
00229 
00231   JointState* getJoint(unsigned int actuated_joint_i) const
00232   {
00233     if (actuated_joint_i >= joints_.size())
00234       return NULL;
00235     else
00236       return joints_[actuated_joint_i];
00237   }
00238 
00240   int size() const
00241   {
00242     return joints_.size();
00243   }
00244 
00245 private:
00246   KDL::Tree kdl_tree_;
00248   std::vector<JointState*> joints_;
00249 };
00250 
00251 } // namespace
00252 
00253 #endif /* ros_ethercat_model_TREE_H */


ros_ethercat_model
Author(s): Manos Nikolaidis
autogenerated on Thu Aug 27 2015 14:47:12