Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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(_q_min), q_max(_q_max), iksolver(_iksolver), fksolver(_fksolver), delta_q(_chain.getNrOfJoints()),
00031 maxiter(_maxiter),eps(_eps)
00032 {
00033
00034 }
00035
00036 int ChainIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
00037 {
00038 q_out = q_init;
00039
00040 unsigned int i;
00041 for(i=0;i<maxiter;i++){
00042 fksolver.JntToCart(q_out,f);
00043 delta_twist = diff(f,p_in);
00044
00045 if(Equal(delta_twist,Twist::Zero(),eps))
00046 break;
00047
00048 iksolver.CartToJnt(q_out,delta_twist,delta_q);
00049 Add(q_out,delta_q,q_out);
00050
00051 for(unsigned int j=0; j<q_min.rows(); j++) {
00052 if(q_out(j) < q_min(j))
00053 q_out(j) = q_min(j);
00054 }
00055
00056
00057 for(unsigned int j=0; j<q_max.rows(); j++) {
00058 if(q_out(j) > q_max(j))
00059 q_out(j) = q_max(j);
00060 }
00061 }
00062
00063 if(i!=maxiter)
00064 return 0;
00065 else
00066 return -3;
00067 }
00068
00069 ChainIkSolverPos_NR_JL::~ChainIkSolverPos_NR_JL()
00070 {
00071 }
00072
00073 }
00074