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 <utilities/utility.h>
58 
59 #include <boost/timer.hpp>
60 
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  KDL::ChainFkSolverPos_recursive fwdkin(chain);
87  KDL::ChainIkSolverPos_LMA solver(chain,L);
88  KDL::JntArray q(n);
89  KDL::JntArray q_init(n);
90  KDL::JntArray q_sol(n);
91  for (int trial=0;trial<num_of_trials;++trial) {
92  q.data.setRandom();
93  q.data *= PI;
94  q_init.data.setRandom();
95  q_init.data *= PI;
96  KDL::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  std::cout << "-------------- failed ----------------------------" << std::endl;
118  std::cout << "pos " << pos_goal << std::endl;
119  std::cout << "reached pos " << solver.T_base_head << std::endl;
120  std::cout << "TF from pos to head \n" << pos_goal.Inverse()*solver.T_base_head << std::endl;
121  std::cout << "gradient " << solver.grad.transpose() << std::endl;
122  std::cout << "q " << q.data.transpose()/M_PI*180.0 << std::endl;
123  std::cout << "q_sol " << q_sol.data.transpose()/M_PI*180.0 << std::endl;
124  std::cout << "q_init " << q_init.data.transpose()/M_PI*180.0 << std::endl;
125  std::cout << "return value " << retval << std::endl;
126  std::cout << "last #iter " << solver.lastNrOfIter << std::endl;
127  std::cout << "last diff " << solver.lastDifference << std::endl;
128  std::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  total_number_of_iter += solver.lastNrOfIter;
134  if (solver.lastNrOfIter > max_num_of_iter) max_num_of_iter = solver.lastNrOfIter;
135  if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter;
136  if (retval!=-3) {
137  if (solver.lastDifference > max_diff) max_diff = solver.lastDifference;
138  if (solver.lastDifference < min_diff) min_diff = solver.lastDifference;
139 
140  if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff;
141  if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff;
142 
143  if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff;
144  if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff;
145  }
146  fwdkin.JntToCart(q_sol,pos_reached);
147  }
148  std::cout << "------------------ statistics ------------------------------"<<std::endl;
149  std::cout << "#times successful " << nrofresult_ok << std::endl;
150  std::cout << "#times -1 result " << nrofresult_minus1 << std::endl;
151  std::cout << "#times -2 result " << nrofresult_minus2 << std::endl;
152  std::cout << "#times -3 result " << nrofresult_minus3 << std::endl;
153  std::cout << "average number of iter " << (double)total_number_of_iter/(double)num_of_trials << std::endl;
154  std::cout << "min. nr of iter " << min_num_of_iter << std::endl;
155  std::cout << "max. nr of iter " << max_num_of_iter << std::endl;
156  std::cout << "min. difference after solving " << min_diff << std::endl;
157  std::cout << "max. difference after solving " << max_diff << std::endl;
158  std::cout << "min. trans. difference after solving " << min_trans_diff << std::endl;
159  std::cout << "max. trans. difference after solving " << max_trans_diff << std::endl;
160  std::cout << "min. rot. difference after solving " << min_rot_diff << std::endl;
161  std::cout << "max. rot. difference after solving " << max_rot_diff << std::endl;
162  double el = timer.elapsed();
163  std::cout << "elapsed time " << el << std::endl;
164  std::cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << std::endl;
165  std::cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << std::endl;
166  std::cout << "estimate of shortest time per invposkin (ms) " << el/total_number_of_iter*min_num_of_iter *1000 << std::endl;
167 }
168 
169 int main(int argc,char* argv[]) {
170  std::cout << " This example generates random joint positions, applies a forward kinematic transformation,\n"
171  << " and calls ChainIkSolverPos_LMA on the resulting pose. In this way we are sure that\n"
172  << " the resulting pose is reachable. However, some percentage of the trials will be at\n"
173  << " near singular position, where it is more difficult to achieve convergence and accuracy\n"
174  << " The initial position given to the algorithm is also a random joint position\n"
175  << " This routine asks for an accuracy of 10 micrometer, one order of magnitude better than a\n"
176  << " a typical industrial robot.\n"
177  << " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n"
178  << " and failures.\n"
179  << " Typically when executed 1 000 000 times, you will still see some small amount of failures\n"
180  << " Typically these failures are in the neighbourhoud of singularities. Most failures of type -2 still\n"
181  << " reach an accuracy better than 1E-4.\n"
182  << " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n";
183 
184  KDL::Chain chain;
185  chain = KDL::Puma560();
186  //chain = KDL::KukaLWR_DHnew();
187 
188  KDL::ChainIkSolverPos_LMA solver(chain);
189 
190  test_inverseposkin(chain);
191 
192  return 0;
193 }
194 
Chain Puma560()
Definition: puma560.cpp:27
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
const double PI
the value of pi
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.
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
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[])
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:572
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 Mon Nov 28 2022 03:17:44