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
