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> 16 cout <<
"========================================================================================" << endl;
17 cout <<
" test_zxxzxz : forward and inverse position kinematics for all configurations" << endl;
18 cout <<
"========================================================================================" << endl;
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();
28 std::vector<double> q(6);
29 std::vector<double> q2(6);
31 int config,config2,config3;
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;
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;
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;
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;
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;
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;
99 if (exitflag)
return exit(-1);
110 int main(
int argc,
char* argv[]) {
void test_zxxzxz(double l1, double l2, double l3, double l6)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
represents a frame transformation in 3D space (rotation + translation)
int main(int argc, char *argv[])