chainiksolverpos_lma_demo.cpp
Go to the documentation of this file.
00001 
00027 /**************************************************************************
00028     begin                : May 2011
00029     copyright            : (C) 2011 Erwin Aertbelien
00030     email                : firstname.lastname@mech.kuleuven.ac.be
00031 
00032  History (only major changes)( AUTHOR-Description ) :
00033 
00034  ***************************************************************************
00035  *   This library is free software; you can redistribute it and/or         *
00036  *   modify it under the terms of the GNU Lesser General Public            *
00037  *   License as published by the Free Software Foundation; either          *
00038  *   version 2.1 of the License, or (at your option) any later version.    *
00039  *                                                                         *
00040  *   This library is distributed in the hope that it will be useful,       *
00041  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00042  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00043  *   Lesser General Public License for more details.                       *
00044  *                                                                         *
00045  *   You should have received a copy of the GNU Lesser General Public      *
00046  *   License along with this library; if not, write to the Free Software   *
00047  *   Foundation, Inc., 59 Temple Place,                                    *
00048  *   Suite 330, Boston, MA  02111-1307  USA                                *
00049  *                                                                         *
00050  ***************************************************************************/
00051 
00052 #include <iostream>
00053 #include <frames_io.hpp>
00054 #include <models.hpp>
00055 #include <chainiksolverpos_lma.hpp>
00056 #include <chainfksolverpos_recursive.hpp>
00057 #include <boost/timer.hpp>
00058 
00064 void test_inverseposkin(KDL::Chain& chain) {
00065         using namespace KDL;
00066         using namespace std;
00067         boost::timer timer;
00068         int num_of_trials = 1000000;
00069         int total_number_of_iter = 0;
00070         int n = chain.getNrOfJoints();
00071         int nrofresult_ok = 0;
00072         int nrofresult_minus1=0;
00073         int nrofresult_minus2=0;
00074         int nrofresult_minus3=0;
00075         int min_num_of_iter = 10000000;
00076         int max_num_of_iter = 0;
00077         double min_diff = 1E10;
00078         double max_diff = 0;
00079         double min_trans_diff = 1E10;
00080         double max_trans_diff = 0;
00081         double min_rot_diff = 1E10;
00082         double max_rot_diff = 0;
00083         Eigen::Matrix<double,6,1> L;
00084         L(0)=1;L(1)=1;L(2)=1;
00085         L(3)=0.01;L(4)=0.01;L(5)=0.01;
00086         ChainFkSolverPos_recursive fwdkin(chain);
00087         ChainIkSolverPos_LMA solver(chain,L);
00088         JntArray q(n);
00089         JntArray q_init(n);
00090         JntArray q_sol(n);
00091         for (int trial=0;trial<num_of_trials;++trial) {
00092                 q.data.setRandom();
00093                 q.data *= M_PI;
00094                 q_init.data.setRandom();
00095                 q_init.data *= M_PI;
00096                 Frame pos_goal,pos_reached;
00097                 fwdkin.JntToCart(q,pos_goal);
00098                 //solver.compute_fwdpos(q.data);
00099                 //pos_goal = solver.T_base_head;
00100                 int retval;
00101                 retval = solver.CartToJnt(q_init,pos_goal,q_sol);
00102                 switch (retval) {
00103                 case 0:
00104                         nrofresult_ok++;
00105                         break;
00106                 case -1:
00107                         nrofresult_minus1++;
00108                         break;
00109                 case -2:
00110                         nrofresult_minus2++;
00111                         break;
00112                 case -3:
00113                         nrofresult_minus3++;
00114                         break;
00115                 }
00116                 if (retval !=0) {
00117                         cout << "-------------- failed ----------------------------" << endl;
00118                         cout << "pos " << pos_goal << endl;
00119                         cout << "reached pos " << solver.T_base_head << endl;
00120                         cout << "TF from pos to head \n" << pos_goal.Inverse()*solver.T_base_head << endl;
00121                         cout << "gradient " << solver.grad.transpose() << endl;
00122                         cout << "q   " << q.data.transpose()/M_PI*180.0 << endl;
00123                         cout << "q_sol " << q_sol.data.transpose()/M_PI*180.0 << endl;
00124                         cout << "q_init " << q_init.data.transpose()/M_PI*180.0 << endl;
00125                         cout << "return value " << retval << endl;
00126                         cout << "last #iter " << solver.lastNrOfIter << endl;
00127                         cout << "last diff  " << solver.lastDifference << endl;
00128                         cout << "jacobian of goal values ";
00129                         solver.display_jac(q);
00130                         std::cout << "jacobian of solved values ";
00131                         solver.display_jac(q_sol);
00132 
00133                 }
00134                 total_number_of_iter += solver.lastNrOfIter;
00135                 if (solver.lastNrOfIter > max_num_of_iter) max_num_of_iter = solver.lastNrOfIter;
00136                 if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter;
00137                 if (retval!=-3) {
00138                 if (solver.lastDifference > max_diff) max_diff = solver.lastDifference;
00139                 if (solver.lastDifference < min_diff) min_diff = solver.lastDifference;
00140 
00141                 if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff;
00142                 if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff;
00143 
00144                 if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff;
00145                 if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff;
00146                 }
00147                 fwdkin.JntToCart(q_sol,pos_reached);
00148         }
00149         cout << "------------------ statistics ------------------------------"<<endl;
00150         cout << "#times successful " << nrofresult_ok << endl;
00151         cout << "#times -1 result " << nrofresult_minus1 << endl;
00152         cout << "#times -2 result " << nrofresult_minus2 << endl;
00153         cout << "#times -3 result " << nrofresult_minus3 << endl;
00154         cout << "average number of iter " << (double)total_number_of_iter/(double)num_of_trials << endl;
00155         cout << "min. nr of iter " << min_num_of_iter << endl;
00156         cout << "max. nr of iter " << max_num_of_iter << endl;
00157         cout << "min. difference after solving " << min_diff << endl;
00158         cout << "max. difference after solving " << max_diff << endl;
00159         cout << "min. trans. difference after solving " << min_trans_diff << endl;
00160         cout << "max. trans. difference after solving " << max_trans_diff << endl;
00161         cout << "min. rot. difference after solving " << min_rot_diff << endl;
00162         cout << "max. rot. difference after solving " << max_rot_diff << endl;
00163         double el = timer.elapsed();
00164         cout << "elapsed time " << el << endl;
00165         cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << endl;
00166         cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << endl;
00167         cout << "estimate of shortest time per invposkin (ms) " << el/total_number_of_iter*min_num_of_iter *1000 << endl;
00168 }
00169 
00170 int main(int argc,char* argv[]) {
00171         std::cout <<
00172                         " This example generates random joint positions, applies a forward kinematic transformation,\n"
00173                 <<  " and calls ChainIkSolverPos_LMA on the resulting pose.  In this way we are sure that\n"
00174                 <<  " the resulting pose is reachable.  However, some percentage of the trials will be at\n"
00175                 <<  " near singular position, where it is more difficult to achieve convergence and accuracy\n"
00176                 <<  " The initial position given to the algorithm is also a random joint position\n"
00177                 <<  " This routine asks for an accuracy of 10 micrometer, one order of magnitude better than a\n"
00178                 <<  " a typical industrial robot.\n"
00179                 <<  " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n"
00180                 <<  " and failures.\n"
00181                 <<  " Typically when executed 1 000 000 times, you will still see some small amount of failures\n"
00182                 <<  " Typically these failures are in the neighbourhoud of singularities.  Most failures of type -2 still\n"
00183                 <<  " reach an accuracy better than 1E-4.\n"
00184                 <<  " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n";
00185         using namespace KDL;
00186         Chain chain;
00187         chain = KDL::Puma560();
00188         //chain = KDL::KukaLWR_DHnew();
00189 
00190         ChainIkSolverPos_LMA solver(chain);
00191 
00192         test_inverseposkin(chain);
00193 
00194     return 0;
00195 }
00196 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14