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


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44