chainiksolverpos_lma_demo.cpp
Go to the documentation of this file.
1 
27 /**************************************************************************
28  begin : May 2011
29  copyright : (C) 2011 Erwin Aertbelien
30  email : firstname.lastname@mech.kuleuven.ac.be
31 
32  History (only major changes)( AUTHOR-Description ) :
33 
34  ***************************************************************************
35  * This library is free software; you can redistribute it and/or *
36  * modify it under the terms of the GNU Lesser General Public *
37  * License as published by the Free Software Foundation; either *
38  * version 2.1 of the License, or (at your option) any later version. *
39  * *
40  * This library is distributed in the hope that it will be useful, *
41  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
42  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
43  * Lesser General Public License for more details. *
44  * *
45  * You should have received a copy of the GNU Lesser General Public *
46  * License along with this library; if not, write to the Free Software *
47  * Foundation, Inc., 59 Temple Place, *
48  * Suite 330, Boston, MA 02111-1307 USA *
49  * *
50  ***************************************************************************/
51 
52 #include <iostream>
53 #include <frames_io.hpp>
54 #include <models.hpp>
55 #include <chainiksolverpos_lma.hpp>
57 #include <boost/timer.hpp>
58 
65  using namespace KDL;
66  using namespace std;
67  boost::timer timer;
68  int num_of_trials = 1000000;
69  int total_number_of_iter = 0;
70  int n = chain.getNrOfJoints();
71  int nrofresult_ok = 0;
72  int nrofresult_minus1=0;
73  int nrofresult_minus2=0;
74  int nrofresult_minus3=0;
75  int min_num_of_iter = 10000000;
76  int max_num_of_iter = 0;
77  double min_diff = 1E10;
78  double max_diff = 0;
79  double min_trans_diff = 1E10;
80  double max_trans_diff = 0;
81  double min_rot_diff = 1E10;
82  double max_rot_diff = 0;
83  Eigen::Matrix<double,6,1> L;
84  L(0)=1;L(1)=1;L(2)=1;
85  L(3)=0.01;L(4)=0.01;L(5)=0.01;
86  ChainFkSolverPos_recursive fwdkin(chain);
87  ChainIkSolverPos_LMA solver(chain,L);
88  JntArray q(n);
89  JntArray q_init(n);
90  JntArray q_sol(n);
91  for (int trial=0;trial<num_of_trials;++trial) {
92  q.data.setRandom();
93  q.data *= M_PI;
94  q_init.data.setRandom();
95  q_init.data *= M_PI;
96  Frame pos_goal,pos_reached;
97  fwdkin.JntToCart(q,pos_goal);
98  //solver.compute_fwdpos(q.data);
99  //pos_goal = solver.T_base_head;
100  int retval;
101  retval = solver.CartToJnt(q_init,pos_goal,q_sol);
102  switch (retval) {
103  case 0:
104  nrofresult_ok++;
105  break;
106  case -1:
107  nrofresult_minus1++;
108  break;
109  case -2:
110  nrofresult_minus2++;
111  break;
112  case -3:
113  nrofresult_minus3++;
114  break;
115  }
116  if (retval !=0) {
117  cout << "-------------- failed ----------------------------" << endl;
118  cout << "pos " << pos_goal << endl;
119  cout << "reached pos " << solver.T_base_head << endl;
120  cout << "TF from pos to head \n" << pos_goal.Inverse()*solver.T_base_head << endl;
121  cout << "gradient " << solver.grad.transpose() << endl;
122  cout << "q " << q.data.transpose()/M_PI*180.0 << endl;
123  cout << "q_sol " << q_sol.data.transpose()/M_PI*180.0 << endl;
124  cout << "q_init " << q_init.data.transpose()/M_PI*180.0 << endl;
125  cout << "return value " << retval << endl;
126  cout << "last #iter " << solver.lastNrOfIter << endl;
127  cout << "last diff " << solver.lastDifference << endl;
128  cout << "jacobian of goal values ";
129  solver.display_jac(q);
130  std::cout << "jacobian of solved values ";
131  solver.display_jac(q_sol);
132 
133  }
134  total_number_of_iter += solver.lastNrOfIter;
135  if (solver.lastNrOfIter > max_num_of_iter) max_num_of_iter = solver.lastNrOfIter;
136  if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter;
137  if (retval!=-3) {
138  if (solver.lastDifference > max_diff) max_diff = solver.lastDifference;
139  if (solver.lastDifference < min_diff) min_diff = solver.lastDifference;
140 
141  if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff;
142  if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff;
143 
144  if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff;
145  if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff;
146  }
147  fwdkin.JntToCart(q_sol,pos_reached);
148  }
149  cout << "------------------ statistics ------------------------------"<<endl;
150  cout << "#times successful " << nrofresult_ok << endl;
151  cout << "#times -1 result " << nrofresult_minus1 << endl;
152  cout << "#times -2 result " << nrofresult_minus2 << endl;
153  cout << "#times -3 result " << nrofresult_minus3 << endl;
154  cout << "average number of iter " << (double)total_number_of_iter/(double)num_of_trials << endl;
155  cout << "min. nr of iter " << min_num_of_iter << endl;
156  cout << "max. nr of iter " << max_num_of_iter << endl;
157  cout << "min. difference after solving " << min_diff << endl;
158  cout << "max. difference after solving " << max_diff << endl;
159  cout << "min. trans. difference after solving " << min_trans_diff << endl;
160  cout << "max. trans. difference after solving " << max_trans_diff << endl;
161  cout << "min. rot. difference after solving " << min_rot_diff << endl;
162  cout << "max. rot. difference after solving " << max_rot_diff << endl;
163  double el = timer.elapsed();
164  cout << "elapsed time " << el << endl;
165  cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << endl;
166  cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << endl;
167  cout << "estimate of shortest time per invposkin (ms) " << el/total_number_of_iter*min_num_of_iter *1000 << endl;
168 }
169 
170 int main(int argc,char* argv[]) {
171  std::cout <<
172  " This example generates random joint positions, applies a forward kinematic transformation,\n"
173  << " and calls ChainIkSolverPos_LMA on the resulting pose. In this way we are sure that\n"
174  << " the resulting pose is reachable. However, some percentage of the trials will be at\n"
175  << " near singular position, where it is more difficult to achieve convergence and accuracy\n"
176  << " The initial position given to the algorithm is also a random joint position\n"
177  << " This routine asks for an accuracy of 10 micrometer, one order of magnitude better than a\n"
178  << " a typical industrial robot.\n"
179  << " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n"
180  << " and failures.\n"
181  << " Typically when executed 1 000 000 times, you will still see some small amount of failures\n"
182  << " Typically these failures are in the neighbourhoud of singularities. Most failures of type -2 still\n"
183  << " reach an accuracy better than 1E-4.\n"
184  << " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n";
185  using namespace KDL;
186  Chain chain;
187  chain = KDL::Puma560();
188  //chain = KDL::KukaLWR_DHnew();
189 
190  ChainIkSolverPos_LMA solver(chain);
191 
192  test_inverseposkin(chain);
193 
194  return 0;
195 }
196 
Chain Puma560()
Definition: puma560.cpp:26
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
KDL::Frame T_base_head
for internal use only.
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt...
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
void display_jac(const KDL::JntArray &jval)
for internal use only. Only exposed for test and diagnostic purposes.
computing inverse position kinematics using Levenberg-Marquardt.
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Eigen::VectorXd data
Definition: jntarray.hpp:72
int main(int argc, char *argv[])
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
double lastDifference
contains the last value for after an execution of CartToJnt.
VectorXq grad
for internal use only.
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
void test_inverseposkin(KDL::Chain &chain)
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36