jacobianframetests.cpp
Go to the documentation of this file.
1 #include <kdl/frames.hpp>
2 #include <kdl/frames_io.hpp>
3 #include <kdl/framevel.hpp>
4 #include <kdl/framevel_io.hpp>
5 #include <kdl/jacobianexpr.hpp>
6 #include <kdl/jacobianframe.hpp>
7 #include <kdl/jacobianframevel.hpp>
8 #include "jacobianframetests.hpp"
9 
10 namespace KDL {
11 
12 
13 void checkDiffs() {
14  KDL_CTX;
15  double adouble,bdouble;
16  Vector avector,bvector;
17  Twist atwist,btwist;
18  Rotation arot,brot;
19  Frame aframe,bframe;
20  // large deviations :
21  random(adouble);random(bdouble);
22  random(avector);random(bvector);
23  random(atwist);random(btwist);
24  random(arot);random(brot);
25  random(aframe);random(bframe);
26  double dt=0.1;
27  double eps = 1E-10;
28  checkEqual(bdouble,addDelta(adouble,diff(adouble,bdouble,dt),dt),eps);
29  checkEqual(bvector,addDelta(avector,diff(avector,bvector,dt),dt),eps);
30  checkEqual(btwist,addDelta(atwist,diff(atwist,btwist,dt),dt),eps);
31  checkEqual(brot,addDelta(arot,diff(arot,brot,dt),dt),eps);
32  checkEqual(bframe,addDelta(aframe,diff(aframe,bframe,dt),dt),eps);
33 
34  // small deviations
35  dt = 0.00001;
36  double ddouble;
37  Vector dvector;
38  Twist dtwist;
39  Vector drot; // there is no error in the naming ...
40  Twist dframe;
41  random(ddouble);random(dvector);random(dtwist);random(drot);random(dframe);
42  checkEqual(ddouble,diff(adouble,addDelta(adouble,ddouble,dt),dt),eps);
43  checkEqual(dvector,diff(avector,addDelta(avector,dvector,dt),dt),eps);
44  checkEqual(dtwist,diff(atwist,addDelta(atwist,dtwist,dt),dt),eps);
45  checkEqual(drot,diff(arot,addDelta(arot,drot,dt),dt),eps);
46  checkEqual(dframe,diff(aframe,addDelta(aframe,dframe,dt),dt),eps);
47 }
48 
49 
50 void checkEulerZYX() {
51  // Take care of the order of the arguments :
52  KDL_CTX;
53  int nrofcol=3;
54  Jacobian<Rotation> R(nrofcol);
55  Jacobian<Rotation> R2(nrofcol);
56  Jacobian<double> alpha(nrofcol);
57  random(alpha);
58  Jacobian<double> beta(nrofcol);
59  random(beta);
60  Jacobian<double> gamma(nrofcol);
61  random(gamma);
62  SetEulerZYX(gamma,beta,alpha,R);
63  // now we have a random frame R.
64  int result=GetEulerZYX(R,gamma,beta,alpha);
65  assert( result == 0);
66  SetEulerZYX(gamma,beta,alpha,R2);
67  checkEqual(R.value(),R2.value(),0.0001);
68  for (int i=0;i<nrofcol;i++) {
69  checkEqual(R.deriv(i),R2.deriv(i),0.0001);
70  }
71  double dt= 1E-8;
72  double eps = 1E-4;
73  std::cout << "Tests with numerical derivatives for EulerZYX " << std::endl;
74  for (int i=0;i<nrofcol;i++) {
75  Rotation R2 = Rotation::EulerZYX(alpha.value(),beta.value(),gamma.value());
76  checkEqual( R.deriv(i),
77  diff(
78  R.value(),
80  alpha.value()+ alpha.deriv(i)*dt,
81  beta.value() + beta.deriv(i)*dt,
82  gamma.value()+ gamma.deriv(i)*dt),
83  dt),
84  eps);
85  }
86 }
87 
88 
89 void checkFrameOps() {
90  KDL_CTX;
91  checkDiffs();
92 
104 
125 
126  //checkBinary_displ<OpDiff,Rotation,Rotation>::check();
127 
128 
129  checkEulerZYX();
130 }
131 
133  KDL_CTX;
134  checkDiffs();
135 
148 
149  //checkUnary<OpRotX,double>::check();
150  //checkUnary<OpRotY,double>::check();
151  //checkUnary<OpRotZ,double>::check();
154 
171  /*
172  checkBinary<OpAdd,Wrench,Wrench>::check();
173  checkBinary<OpSub,Wrench,Wrench>::check();
174  checkBinary<OpMult,Wrench,double>::check();
175  checkBinary<OpMult,double,Wrench>::check();
176  checkBinary<OpRefPoint,Wrench,Vector>::check(); */
178  //checkBinaryVel<OpDiff,RotationVel,RotationVel>::check(); WHY ?
179  //checkBinaryVel<OpDiff,FrameVel,FrameVel>::check(); WHY ?
180 
181 
182  //checkEulerZYX();
183 }
184 
185 
186 } // namespace ORO_Geometry
187 
188 
189 
190 
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
void checkEqual(const T &a, const T &b, double eps)
void checkFrameVelOps()
void checkEulerZYX()
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:469
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
represents both translational and rotational velocities.
Definition: frames.hpp:720
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
void checkDiffs()
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
void checkFrameOps()
static void check(double dt=1E-8, double eps=1E-4, int size=1)
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
Definition: frames.hpp:1157
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1215
static void check(double dt=1E-8, double eps=1E-4, int size=1)


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