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 "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
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
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 }
00084