Skip to content

Commit

Permalink
Get rid of using std namespace
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatthijsBurgh
MatthijsBurgh committed Sep 8, 2021
1 parent 86fffc1 commit 98bc4b5
Show file tree
Hide file tree
Showing 16 changed files with 200 additions and 231 deletions.
247 changes: 121 additions & 126 deletions orocos_kdl/examples/chainiksolverpos_lma_demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,134 +64,130 @@ estimate of shortest time per invposkin (ms) 0.155544
* \TODO provide other examples.
*/
void test_inverseposkin(KDL::Chain& chain) {
using namespace KDL;
using namespace std;
boost::timer timer;
int num_of_trials = 1000000;
int total_number_of_iter = 0;
int n = chain.getNrOfJoints();
int nrofresult_ok = 0;
int nrofresult_minus1=0;
int nrofresult_minus2=0;
int nrofresult_minus3=0;
int min_num_of_iter = 10000000;
int max_num_of_iter = 0;
double min_diff = 1E10;
double max_diff = 0;
double min_trans_diff = 1E10;
double max_trans_diff = 0;
double min_rot_diff = 1E10;
double max_rot_diff = 0;
Eigen::Matrix<double,6,1> L;
L(0)=1;L(1)=1;L(2)=1;
L(3)=0.01;L(4)=0.01;L(5)=0.01;
ChainFkSolverPos_recursive fwdkin(chain);
ChainIkSolverPos_LMA solver(chain,L);
JntArray q(n);
JntArray q_init(n);
JntArray q_sol(n);
for (int trial=0;trial<num_of_trials;++trial) {
q.data.setRandom();
q.data *= PI;
q_init.data.setRandom();
q_init.data *= PI;
Frame pos_goal,pos_reached;
fwdkin.JntToCart(q,pos_goal);
//solver.compute_fwdpos(q.data);
//pos_goal = solver.T_base_head;
int retval;
retval = solver.CartToJnt(q_init,pos_goal,q_sol);
switch (retval) {
case 0:
nrofresult_ok++;
break;
case -1:
nrofresult_minus1++;
break;
case -2:
nrofresult_minus2++;
break;
case -3:
nrofresult_minus3++;
break;
}
if (retval !=0) {
cout << "-------------- failed ----------------------------" << endl;
cout << "pos " << pos_goal << endl;
cout << "reached pos " << solver.T_base_head << endl;
cout << "TF from pos to head \n" << pos_goal.Inverse()*solver.T_base_head << endl;
cout << "gradient " << solver.grad.transpose() << endl;
cout << "q " << q.data.transpose()/M_PI*180.0 << endl;
cout << "q_sol " << q_sol.data.transpose()/M_PI*180.0 << endl;
cout << "q_init " << q_init.data.transpose()/M_PI*180.0 << endl;
cout << "return value " << retval << endl;
cout << "last #iter " << solver.lastNrOfIter << endl;
cout << "last diff " << solver.lastDifference << endl;
cout << "jacobian of goal values ";
solver.display_jac(q);
std::cout << "jacobian of solved values ";
solver.display_jac(q_sol);

}
total_number_of_iter += solver.lastNrOfIter;
if (solver.lastNrOfIter > max_num_of_iter) max_num_of_iter = solver.lastNrOfIter;
if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter;
if (retval!=-3) {
if (solver.lastDifference > max_diff) max_diff = solver.lastDifference;
if (solver.lastDifference < min_diff) min_diff = solver.lastDifference;

if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff;
if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff;

if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff;
if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff;
}
fwdkin.JntToCart(q_sol,pos_reached);
}
cout << "------------------ statistics ------------------------------"<<endl;
cout << "#times successful " << nrofresult_ok << endl;
cout << "#times -1 result " << nrofresult_minus1 << endl;
cout << "#times -2 result " << nrofresult_minus2 << endl;
cout << "#times -3 result " << nrofresult_minus3 << endl;
cout << "average number of iter " << (double)total_number_of_iter/(double)num_of_trials << endl;
cout << "min. nr of iter " << min_num_of_iter << endl;
cout << "max. nr of iter " << max_num_of_iter << endl;
cout << "min. difference after solving " << min_diff << endl;
cout << "max. difference after solving " << max_diff << endl;
cout << "min. trans. difference after solving " << min_trans_diff << endl;
cout << "max. trans. difference after solving " << max_trans_diff << endl;
cout << "min. rot. difference after solving " << min_rot_diff << endl;
cout << "max. rot. difference after solving " << max_rot_diff << endl;
double el = timer.elapsed();
cout << "elapsed time " << el << endl;
cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << endl;
cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << endl;
cout << "estimate of shortest time per invposkin (ms) " << el/total_number_of_iter*min_num_of_iter *1000 << endl;
boost::timer timer;
int num_of_trials = 1000000;
int total_number_of_iter = 0;
int n = chain.getNrOfJoints();
int nrofresult_ok = 0;
int nrofresult_minus1=0;
int nrofresult_minus2=0;
int nrofresult_minus3=0;
int min_num_of_iter = 10000000;
int max_num_of_iter = 0;
double min_diff = 1E10;
double max_diff = 0;
double min_trans_diff = 1E10;
double max_trans_diff = 0;
double min_rot_diff = 1E10;
double max_rot_diff = 0;
Eigen::Matrix<double,6,1> L;
L(0)=1;L(1)=1;L(2)=1;
L(3)=0.01;L(4)=0.01;L(5)=0.01;
KDL::ChainFkSolverPos_recursive fwdkin(chain);
KDL::ChainIkSolverPos_LMA solver(chain,L);
KDL::JntArray q(n);
KDL::JntArray q_init(n);
KDL::JntArray q_sol(n);
for (int trial=0;trial<num_of_trials;++trial) {
q.data.setRandom();
q.data *= PI;
q_init.data.setRandom();
q_init.data *= PI;
KDL::Frame pos_goal,pos_reached;
fwdkin.JntToCart(q,pos_goal);
//solver.compute_fwdpos(q.data);
//pos_goal = solver.T_base_head;
int retval;
retval = solver.CartToJnt(q_init,pos_goal,q_sol);
switch (retval) {
case 0:
nrofresult_ok++;
break;
case -1:
nrofresult_minus1++;
break;
case -2:
nrofresult_minus2++;
break;
case -3:
nrofresult_minus3++;
break;
}
if (retval !=0) {
std::cout << "-------------- failed ----------------------------" << std::endl;
std::cout << "pos " << pos_goal << std::endl;
std::cout << "reached pos " << solver.T_base_head << std::endl;
std::cout << "TF from pos to head \n" << pos_goal.Inverse()*solver.T_base_head << std::endl;
std::cout << "gradient " << solver.grad.transpose() << std::endl;
std::cout << "q " << q.data.transpose()/M_PI*180.0 << std::endl;
std::cout << "q_sol " << q_sol.data.transpose()/M_PI*180.0 << std::endl;
std::cout << "q_init " << q_init.data.transpose()/M_PI*180.0 << std::endl;
std::cout << "return value " << retval << std::endl;
std::cout << "last #iter " << solver.lastNrOfIter << std::endl;
std::cout << "last diff " << solver.lastDifference << std::endl;
std::cout << "jacobian of goal values ";
solver.display_jac(q);
std::cout << "jacobian of solved values ";
solver.display_jac(q_sol);
}
total_number_of_iter += solver.lastNrOfIter;
if (solver.lastNrOfIter > max_num_of_iter) max_num_of_iter = solver.lastNrOfIter;
if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter;
if (retval!=-3) {
if (solver.lastDifference > max_diff) max_diff = solver.lastDifference;
if (solver.lastDifference < min_diff) min_diff = solver.lastDifference;

if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff;
if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff;

if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff;
if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff;
}
fwdkin.JntToCart(q_sol,pos_reached);
}
std::cout << "------------------ statistics ------------------------------"<<std::endl;
std::cout << "#times successful " << nrofresult_ok << std::endl;
std::cout << "#times -1 result " << nrofresult_minus1 << std::endl;
std::cout << "#times -2 result " << nrofresult_minus2 << std::endl;
std::cout << "#times -3 result " << nrofresult_minus3 << std::endl;
std::cout << "average number of iter " << (double)total_number_of_iter/(double)num_of_trials << std::endl;
std::cout << "min. nr of iter " << min_num_of_iter << std::endl;
std::cout << "max. nr of iter " << max_num_of_iter << std::endl;
std::cout << "min. difference after solving " << min_diff << std::endl;
std::cout << "max. difference after solving " << max_diff << std::endl;
std::cout << "min. trans. difference after solving " << min_trans_diff << std::endl;
std::cout << "max. trans. difference after solving " << max_trans_diff << std::endl;
std::cout << "min. rot. difference after solving " << min_rot_diff << std::endl;
std::cout << "max. rot. difference after solving " << max_rot_diff << std::endl;
double el = timer.elapsed();
std::cout << "elapsed time " << el << std::endl;
std::cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << std::endl;
std::cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << std::endl;
std::cout << "estimate of shortest time per invposkin (ms) " << el/total_number_of_iter*min_num_of_iter *1000 << std::endl;
}

int main(int argc,char* argv[]) {
std::cout <<
" This example generates random joint positions, applies a forward kinematic transformation,\n"
<< " and calls ChainIkSolverPos_LMA on the resulting pose. In this way we are sure that\n"
<< " the resulting pose is reachable. However, some percentage of the trials will be at\n"
<< " near singular position, where it is more difficult to achieve convergence and accuracy\n"
<< " The initial position given to the algorithm is also a random joint position\n"
<< " This routine asks for an accuracy of 10 micrometer, one order of magnitude better than a\n"
<< " a typical industrial robot.\n"
<< " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n"
<< " and failures.\n"
<< " Typically when executed 1 000 000 times, you will still see some small amount of failures\n"
<< " Typically these failures are in the neighbourhoud of singularities. Most failures of type -2 still\n"
<< " reach an accuracy better than 1E-4.\n"
<< " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n";
using namespace KDL;
Chain chain;
chain = KDL::Puma560();
//chain = KDL::KukaLWR_DHnew();

ChainIkSolverPos_LMA solver(chain);

test_inverseposkin(chain);
std::cout << " This example generates random joint positions, applies a forward kinematic transformation,\n"
<< " and calls ChainIkSolverPos_LMA on the resulting pose. In this way we are sure that\n"
<< " the resulting pose is reachable. However, some percentage of the trials will be at\n"
<< " near singular position, where it is more difficult to achieve convergence and accuracy\n"
<< " The initial position given to the algorithm is also a random joint position\n"
<< " This routine asks for an accuracy of 10 micrometer, one order of magnitude better than a\n"
<< " a typical industrial robot.\n"
<< " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n"
<< " and failures.\n"
<< " Typically when executed 1 000 000 times, you will still see some small amount of failures\n"
<< " Typically these failures are in the neighbourhoud of singularities. Most failures of type -2 still\n"
<< " reach an accuracy better than 1E-4.\n"
<< " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n";

KDL::Chain chain;
chain = KDL::Puma560();
//chain = KDL::KukaLWR_DHnew();

KDL::ChainIkSolverPos_LMA solver(chain);

test_inverseposkin(chain);

return 0;
}
Expand All @@ -217,4 +213,3 @@ estimate of longest time per invposkin (ms) 5.98245
estimate of shortest time per invposkin (ms) 0.155544
*/

39 changes: 20 additions & 19 deletions orocos_kdl/models/kukaLWRtestDHnew.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@
#include <chainidsolver_recursive_newton_euler.hpp>

using namespace KDL;
using namespace std;

using std::setw;

void outputLine( double, double, double, double, double, double, double);
int getInputs(JntArray&, JntArray&, JntArray&, int&);
Expand Down Expand Up @@ -35,23 +36,23 @@ int main(int argc , char** argv){
std::cout<<"tau: "<<tau<<std::endl;

//write file: code based on example 14.4, c++ how to program, Deitel and Deitel, book p 708
ofstream outPoseFile("poseResultaat.dat",ios::app);
std::ofstream outPoseFile("poseResultaat.dat",std::ios::app);
if(!outPoseFile)
{
cerr << "File poseResultaat could not be opened" <<endl;
std::cerr << "File poseResultaat could not be opened" <<std::endl;
exit(1);
}
outPoseFile << "linenumber=experimentnr= "<< linenum << "\n";
outPoseFile << T << "\n \n";
outPoseFile.close();

ofstream outTauFile("tauResultaat.dat",ios::app);
std::ofstream outTauFile("tauResultaat.dat",std::ios::app);
if(!outTauFile)
{
cerr << "File tauResultaat could not be opened" <<endl;
std::cerr << "File tauResultaat could not be opened" <<std::endl;
exit(1);
}
outTauFile << setiosflags( ios::left) << setw(10) << linenum;
outTauFile << setiosflags( std::ios::left) << setw(10) << linenum;
outTauFile << tau << "\n";
outTauFile.close();
}
Expand All @@ -60,7 +61,7 @@ int main(int argc , char** argv){

int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
{
//cout << " q" << _q<< "\n";
//std::cout << " q" << _q<< "\n";

//declaration
//int linenr; //line =experiment number
Expand All @@ -70,25 +71,25 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
counter=0;

//ask which experiment number= line number in files
cout << "Give experiment number= line number in files \n ?";
cin >> linenr;
std::cout << "Give experiment number= line number in files \n ?";
std::cin >> linenr;

//read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712

/*
*READING Q = joint positions
*/

ifstream inQfile("interpreteerbaar/q", ios::in);
std::ifstream inQfile("interpreteerbaar/q", std::ios::in);

if (!inQfile)
{
cerr << "File q could not be opened \n";
std::cerr << "File q could not be opened \n";
exit(1);
}

//print headers
cout << setiosflags( ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ;
std::cout << setiosflags( std::ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ;

while(!inQfile.eof())
{
Expand All @@ -108,16 +109,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
*READING Qdot = joint velocities
*/
counter=0;//reset counter
ifstream inQdotfile("interpreteerbaar/qdot", ios::in);
ifstream inQdotfile("interpreteerbaar/qdot", std::ios::in);

if (!inQdotfile)
{
cerr << "File qdot could not be opened \n";
std::cerr << "File qdot could not be opened \n";
exit(1);
}

//print headers
cout << setiosflags( ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ;
std::cout << setiosflags( std::ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ;

while(!inQdotfile.eof())
{
Expand All @@ -137,16 +138,16 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
*READING Qdotdot = joint accelerations
*/
counter=0;//reset counter
ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in);
std::ifstream inQdotdotfile("interpreteerbaar/qddot", std::ios::in);

if (!inQdotdotfile)
{
cerr << "File qdotdot could not be opened \n";
std::cerr << "File qdotdot could not be opened \n";
exit(1);
}

//print headers
cout << setiosflags( ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ;
std::cout << setiosflags( std::ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ;

while(!inQdotdotfile.eof())
{
Expand All @@ -168,6 +169,6 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)

void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7)
{
cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) <<setw(15)
std::cout << setiosflags(std::ios::left) << setiosflags(std::ios::fixed | std::ios::showpoint) <<setw(15)
<< x1 << setw(15) << x2 <<setw(15) <<setw(15) << x3 <<setw(15) << x4 <<setw(15) << x5 <<setw(15) << x6 <<setw(15) << x7 <<"\n";
}
Loading

0 comments on commit 98bc4b5

Please sign in to comment.