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