treeiksolverpos_nr_jl.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007-2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 // Copyright (C) 2008 Mikael Mayer
3 // Copyright (C) 2008 Julia Jesse
4 
5 // Version: 1.0
6 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
7 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
25 
26 namespace KDL {
28  const std::vector<std::string>& _endpoints,
29  const JntArray& _q_min, const JntArray& _q_max,
30  TreeFkSolverPos& _fksolver, TreeIkSolverVel& _iksolver,
31  unsigned int _maxiter, double _eps) :
32  tree(_tree), q_min(_q_min), q_max(_q_max), iksolver(_iksolver),
33  fksolver(_fksolver), delta_q(tree.getNrOfJoints()),
34  endpoints(_endpoints), maxiter(_maxiter), eps(_eps)
35  {
36  for (size_t i = 0; i < endpoints.size(); i++) {
37  frames.insert(Frames::value_type(endpoints[i], Frame::Identity()));
38  delta_twists.insert(Twists::value_type(endpoints[i], Twist::Zero()));
39  }
40  }
41 
42  double TreeIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frames& p_in, JntArray& q_out) {
43  q_out = q_init;
44 
45  //First check if all elements in p_in are available:
46  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
47  if(frames.find(f_des_it->first)==frames.end())
48  return -2;
49 
50  unsigned int k=0;
51  while(++k <= maxiter) {
52  for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it){
53  //Get all iterators for this endpoint
54  Frames::iterator f_it = frames.find(f_des_it->first);
55  Twists::iterator delta_twist = delta_twists.find(f_des_it->first);
56 
57  fksolver.JntToCart(q_out, f_it->second, f_it->first);
58  delta_twist->second = diff(f_it->second, f_des_it->second);
59  }
60  double res = iksolver.CartToJnt(q_out, delta_twists, delta_q);
61  if (res < eps) return res;
62 
63  Add(q_out, delta_q, q_out);
64 
65  for (unsigned int j = 0; j < q_min.rows(); j++) {
66  if (q_out(j) < q_min(j))
67  q_out( j) = q_min(j);
68  else if (q_out(j) > q_max(j))
69  q_out( j) = q_max(j);
70  }
71  }
72  if (k <= maxiter)
73  return 0;
74  else
75  return -3;
76  }
77 
78 
80 
81  }
82 
83 }//namespace
84 
static Twist Zero()
Definition: frames.hpp:291
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)=0
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Tree...
unsigned int rows() const
Definition: jntarray.cpp:72
TreeIkSolverPos_NR_JL(const Tree &tree, const std::vector< std::string > &endpoints, const JntArray &q_min, const JntArray &q_max, TreeFkSolverPos &fksolver, TreeIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
std::vector< std::string > endpoints
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:82
virtual double CartToJnt(const JntArray &q_init, const Frames &p_in, JntArray &q_out)
std::map< std::string, Frame > Frames
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
static Frame Identity()
Definition: frames.hpp:701
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Definition: tree.hpp:99
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)=0


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44