orocos_kdl
zxxzxztest.cpp
Go to the documentation of this file.
1 #include <kdl/kinfam/zxxzxz.hpp>
2 #include <kdl/kinfam/zxxzxzjnt2cartpos.hpp>
3 #include <kdl/kinfam/zxxzxzcartpos2jnt.hpp>
4 #include <kdl/frames.hpp>
5 #include <kdl/frames_io.hpp>
6 #include <kdl/kinfam/lineartransmission.hpp>
7 #include <kdl/kinfam/unittransmission.hpp>
8 
9 #include <iostream>
10 
11 using namespace KDL;
12 
13 using std::cerr;
14 using std::cout;
15 using std::endl;
16 
21 void test_zxxzxz(double l1,double l2,double l3,double l6) {
22  cout << "========================================================================================" << endl;
23  cout << " test_zxxzxz : forward and inverse position kinematics for all configurations" << endl;
24  cout << "========================================================================================" << endl;
25  ZXXZXZ kf("tst");
26  kf.setLinkLengths(l1,l2,l3,l6);
27  ZXXZXZJnt2CartPos* jnt2cartpos =
28  (ZXXZXZJnt2CartPos*) kf.createJnt2CartPos();
29  ZXXZXZCartPos2Jnt* cartpos2jnt =
30  (ZXXZXZCartPos2Jnt*) kf.createCartPos2Jnt();
31  SerialChain* kf2 = kf.createSerialChain();
32  Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
33 
35  std::vector<double> q2(6);
36  double epsq = 1E-8;
37  int config,config2,config3;
38  Frame F,F2,F3;
39  bool exitflag=false;
40 
41  for (int config=0;config<8;config++) {
42  cout << endl<<"=== Testing configuration : " << config << " === "<< endl;
43  kf.setConfigurationToJoints(config,q);
44  config2=kf.getConfigurationFromJoints(q);
45  if (config != config2) {
46  cout << "FAIL :test_zxxzxz(): configurations do not match using configuration representation transformation" << endl;
47  cerr << "FAIL :test_zxxzxz(): configurations do not match using configuration representation transformation" << endl;
48  cout << "original configuration " << kf.getConfigurationDescription(config) << endl;
49  cout << "reached configuration " << kf.getConfigurationDescription(config2) << endl;
50  exitflag=true;
51  }
52 
53  // fwd kin
54  jnt2cartpos->evaluate(q);
55  jnt2cartpos->getFrame(F);
56  jnt2cartpos->getConfiguration(config2);
57  jnt2cartpos2->evaluate(q);
58  jnt2cartpos2->getFrame(F3);
59  if (!Equal(F,F3,1E-6)) {
60  cout << "FAIL :test_zxxzxz(): fwd(q)!=fwd_serialchain(q)" << endl;
61  cerr << "FAIL :test_zxxzxz(): fwd(q)!=fwd_serialchain(q)" << endl;
62  cout << "Frame F = fwd(q) " << F << endl;
63  cout << "Frame F = fwd_serialchain(q) " << F3 << endl;
64  exitflag=true;
65  }
66 
67  if (config!=config2) {
68  cout << "FAIL :test_zxxzxz(): configurations do not match after jnt2cartpos transform" << endl;
69  cout << "original configuration " << kf.getConfigurationDescription(config) << endl;
70  cout << "reached configuration " << kf.getConfigurationDescription(config2) << endl;
71  cerr << "FAIL :test_zxxzxz(): configurations do not match after jnt2cartpos transform" << endl;
72  exitflag=true;
73  }
74  // inv kin
75  cartpos2jnt->setConfiguration(config);
76  cartpos2jnt->setFrame(F);
77  cartpos2jnt->evaluate(q2);
78  jnt2cartpos->evaluate(q2);
79  jnt2cartpos->getConfiguration(config3);
80  jnt2cartpos->getFrame(F2);
81  if (!Equal(F,F2,1E-6)) {
82  cout << "FAIL :test_zxxzxz(): fwd(inv(F))!=F" << endl;
83  cerr << "FAIL :test_zxxzxz(): fwd(inv(F))!=F" << endl;
84  exitflag=true;
85  }
86  for (int i=0;i<q.size();++i) {
87  if (fabs(q[i]-q2[i])>epsq) {
88  cout << "FAIL :test_zxxzxz(): inv(fwd(q))!=q" << endl;
89  cerr << "FAIL :test_zxxzxz(): inv(fwd(q))!=q" << endl;
90  exitflag=true;
91  }
92  }
93  if (exitflag) {
94  cout << "=========== FAILURE REPORT ================" << endl;
95  cout << "Frame F = fwd(q) = " << F << endl;
96  cout << "original configuration " << kf.getConfigurationDescription(config) << endl;
97  cout << "Frame F2 = fwd(inv(F)) = " << F2 << endl;
98  cout << "configuration of fwd(inv(F)) = " << kf.getConfigurationDescription(config3) << endl;
99  cout << "Comparing q with q2=inv(fwd(q)) "<< endl;
100  for (int j=0;j<6;++j) {
101  std::cout << "q["<<j<<"]="<<q[j]<<" and q2["<<j<<"]="<<q2[j]<<std::endl;
102  }
103  }
104  }
105  if (exitflag) return exit(-1);
106  delete jnt2cartpos2;
107  delete kf2;
108  delete cartpos2jnt;
109  delete jnt2cartpos;
110 }
111 
112 
113 
114 
115 
116 int main(int argc,char* argv[]) {
117  test_zxxzxz(0.33,0.305,0.33,0.176);
118  test_zxxzxz(0.1,0.2,0.3,0.4);
119  test_zxxzxz(0.5,0.3,0.2,0.5);
120  return 0;
121 }
122 
std::vector< double >
std::vector::size
T size(T... args)
std::cerr
iostream
KDL
Definition: kukaLWR_DHnew.cpp:25
std::cout
main
int main(int argc, char *argv[])
Definition: zxxzxztest.cpp:116
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
std::endl
T endl(T... args)
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
test_zxxzxz
void test_zxxzxz(double l1, double l2, double l3, double l6)
Definition: zxxzxztest.cpp:21