serialchaintest.cpp
Go to the documentation of this file.
1 #include <kdl/kinfam/serialchain.hpp>
2 #include <kdl/frames.hpp>
3 #include <kdl/framevel.hpp>
4 #include <kdl/frames_io.hpp>
5 #include <kdl/kinfam/crs450.hpp>
6 #include <kdl/kinfam/kuka160.hpp>
7 #include <kdl/kinfam/serialchaincartpos2jnt.hpp>
8 #include <kdl/kinfam/lineartransmission.hpp>
9 
10 using namespace KDL;
11 
12 
13 
14 void CompareFamilies(KinematicFamily* KF1,KinematicFamily* KF2) {
15  Jnt2CartPos* jnt2cartpos1 = KF1->createJnt2CartPos();
16  Jnt2CartPos* jnt2cartpos2 = KF2->createJnt2CartPos();
17  JointVector q(6);
18  q[0] = 0*KDL::deg2rad;
19  q[1] = 10*KDL::deg2rad;
20  q[2] = 20*KDL::deg2rad;
21  q[3] = 30*KDL::deg2rad;
22  q[4] = 40*KDL::deg2rad;
23  q[5] = 50*KDL::deg2rad;
24  Frame F1,F2;
25  jnt2cartpos1->evaluate(q);
26  jnt2cartpos1->getFrame(F1);
27  jnt2cartpos2->evaluate(q);
28  jnt2cartpos2->getFrame(F2);
29  if (!Equal(F1,F2,1E-7)) {
30  std::cout << "the two kinematic families do not give the same result." << std::endl;
31  std::cout << "Result of first kinematic family " << std::endl;
32  std::cout << F1 << std::endl;
33  std::cout << "Result of second kinematic family " << std::endl;
34  std::cout << F2 << std::endl;
35  exit(1);
36  }
37  delete jnt2cartpos1;
38  delete jnt2cartpos2;
39 }
40 
41 //
42 // Test whether Jnt2CartPos and CartPos2Jnt give a consistent result.
43 //
44 class TestForwardAndInverse {
45  KinematicFamily* family;
46  Jnt2CartPos* jnt2cartpos;
47  Frame F_base_ee;
48  Frame F_base_ee2;
49  JointVector q_solved;
50  JointVector q_initial;
51 public:
52  CartPos2Jnt* cartpos2jnt;
53 public:
54  static void TestFamily(KinematicFamily* _family) {
55  JointVector q_initial(6);
56  q_initial[0] = 0*KDL::deg2rad;
57  q_initial[1] = 0*KDL::deg2rad;
58  q_initial[2] = 90*KDL::deg2rad;
59  q_initial[3] = 0*KDL::deg2rad;
60  q_initial[4] = 90*KDL::deg2rad;
61  q_initial[5] = 0*KDL::deg2rad;
62  TestForwardAndInverse testobj(_family,q_initial);
63  JointVector q(6);
64  q[0] = 0*KDL::deg2rad;
65  q[1] = 10*KDL::deg2rad;
66  q[2] = 20*KDL::deg2rad;
67  q[3] = 30*KDL::deg2rad;
68  q[4] = 40*KDL::deg2rad;
69  q[5] = 50*KDL::deg2rad;
70  testobj.test(q);
71  //std::cout << "number of iterations " << ((SerialChainCartPos2Jnt*)testobj.cartpos2jnt)->iter << std::endl;
72  q[0] = -10*KDL::deg2rad;
73  q[1] = -10*KDL::deg2rad;
74  q[2] = 40*KDL::deg2rad;
75  q[3] = -30*KDL::deg2rad;
76  q[4] = 20*KDL::deg2rad;
77  q[5] = 60*KDL::deg2rad;
78  testobj.test(q);
79  //std::cout << "number of iterations " << ((SerialChainCartPos2Jnt*)testobj.cartpos2jnt)->iter << std::endl;
80  }
81 
82 
83 //
84 // Test whether Jnt2CartPos and Jnt2Jac give consistent
85 // results.
86 //
87 class TestForwardPosAndJac {
88  KinematicFamily* family;
89  Jnt2CartPos* jnt2cartpos;
90  Jnt2Jac* jnt2jac;
91  Jacobian<Frame> FJ_base_ee;
92  Frame F_base_ee1;
93  Frame F_base_ee2;
94 public:
95  static void TestFamily(KinematicFamily* _family) {
96  TestForwardPosAndJac testobj(_family);
97  JointVector q(6);
98  q[0] = 0*KDL::deg2rad;
99  q[1] = 10*KDL::deg2rad;
100  q[2] = 20*KDL::deg2rad;
101  q[3] = 30*KDL::deg2rad;
102  q[4] = 40*KDL::deg2rad;
103  q[5] = 50*KDL::deg2rad;
104  testobj.test(q);
105  q[0] = -50*KDL::deg2rad;
106  q[1] = -10*KDL::deg2rad;
107  q[2] = 20*KDL::deg2rad;
108  q[3] = -30*KDL::deg2rad;
109  q[4] = 20*KDL::deg2rad;
110  q[5] = 110*KDL::deg2rad;
111  testobj.test(q);
112  }
113 
114  TestForwardPosAndJac(KinematicFamily* _family) :
115  family(_family),
116  jnt2cartpos(_family->createJnt2CartPos()),
117  jnt2jac(_family->createJnt2Jac()),
118  FJ_base_ee(_family->nrOfJoints())
119  {
120  // the transformations should exist :
121  assert( jnt2jac != 0);
122  assert( jnt2cartpos != 0);
123  }
124 
125  int test(JointVector& q) {
126  double deltaq = 1E-4;
127  double epsJ = 1E-4;
128  if (jnt2jac->evaluate(q)!=0) return 1;
129  jnt2jac->getJacobian(FJ_base_ee);
130  for (int i=0; i< q.size() ;i++) {
131  // test the derivative of J towards qi
132  double oldqi = q[i];
133  q[i] = oldqi+deltaq;
134  if (jnt2cartpos->evaluate(q)!=0) return 1;
135  jnt2cartpos->getFrame(F_base_ee2);
136  q[i] = oldqi-deltaq;
137  if (jnt2cartpos->evaluate(q)!=0) return 1;
138  jnt2cartpos->getFrame(F_base_ee1);
139  q[i] = oldqi;
140  // check Jacobian :
141  Twist Jcol = diff(F_base_ee1,F_base_ee2,2*deltaq);
142  if (!Equal(Jcol,FJ_base_ee.deriv(i),epsJ)) {
143  std::cout << "Difference between symbolic and numeric calculation of Jacobian for column "
144  << i << std::endl;
145  std::cout << "Numeric " << Jcol << std::endl;
146  std::cout << "Symbolic " << FJ_base_ee.deriv(i) << std::endl;
147  exit(1);
148  }
149  }
150  }
151 
152  ~TestForwardPosAndJac() {
153  delete jnt2cartpos;
154  delete jnt2jac;
155  }
156 };
157 
158 
159 //
160 // Test whether Jnt2CartVel and Jnt2Jac give consistent
161 // results.
162 //
163 class TestCartVelAndJac {
164  KinematicFamily* family;
165  Jnt2CartVel* jnt2cartvel;
166  Jnt2Jac* jnt2jac;
167  Jacobian<Frame> FJ_base_ee;
168  FrameVel F_base_ee;
169 public:
170  static void TestFamily(KinematicFamily* _family) {
171  TestCartVelAndJac testobj(_family);
172  JointVector qdot(6);
173  qdot[0] = 0.1;
174  qdot[1] = 0.2;
175  qdot[2] = -0.3;
176  qdot[3] = 0.4;
177  qdot[4] = -0.5;
178  qdot[5] = 0.6;
179  JointVector q(6);
180  q[0] = 0*KDL::deg2rad;
181  q[1] = 10*KDL::deg2rad;
182  q[2] = 20*KDL::deg2rad;
183  q[3] = 30*KDL::deg2rad;
184  q[4] = 40*KDL::deg2rad;
185  q[5] = 50*KDL::deg2rad;
186  std::cout << "q[1] = " << q[1] << std::endl;
187  testobj.test(q,qdot);
188  q[0] = -50*KDL::deg2rad;
189  q[1] = -10*KDL::deg2rad;
190  q[2] = 20*KDL::deg2rad;
191  q[3] = -30*KDL::deg2rad;
192  q[4] = 20*KDL::deg2rad;
193  q[5] = 110*KDL::deg2rad;
194  testobj.test(q,qdot);
195  }
196 
197  TestCartVelAndJac(KinematicFamily* _family) :
198  family(_family),
199  jnt2cartvel(_family->createJnt2CartVel()),
200  jnt2jac(_family->createJnt2Jac()),
201  FJ_base_ee(_family->nrOfJoints())
202  {
203  // the transformations should exist :
204  assert( jnt2jac != 0);
205  assert( jnt2cartvel != 0);
206  }
207 
208  int test(JointVector& q,JointVector& qdot) {
209  std::cout << "Testing whether Jnt2CartVel and Jnt2Jac are consistent " << std::endl;
210  std::cout << "q[1] = " << q[1] << std::endl;
211  double deltaq = 1E-4;
212  double epsJ = 1E-4;
213  int result;
214  result = jnt2jac->evaluate(q);
215  assert(result==0);
216  jnt2jac->getJacobian(FJ_base_ee);
217  result = jnt2cartvel->evaluate(q,qdot);
218  jnt2cartvel->getFrameVel(F_base_ee);
219  assert(result==0);
220  Twist t = Twist::Zero();
221  for (int i=0; i< q.size() ;i++) {
222  t += FJ_base_ee.deriv(i)*qdot[i];
223  }
224  if (!Equal(t,F_base_ee.GetTwist(),1E-6)) {
225  std::cout << "Difference between the resuls"<< std::endl;
226  std::cout << "via the Jacobian " << t << std::endl;
227  std::cout << "via the jnt2cartvel transformation " << F_base_ee.GetTwist() << std::endl;
228  exit(1);
229  }
230  }
231 
232  ~TestCartVelAndJac() {
233  delete jnt2cartvel;
234  delete jnt2jac;
235  }
236 };
237 
238 
239 
240 //
241 // Test whether Jnt2CartVel and CartVel2Jnt give consistent
242 // results.
243 //
244 class TestCartVelAndInverse {
245  KinematicFamily* family;
246  Jnt2CartVel* jnt2cartvel;
247  CartVel2Jnt* cartvel2jnt;
248  FrameVel F_base_ee;
249  JointVector qdot2;
250 public:
251  static void TestFamily(KinematicFamily* _family) {
252  TestCartVelAndInverse testobj(_family);
253  JointVector qdot(6);
254  qdot[0] = 0.1;
255  qdot[1] = 0.2;
256  qdot[2] = -0.3;
257  qdot[3] = 0.4;
258  qdot[4] = -0.5;
259  qdot[5] = 0.6;
260  JointVector q(6);
261  q[0] = 0*KDL::deg2rad;
262  q[1] = 10*KDL::deg2rad;
263  q[2] = 20*KDL::deg2rad;
264  q[3] = 30*KDL::deg2rad;
265  q[4] = 40*KDL::deg2rad;
266  q[5] = 50*KDL::deg2rad;
267  testobj.test(q,qdot);
268  q[0] = -50*KDL::deg2rad;
269  q[1] = -10*KDL::deg2rad;
270  q[2] = 20*KDL::deg2rad;
271  q[3] = -30*KDL::deg2rad;
272  q[4] = 20*KDL::deg2rad;
273  q[5] = 110*KDL::deg2rad;
274  testobj.test(q,qdot);
275  }
276 
277  TestCartVelAndInverse(KinematicFamily* _family) :
278  family(_family),
279  jnt2cartvel(_family->createJnt2CartVel()),
280  cartvel2jnt(_family->createCartVel2Jnt()),
281  qdot2(_family->nrOfJoints())
282  {
283  // the transformations should exist :
284  assert( cartvel2jnt != 0);
285  assert( jnt2cartvel != 0);
286  }
287 
288  int test(const JointVector& q,const JointVector& qdot) {
289  std::cout << "Testing whether Jnt2CartVel and CartVel2Jnt are consistent " << std::endl;
290  double epsJ = 1E-7;
291  int result;
292 
293  result = jnt2cartvel->evaluate(q,qdot);
294  assert(result==0);
295  jnt2cartvel->getFrameVel(F_base_ee);
296 
297  cartvel2jnt->setTwist(F_base_ee.GetTwist());
298  result = cartvel2jnt->evaluate(q, qdot2);
299  assert(result==0);
300 
301  for (int i=0;i<qdot.size();++i) {
302  if (fabs(qdot[i]-qdot2[i])>epsJ) {
303  std::cout << " original joint velocities and calculated joint velocities do not match" << std::endl;
304  //std::cerr << " original joint velocities and calculated joint velocities do not match" << std::endl;
305  for (int j=0;j<qdot.size();j++) {
306  std::cout << "qdot["<<j<<"]="<<qdot[j]<<" and qdot2["<<j<<"]="<<qdot2[j] << std::endl;
307  }
308  std::cout << "Frame : " << F_base_ee.GetFrame() << std::endl;
309  std::cout << "Twist : " << F_base_ee.GetTwist() << std::endl;
310  exit(1);
311  }
312  }
313  }
314 
315  ~TestCartVelAndInverse() {
316  delete jnt2cartvel;
317  delete cartvel2jnt;
318  }
319 };
320 
321 
322 
323 
324 
325 
326 
327 
331 class CRS450_exp: public SerialChain {
332  public:
333  explicit CRS450_exp(int jointoffset=0) :
334  SerialChain("CRS450", 6, jointoffset,new LinearTransmission(6) )
335  {
336  double L1 = 0.33;
337  double L2 = 0.305;
338  double L3 = 0.33;
339  double L4 = 0.176;
340  LinearTransmission* tr = (LinearTransmission*)transmission;
341  tr->setTransmission(2,2.0,1.0);
342  tr->setTransmission(3,3.0,2.0);
343  addJoint(new JointRotZ(Frame::Identity())); // j1
344  addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L1))));// j2
345  addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L2))));// j3
346  addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0)))); // j4
347  addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L3))));// j5
348  addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0)))); // j6
349  setLastJointToEE(Frame(Rotation::Identity(),Vector(0,0,L4)));
350  };
351 };
352 
353 
354 int main(int argc,char* argv[]) {
355  KinematicFamily* family1,*family2,*family3,*family4;
356  std::cout << std::endl << "Tests on CRS450 " << std::endl;
357  family1 = new CRS450();
358  TestForwardAndInverse::TestFamily(family1);
359  TestForwardPosAndJac::TestFamily(family1);
360  TestCartVelAndJac::TestFamily(family1);
361  TestCartVelAndInverse::TestFamily(family1);//
362  std::cout <<std::endl << "Tests on CRS450_exp " << std::endl;
363  family2 = new CRS450_exp();
364  TestForwardAndInverse::TestFamily(family2);
365  TestForwardPosAndJac::TestFamily(family2);
366  TestCartVelAndJac::TestFamily(family2);
367  TestCartVelAndInverse::TestFamily(family2);//
368  std::cout << std::endl << "Tests on CRS450Feath " << std::endl;
369  family3 = new CRS450Feath();
370  TestForwardAndInverse::TestFamily(family3);
371  TestForwardPosAndJac::TestFamily(family3);
372  TestCartVelAndJac::TestFamily(family3);
373  TestCartVelAndInverse::TestFamily(family3);//
374  std::cout << std::endl << "Comparing the kinematic families "<< std::endl;
375  CompareFamilies(family1,family3);
376  std::cout << std::endl << "Tests on CRS450Feath->createSerialChain " << std::endl;
377  family4 = ((ZXXZXZ*)family3)->createSerialChain();
378  TestForwardAndInverse::TestFamily(family4);
379  TestForwardPosAndJac::TestFamily(family4);
380  TestCartVelAndJac::TestFamily(family4);
381  TestCartVelAndInverse::TestFamily(family4);//
382 
383 
384 
385  // testing the clone functionality (valgrind)
386  KinematicFamily* family1b,*family2b,*family3b,*family4b;
387  family1b = family1->clone();
388  family2b = family2->clone();
389  family3b = family3->clone();
390  family4b = family4->clone();
391 
392  delete family4;
393  delete family3;
394  delete family2;
395  delete family1;
396 
397  delete family4b;
398  delete family3b;
399  delete family2b;
400  delete family1b;
401 
402  return 0;
403 }
404 
KDL::L3
static const double L3
Definition: jacobiandottest.cpp:15
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
CompareFamilies
void CompareFamilies(KinematicFamily *KF1, KinematicFamily *KF2)
Definition: serialchaintest.cpp:14
KDL::Frame::Identity
static Frame Identity()
Definition: frames.hpp:701
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::L2
static const double L2
Definition: jacobiandottest.cpp:14
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
main
int main(int argc, char *argv[])
Definition: chainiksolverpos_lma_demo.cpp:169
KDL::L4
static const double L4
Definition: jacobiandottest.cpp:16
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::FrameVel::GetTwist
IMETHOD Twist GetTwist() const
Definition: framevel.hpp:125
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.hpp:553
KDL::L1
static const double L1
Definition: jacobiandottest.cpp:13
KDL::deg2rad
const double deg2rad
the value pi/180
Definition: utility.cxx:19
KDL::Twist::Zero
static Twist Zero()
Definition: frames.hpp:291
KDL::FrameVel::GetFrame
IMETHOD Frame GetFrame() const
Definition: framevel.hpp:121

orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14