tree.h
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  * 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 */


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02