$search
00001 // Copyright (C) 2007 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 "chainiksolverpos_nr_jl.hpp" 00025 00026 namespace KDL 00027 { 00028 ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, 00029 unsigned int _maxiter, double _eps): 00030 chain(_chain), q_min(chain.getNrOfJoints()), q_max(chain.getNrOfJoints()), fksolver(_fksolver),iksolver(_iksolver),delta_q(_chain.getNrOfJoints()), 00031 maxiter(_maxiter),eps(_eps) 00032 { 00033 q_min = _q_min; 00034 q_max = _q_max; 00035 } 00036 00037 int ChainIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out) 00038 { 00039 q_out = q_init; 00040 00041 unsigned int i; 00042 for(i=0;i<maxiter;i++){ 00043 fksolver.JntToCart(q_out,f); 00044 delta_twist = diff(f,p_in); 00045 00046 if(Equal(delta_twist,Twist::Zero(),eps)) 00047 break; 00048 00049 iksolver.CartToJnt(q_out,delta_twist,delta_q); 00050 Add(q_out,delta_q,q_out); 00051 00052 for(unsigned int j=0; j<q_min.rows(); j++) { 00053 if(q_out(j) < q_min(j)) 00054 q_out(j) = q_min(j); 00055 } 00056 00057 00058 for(unsigned int j=0; j<q_max.rows(); j++) { 00059 if(q_out(j) > q_max(j)) 00060 q_out(j) = q_max(j); 00061 } 00062 } 00063 00064 if(i!=maxiter) 00065 return 0; 00066 else 00067 return -3; 00068 } 00069 00070 ChainIkSolverPos_NR_JL::~ChainIkSolverPos_NR_JL() 00071 { 00072 } 00073 00074 } 00075