chain.hpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Stuart Glaser
00031 
00032 #ifndef ROS_ETHERCAT_MODEL_CHAIN_H
00033 #define ROS_ETHERCAT_MODEL_CHAIN_H
00034 
00035 #include "ros_ethercat_model/robot_state.hpp"
00036 #include <kdl/chain.hpp>
00037 #include <kdl/jntarray.hpp>
00038 #include <kdl/jntarrayvel.hpp>
00039 #include <kdl/jntarrayacc.hpp>
00040 #include <kdl/tree.hpp>
00041 #include <kdl_parser/kdl_parser.hpp>
00042 
00043 namespace ros_ethercat_model
00044 {
00045 
00046 class Chain
00047 {
00048 public:
00056   bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
00057   {
00058     robot_state_ = robot_state;
00059     // Constructs the kdl chain
00060     KDL::Tree kdl_tree;
00061     if (!kdl_parser::treeFromUrdfModel(robot_state->robot_model_, kdl_tree))
00062     {
00063       ROS_ERROR("Could not convert urdf into kdl tree");
00064       return false;
00065     }
00066     bool res;
00067     try
00068     {
00069       res = kdl_tree.getChain(root, tip, kdl_chain_);
00070     }
00071     catch (...)
00072     {
00073       res = false;
00074     }
00075     if (!res)
00076     {
00077       ROS_ERROR("Could not extract chain between %s and %s from kdl tree",
00078                 root.c_str(), tip.c_str());
00079       return false;
00080     }
00081     // Pulls out all the joint indices
00082     joints_.clear();
00083     for (size_t i = 0; i < kdl_chain_.getNrOfSegments(); i++)
00084     {
00085       if (kdl_chain_.getSegment(i).getJoint().getType() != KDL::Joint::None)
00086       {
00087         JointState* jnt = robot_state->getJointState(kdl_chain_.getSegment(i).getJoint().getName());
00088         if (!jnt)
00089         {
00090           ROS_ERROR("Joint '%s' is not found in joint state vector",
00091                     kdl_chain_.getSegment(i).getJoint().getName().c_str());
00092           return false;
00093         }
00094         joints_.push_back(jnt);
00095       }
00096     }
00097     ROS_DEBUG("Added %i joints", int(joints_.size()));
00098     return true;
00099   }
00100   void getPositions(std::vector<double> &positions)
00101   {
00102     positions.clear();
00103     for (unsigned int i = 0; i < joints_.size(); ++i)
00104       positions.push_back(joints_[i]->position_);
00105   }
00106   void getPositions(KDL::JntArray &a)
00107   {
00108     assert(a.rows() == joints_.size());
00109     for (unsigned int i = 0; i < joints_.size(); ++i)
00110       a(i) = joints_[i]->position_;
00111   }
00112 
00114   template <class Vec> void getPositions(Vec &v)
00115   {
00116     assert((int) v.size() == (int) joints_.size());
00117     for (size_t i = 0; i < joints_.size(); ++i)
00118       v[i] = joints_[i]->position_;
00119   }
00120 
00122   void getVelocities(std::vector<double> &velocities)
00123   {
00124     velocities.clear();
00125     for (unsigned int i = 0; i < joints_.size(); ++i)
00126       velocities.push_back(joints_[i]->velocity_);
00127   }
00129   void getVelocities(KDL::JntArrayVel &a)
00130   {
00131     assert(a.q.rows() == joints_.size());
00132     assert(a.qdot.rows() == joints_.size());
00133     for (unsigned int i = 0; i < joints_.size(); ++i)
00134     {
00135       a.q(i) = joints_[i]->position_;
00136       a.qdot(i) = joints_[i]->velocity_;
00137     }
00138   }
00139 
00141   template <class Vec> void getVelocities(Vec &v)
00142   {
00143     assert((int) v.size() == (int) joints_.size());
00144     for (size_t i = 0; i < joints_.size(); ++i)
00145       v[i] = joints_[i]->velocity_;
00146   }
00147   void getEfforts(std::vector<double> &efforts)
00148   {
00149     efforts.clear();
00150     for (unsigned int i = 0; i < joints_.size(); ++i)
00151       efforts.push_back(joints_[i]->measured_effort_);
00152   }
00153 
00155   void getEfforts(KDL::JntArray &a)
00156   {
00157     assert(a.rows() == joints_.size());
00158     for (unsigned int i = 0; i < joints_.size(); ++i)
00159       a(i) = joints_[i]->measured_effort_;
00160   }
00161 
00163   void setEfforts(KDL::JntArray &a)
00164   {
00165     assert(a.rows() == joints_.size());
00166     for (unsigned int i = 0; i < joints_.size(); ++i)
00167       joints_[i]->commanded_effort_ = a(i);
00168   }
00169 
00171   void addEfforts(KDL::JntArray &a)
00172   {
00173     assert(a.rows() == joints_.size());
00174     for (unsigned int i = 0; i < joints_.size(); ++i)
00175       joints_[i]->commanded_effort_ += a(i);
00176   }
00177 
00179   template <class Vec> void addEfforts(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 
00187   bool allCalibrated()
00188   {
00189     for (unsigned int i = 0; i < joints_.size(); ++i)
00190       if (!joints_[i]->calibrated_)
00191         return false;
00192     return true;
00193   }
00194 
00196   void toKDL(KDL::Chain &chain)
00197   {
00198     chain = kdl_chain_;
00199   }
00205   JointState* getJoint(unsigned int actuated_joint_i)
00206   {
00207     if (actuated_joint_i >= joints_.size())
00208       return NULL;
00209     else
00210       return joints_[actuated_joint_i];
00211   }
00212 
00214   int size() const
00215   {
00216     return joints_.size();
00217   }
00218 
00219 private:
00220   RobotState *robot_state_;
00221   KDL::Chain kdl_chain_;
00222 
00223   std::vector< JointState* > joints_; // ONLY joints that can be actuated (not fixed joints)
00224 };
00225 
00226 }
00227 
00228 #endif


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