zxxzxztest.cpp
Go to the documentation of this file.
00001 #include <kdl/kinfam/zxxzxz.hpp>
00002 #include <kdl/kinfam/zxxzxzjnt2cartpos.hpp>
00003 #include <kdl/kinfam/zxxzxzcartpos2jnt.hpp>
00004 #include <kdl/frames.hpp>
00005 #include <kdl/frames_io.hpp>
00006 #include <kdl/kinfam/lineartransmission.hpp>
00007 #include <kdl/kinfam/unittransmission.hpp>
00008 
00009 using namespace KDL;
00010 using namespace std;
00015 void test_zxxzxz(double l1,double l2,double l3,double l6) {
00016     cout << "========================================================================================"  << endl;
00017     cout << " test_zxxzxz : forward and inverse position kinematics for all configurations" << endl;
00018     cout << "========================================================================================"  << endl;
00019     ZXXZXZ kf("tst");
00020     kf.setLinkLengths(l1,l2,l3,l6);
00021     ZXXZXZJnt2CartPos* jnt2cartpos =
00022         (ZXXZXZJnt2CartPos*) kf.createJnt2CartPos();
00023     ZXXZXZCartPos2Jnt* cartpos2jnt =
00024         (ZXXZXZCartPos2Jnt*) kf.createCartPos2Jnt();
00025     SerialChain* kf2 = kf.createSerialChain();
00026     Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
00027 
00028     std::vector<double> q(6);
00029     std::vector<double> q2(6);
00030     double epsq = 1E-8;
00031     int config,config2,config3;
00032     Frame F,F2,F3;
00033     bool exitflag=false;
00034 
00035     for (int config=0;config<8;config++) {
00036         cout << endl<<"=== Testing configuration : " << config << " === "<< endl;
00037         kf.setConfigurationToJoints(config,q);
00038         config2=kf.getConfigurationFromJoints(q);
00039         if (config != config2) {
00040             cout << "FAIL :test_zxxzxz(): configurations do not match using configuration representation transformation" << endl;
00041             cerr << "FAIL :test_zxxzxz(): configurations do not match using configuration representation transformation" << endl;
00042             cout << "original configuration " << kf.getConfigurationDescription(config)  << endl;
00043             cout << "reached  configuration " << kf.getConfigurationDescription(config2) << endl;
00044             exitflag=true;
00045         }
00046 
00047         // fwd kin
00048         jnt2cartpos->evaluate(q);
00049         jnt2cartpos->getFrame(F);
00050         jnt2cartpos->getConfiguration(config2);
00051         jnt2cartpos2->evaluate(q);
00052         jnt2cartpos2->getFrame(F3);
00053         if (!Equal(F,F3,1E-6)) {
00054              cout << "FAIL :test_zxxzxz(): fwd(q)!=fwd_serialchain(q)" << endl;
00055              cerr << "FAIL :test_zxxzxz(): fwd(q)!=fwd_serialchain(q)" << endl;
00056              cout << "Frame F = fwd(q) " << F << endl;
00057              cout << "Frame F = fwd_serialchain(q) " << F3 << endl;
00058              exitflag=true;
00059         }
00060 
00061         if (config!=config2) {
00062             cout << "FAIL :test_zxxzxz(): configurations do not match after jnt2cartpos transform" << endl;
00063             cout << "original configuration " << kf.getConfigurationDescription(config)  << endl;
00064             cout << "reached  configuration " << kf.getConfigurationDescription(config2) << endl;
00065             cerr << "FAIL :test_zxxzxz(): configurations do not match after jnt2cartpos transform" << endl;
00066             exitflag=true;
00067         }
00068         // inv kin
00069         cartpos2jnt->setConfiguration(config);
00070         cartpos2jnt->setFrame(F);
00071         cartpos2jnt->evaluate(q2);
00072         jnt2cartpos->evaluate(q2);
00073         jnt2cartpos->getConfiguration(config3);
00074         jnt2cartpos->getFrame(F2);
00075         if (!Equal(F,F2,1E-6)) {
00076              cout << "FAIL :test_zxxzxz(): fwd(inv(F))!=F" << endl;
00077              cerr << "FAIL :test_zxxzxz(): fwd(inv(F))!=F" << endl;
00078              exitflag=true;
00079         }
00080         for (int i=0;i<q.size();++i) {
00081             if (fabs(q[i]-q2[i])>epsq) {
00082                 cout << "FAIL :test_zxxzxz(): inv(fwd(q))!=q" << endl;
00083                 cerr << "FAIL :test_zxxzxz(): inv(fwd(q))!=q" << endl;
00084                 exitflag=true;
00085             }
00086         }
00087         if (exitflag) {
00088                 cout << "=========== FAILURE REPORT ================" << endl;
00089                 cout << "Frame F = fwd(q) = " << F << endl;
00090                 cout << "original configuration " << kf.getConfigurationDescription(config)  << endl;
00091                 cout << "Frame F2 = fwd(inv(F)) = " << F2 << endl;
00092                 cout << "configuration of fwd(inv(F)) = " << kf.getConfigurationDescription(config3)  << endl;
00093                 cout << "Comparing q with q2=inv(fwd(q)) "<< endl;
00094                 for (int j=0;j<6;++j) {
00095                     std::cout << "q["<<j<<"]="<<q[j]<<" and q2["<<j<<"]="<<q2[j]<<std::endl;
00096                 }
00097         }
00098     }
00099     if (exitflag) return exit(-1);
00100     delete jnt2cartpos2;
00101     delete kf2;
00102     delete cartpos2jnt;
00103     delete jnt2cartpos;
00104 }
00105 
00106 
00107 
00108 
00109 
00110 int main(int argc,char* argv[]) {
00111     test_zxxzxz(0.33,0.305,0.33,0.176);
00112     test_zxxzxz(0.1,0.2,0.3,0.4);
00113     test_zxxzxz(0.5,0.3,0.2,0.5);
00114         return 0;
00115 }
00116 


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