treeiksolverpos_nr_jl.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007-2008  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 // Copyright  (C)  2008  Mikael Mayer
00003 // Copyright  (C)  2008  Julia Jesse
00004 
00005 // Version: 1.0
00006 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00007 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00008 // URL: http://www.orocos.org/kdl
00009 
00010 // This library is free software; you can redistribute it and/or
00011 // modify it under the terms of the GNU Lesser General Public
00012 // License as published by the Free Software Foundation; either
00013 // version 2.1 of the License, or (at your option) any later version.
00014 
00015 // This library is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018 // Lesser General Public License for more details.
00019 
00020 // You should have received a copy of the GNU Lesser General Public
00021 // License along with this library; if not, write to the Free Software
00022 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00023 
00024 #include "treeiksolverpos_nr_jl.hpp"
00025 
00026 namespace KDL {
00027     TreeIkSolverPos_NR_JL::TreeIkSolverPos_NR_JL(const Tree& _tree,
00028                                                  const std::vector<std::string>& _endpoints,
00029                                                  const JntArray& _q_min, const JntArray& _q_max,
00030                                                  TreeFkSolverPos& _fksolver, TreeIkSolverVel& _iksolver,
00031                                                  unsigned int _maxiter, double _eps) :
00032         tree(_tree), q_min(_q_min), q_max(_q_max), iksolver(_iksolver),
00033         fksolver(_fksolver), delta_q(tree.getNrOfJoints()),
00034         endpoints(_endpoints), maxiter(_maxiter), eps(_eps)
00035     {
00036         for (size_t i = 0; i < endpoints.size(); i++) {
00037             frames.insert(Frames::value_type(endpoints[i], Frame::Identity()));
00038             delta_twists.insert(Twists::value_type(endpoints[i], Twist::Zero()));
00039         }
00040     }
00041     
00042     double TreeIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frames& p_in, JntArray& q_out) {
00043         q_out = q_init;
00044         
00045         //First check if all elements in p_in are available:
00046         for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
00047             if(frames.find(f_des_it->first)==frames.end())
00048                 return -2;
00049         
00050         unsigned int k=0;
00051         while(++k <= maxiter) {
00052             for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it){
00053                 //Get all iterators for this endpoint
00054                 Frames::iterator f_it = frames.find(f_des_it->first);
00055                 Twists::iterator delta_twist = delta_twists.find(f_des_it->first);
00056                 
00057                 fksolver.JntToCart(q_out, f_it->second, f_it->first);
00058                 delta_twist->second = diff(f_it->second, f_des_it->second);
00059             }
00060             double res = iksolver.CartToJnt(q_out, delta_twists, delta_q);
00061             if (res < eps) return res;
00062             
00063             Add(q_out, delta_q, q_out);
00064             
00065             for (unsigned int j = 0; j < q_min.rows(); j++) {
00066                 if (q_out(j) < q_min(j))
00067                     q_out( j) = q_min(j);
00068                 else if (q_out(j) > q_max(j))
00069                     q_out( j) = q_max(j);
00070             }
00071         }
00072         if (k <= maxiter)
00073             return 0;
00074         else
00075             return -3;
00076     }
00077     
00078     
00079     TreeIkSolverPos_NR_JL::~TreeIkSolverPos_NR_JL() {
00080         
00081     }
00082     
00083 }//namespace
00084 


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:17