chain.cpp
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, Wim Meeussen
00031 
00032 #include "pr2_mechanism_model/chain.h"
00033 #include <kdl/tree.hpp>
00034 #include <kdl_parser/kdl_parser.hpp>
00035 
00036 namespace pr2_mechanism_model {
00037 
00038 using namespace std;
00039 
00040 bool Chain::init(RobotState *robot_state, const std::string &root, const std::string &tip)
00041 {
00042 
00043   robot_state_ = robot_state;
00044 
00045   // Constructs the kdl chain
00046   KDL::Tree kdl_tree;
00047   if (!kdl_parser::treeFromUrdfModel(robot_state->model_->robot_model_, kdl_tree)){
00048     ROS_ERROR("Could not convert urdf into kdl tree");
00049     return false;
00050   }
00051 
00052   bool res;
00053   try{
00054     res = kdl_tree.getChain(root, tip, kdl_chain_);
00055   }
00056   catch(...){
00057     res = false;
00058   }
00059   if (!res){
00060     ROS_ERROR("Could not extract chain between %s and %s from kdl tree",
00061               root.c_str(), tip.c_str());
00062     return false;
00063   }
00064 
00065 
00066   // Pulls out all the joint indices
00067   joints_.clear();
00068   for (size_t i=0; i<kdl_chain_.getNrOfSegments(); i++){
00069     if (kdl_chain_.getSegment(i).getJoint().getType() != KDL::Joint::None){ 
00070       JointState* jnt = robot_state->getJointState(kdl_chain_.getSegment(i).getJoint().getName());
00071       if (!jnt){
00072         ROS_ERROR("Joint '%s' is not found in joint state vector", kdl_chain_.getSegment(i).getJoint().getName().c_str());
00073         return false;
00074       }
00075       joints_.push_back(jnt);
00076     }
00077   }
00078   ROS_DEBUG("Added %i joints", int(joints_.size()));
00079 
00080   return true;
00081 }
00082 
00083 void Chain::getPositions(std::vector<double> &positions)
00084 {
00085   positions.resize(joints_.size());
00086   for (unsigned int i = 0; i < joints_.size(); ++i)
00087   {
00088     positions[i] = joints_[i]->position_;
00089   }
00090 }
00091 
00092 void Chain::getVelocities(std::vector<double> &velocities)
00093 {
00094   velocities.resize(joints_.size());
00095   for (unsigned int i = 0; i < joints_.size(); ++i)
00096   {
00097     velocities[i] = joints_[i]->velocity_;
00098   }
00099 }
00100 
00101 void Chain::getEfforts(std::vector<double> &efforts)
00102 {
00103   efforts.resize(joints_.size());
00104   for (unsigned int i = 0; i < joints_.size(); ++i)
00105   {
00106     efforts[i] = joints_[i]->measured_effort_;
00107   }
00108 }
00109 
00110 bool Chain::allCalibrated()
00111 {
00112   for (unsigned int i = 0; i < joints_.size(); ++i)
00113   {
00114     if (!joints_[i]->calibrated_)
00115       return false;
00116   }
00117   return true;
00118 }
00119 
00120 void Chain::toKDL(KDL::Chain &chain)
00121 {
00122   chain = kdl_chain_;
00123 }
00124 
00125 
00126 void Chain::getPositions(KDL::JntArray& a)
00127 {
00128   assert(a.rows() == joints_.size());
00129   for (unsigned int i = 0; i < joints_.size(); ++i)
00130     a(i) = joints_[i]->position_;
00131 }
00132 
00133 void Chain::getVelocities(KDL::JntArrayVel& a)
00134 {
00135   assert(a.q.rows() == joints_.size());
00136   assert(a.qdot.rows() == joints_.size());
00137   for (unsigned int i = 0; i < joints_.size(); ++i){
00138     a.q(i) = joints_[i]->position_;
00139     a.qdot(i) = joints_[i]->velocity_;
00140   }
00141 }
00142 
00143 void Chain::getEfforts(KDL::JntArray& a)
00144 {
00145   assert(a.rows() == joints_.size());
00146   for (unsigned int i = 0; i < joints_.size(); ++i)
00147     a(i) = joints_[i]->measured_effort_;
00148 }
00149 
00150 void Chain::setEfforts(KDL::JntArray& a)
00151 {
00152   assert(a.rows() == joints_.size());
00153   for (unsigned int i = 0; i < joints_.size(); ++i)
00154     joints_[i]->commanded_effort_ = a(i);
00155 }
00156 
00157 void Chain::addEfforts(KDL::JntArray& a)
00158 {
00159   assert(a.rows() == joints_.size());
00160   for (unsigned int i = 0; i < joints_.size(); ++i)
00161     joints_[i]->commanded_effort_ += a(i);
00162 }
00163 
00164 
00165 JointState *Chain::getJoint(unsigned int actuated_joint_i)
00166 {
00167   if (actuated_joint_i >= joints_.size())
00168     return NULL;
00169   else
00170     return joints_[actuated_joint_i];
00171 }
00172 
00173 
00174 
00175 }


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