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>
22 cout <<
"========================================================================================" << endl;
23 cout <<
" test_zxxzxz : forward and inverse position kinematics for all configurations" << endl;
24 cout <<
"========================================================================================" << endl;
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();
37 int config,config2,config3;
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;
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;
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;
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;
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;
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) {
105 if (exitflag)
return exit(-1);
116 int main(
int argc,
char* argv[]) {