$search
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