Go to the documentation of this file.00001
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
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
00099
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
00189
00190 ChainIkSolverPos_LMA solver(chain);
00191
00192 test_inverseposkin(chain);
00193
00194 return 0;
00195 }
00196