treeiksolverpos_online.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2011  PAL Robotics S.L.  All rights reserved.
00002 // Copyright  (C)  2007-2008  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00003 // Copyright  (C)  2008  Mikael Mayer
00004 // Copyright  (C)  2008  Julia Jesse
00005 
00006 // Version: 1.0
00007 // Author: Marcus Liebhardt
00008 // This class has been derived from the KDL::TreeIkSolverPos_NR_JL class
00009 // by Julia Jesse, Mikael Mayer and Ruben Smits
00010 
00011 // This library is free software; you can redistribute it and/or
00012 // modify it under the terms of the GNU Lesser General Public
00013 // License as published by the Free Software Foundation; either
00014 // version 2.1 of the License, or (at your option) any later version.
00015 
00016 // This library is distributed in the hope that it will be useful,
00017 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00018 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00019 // Lesser General Public License for more details.
00020 
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License along with this library; if not, write to the Free Software
00023 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00024 
00025 #include "treeiksolverpos_online.hpp"
00026 
00027 namespace KDL {
00028 
00029 TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts,
00030                                                const std::vector<std::string>& endpoints,
00031                                                const JntArray& q_min,
00032                                                const JntArray& q_max,
00033                                                const JntArray& q_dot_max,
00034                                                const double x_dot_trans_max,
00035                                                const double x_dot_rot_max,
00036                                                TreeFkSolverPos& fksolver,
00037                                                TreeIkSolverVel& iksolver) :
00038                                                q_min_(nr_of_jnts),
00039                                                q_max_(nr_of_jnts),
00040                                                q_dot_max_(nr_of_jnts),
00041                                                fksolver_(fksolver),
00042                                                iksolver_(iksolver),
00043                                                q_dot_(nr_of_jnts)
00044 {
00045     q_min_ = q_min;
00046     q_max_ = q_max;
00047     q_dot_max_ = q_dot_max;
00048     x_dot_trans_max_ = x_dot_trans_max;
00049     x_dot_rot_max_ = x_dot_rot_max;
00050 
00051     for (size_t i = 0; i < endpoints.size(); i++)
00052     {
00053       frames_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
00054       delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
00055     }
00056 }
00057 
00058 
00059 TreeIkSolverPos_Online::~TreeIkSolverPos_Online()
00060 {}
00061 
00062 
00063 double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
00064 {
00065   q_out = q_in;
00066 
00067   // First check, if all elements in p_in are available
00068   for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
00069     if(frames_.find(f_des_it->first)==frames_.end())
00070       return -2;
00071 
00072   for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
00073   {
00074     // Get all iterators for this endpoint
00075     Frames::iterator f_it = frames_.find(f_des_it->first);
00076     Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
00077 
00078     fksolver_.JntToCart(q_out, f_it->second, f_it->first);
00079     twist_ = diff(f_it->second, f_des_it->second);
00080 
00081     // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
00082     // And scales them, if necessary
00083     enforceCartVelLimits();
00084 
00085     delta_twists_it->second = twist_;
00086   }
00087 
00088   double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
00089 
00090   if(res<0)
00091       return res;
00092   //If we got here q_out is definitely of the right size
00093   if(q_out.rows()!=q_min_.rows() || q_out.rows()!=q_max_.rows() || q_out.rows()!= q_dot_max_.rows())
00094       return -1;
00095 
00096   // Checks, if joint velocities (q_dot_) exceed their maximum and scales them, if necessary
00097   enforceJointVelLimits();
00098 
00099   // Integrate
00100   Add(q_out, q_dot_, q_out);
00101 
00102   // Limit joint positions
00103   for (unsigned int j = 0; j < q_min_.rows(); j++)
00104   {
00105     if (q_out(j) < q_min_(j))
00106       q_out(j) = q_min_(j);
00107     else if (q_out(j) > q_max_(j))
00108       q_out(j) = q_max_(j);
00109   }
00110 
00111   return res;
00112 }
00113 
00114 
00115 void TreeIkSolverPos_Online::enforceJointVelLimits()
00116 {
00117   // check, if one (or more) joint velocities exceed the maximum value
00118   // and if so, safe the biggest overshoot for scaling q_dot_ properly
00119   // to keep the direction of the velocity vector the same
00120   double rel_os, rel_os_max = 0.0; // relative overshoot and the biggest relative overshoot
00121   bool max_exceeded = false;
00122 
00123   for (unsigned int i = 0; i < q_dot_.rows(); i++)
00124   {
00125     if ( q_dot_(i) > q_dot_max_(i) )
00126     {
00127       max_exceeded = true;
00128       rel_os = (q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
00129       if ( rel_os > rel_os_max )
00130       {
00131         rel_os_max = rel_os;
00132       }
00133     }
00134     else if ( q_dot_(i) < -q_dot_max_(i) )
00135     {
00136       max_exceeded = true;
00137       rel_os = (-q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
00138       if ( rel_os > rel_os_max)
00139       {
00140         rel_os_max = rel_os;
00141       }
00142     }
00143   }
00144 
00145   // scales q_out, if one joint exceeds the maximum value
00146   if ( max_exceeded == true )
00147   {
00148     Multiply(q_dot_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_dot_);
00149   }
00150 }
00151 
00152 
00153 void TreeIkSolverPos_Online::enforceCartVelLimits()
00154 {
00155   double x_dot_trans, x_dot_rot; // vector lengths
00156   x_dot_trans = sqrt( pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2));
00157   x_dot_rot = sqrt( pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2));
00158 
00159   if ( x_dot_trans > x_dot_trans_max_ || x_dot_rot > x_dot_rot_max_ )
00160   {
00161     if ( x_dot_trans > x_dot_rot )
00162     {
00163       twist_.vel = twist_.vel * ( x_dot_trans_max_ / x_dot_trans );
00164       twist_.rot = twist_.rot * ( x_dot_trans_max_ / x_dot_trans );
00165     }
00166     else if ( x_dot_rot > x_dot_trans )
00167     {
00168       twist_.vel = twist_.vel * ( x_dot_rot_max_ / x_dot_rot );
00169       twist_.rot = twist_.rot * ( x_dot_rot_max_ / x_dot_rot );
00170     }
00171   }
00172 }
00173 
00174 } // namespace
00175 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14