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(),
79  Rotation::EulerZYX(
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 
93  checkUnary<OpInverse,Frame>::check();
94  checkUnary<OpNegate,Vector>::check();
95  checkUnary<OpNorm,Vector>::check();
96  checkUnary<OpRotX,double>::check();
97  checkUnary<OpRotY,double>::check();
98  checkUnary<OpRotZ,double>::check();
99  checkUnary<OpUnitX,Rotation>::check();
100  checkUnary<OpUnitY,Rotation>::check();
101  checkUnary<OpUnitZ,Rotation>::check();
102  checkUnary<OpInverse,Rotation>::check();
103  checkUnary<OpNegate,Twist>::check();
104 
105  checkBinary<OpMult,Frame,Frame>::check();
106  checkBinary<OpMult,Frame,Vector>::check();
107  checkBinary<OpDot,Vector,Vector>::check();
108  checkBinary<OpMult,Vector,Vector>::check();
109  checkBinary<OpAdd,Vector,Vector>::check();
110  checkBinary<OpSub,Vector,Vector>::check();
111  checkBinary<OpMult,Rotation,Rotation>::check();
112  checkBinary<OpMult,Rotation,Vector>::check();
113  checkBinary<OpMult,Rotation,Twist>::check();
114  checkBinary<OpAdd,Twist,Twist>::check();
115  checkBinary<OpSub,Twist,Twist>::check();
116  checkBinary<OpMult,Twist,double>::check();
117  checkBinary<OpMult,double,Twist>::check();
118  checkBinary<OpRefPoint,Twist,Vector>::check();
119  checkBinary<OpAdd,Wrench,Wrench>::check();
120  checkBinary<OpSub,Wrench,Wrench>::check();
121  checkBinary<OpMult,Wrench,double>::check();
122  checkBinary<OpMult,double,Wrench>::check();
123  checkBinary<OpRefPoint,Wrench,Vector>::check();
124  checkBinary<OpDiff,Vector,Vector>::check();
125 
126  //checkBinary_displ<OpDiff,Rotation,Rotation>::check();
127 
128 
129  checkEulerZYX();
130 }
131 
132 void checkFrameVelOps() {
133  KDL_CTX;
134  checkDiffs();
135 
136  checkUnaryVel<OpNegate,VectorVel>::check();
137  checkUnaryVel<OpNorm,VectorVel>::check();
138  checkUnaryVel<OpInverse,FrameVel>::check();
139  checkUnaryVel<OpRotation,FrameVel>::check();
140  checkUnaryVel<OpOrigin,FrameVel>::check();
141  checkUnaryVel<OpUnitX,RotationVel>::check();
142  checkUnaryVel<OpUnitY,RotationVel>::check();
143  checkUnaryVel<OpUnitZ,RotationVel>::check();
144  checkUnaryVel<OpCoordX,VectorVel>::check();
145  checkUnaryVel<OpCoordY,VectorVel>::check();
146  checkUnaryVel<OpCoordZ,VectorVel>::check();
147  checkUnaryVel<OpRotation,FrameVel>::check();
148 
149  //checkUnary<OpRotX,double>::check();
150  //checkUnary<OpRotY,double>::check();
151  //checkUnary<OpRotZ,double>::check();
152  checkUnaryVel<OpInverse,RotationVel>::check();
153  checkUnaryVel<OpNegate,TwistVel>::check();
154 
155  checkBinaryVel<OpMult,FrameVel,FrameVel>::check();
156  checkBinaryVel<OpMult,FrameVel,VectorVel>::check();
157  checkBinaryVel<OpDot,VectorVel,VectorVel>::check();
158  checkBinaryVel<OpMult,VectorVel,VectorVel>::check();
159  checkBinaryVel<OpAdd,VectorVel,VectorVel>::check();
160  checkBinaryVel<OpSub,VectorVel,VectorVel>::check();
161  checkBinaryVel<OpMult,doubleVel,VectorVel>::check();
162  checkBinaryVel<OpMult,VectorVel,doubleVel>::check();
163  checkBinaryVel<OpMult,RotationVel,RotationVel>::check();
164  checkBinaryVel<OpMult,RotationVel,VectorVel>::check();
165  checkBinaryVel<OpMult,RotationVel,TwistVel>::check();
166  checkBinaryVel<OpAdd,TwistVel,TwistVel>::check();
167  checkBinaryVel<OpSub,TwistVel,TwistVel>::check();
168  checkBinaryVel<OpMult,TwistVel,doubleVel>::check();
169  checkBinaryVel<OpMult,doubleVel,TwistVel>::check();
170  checkBinaryVel<OpRefPoint,TwistVel,VectorVel>::check();
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(); */
177  checkBinaryVel<OpDiff,VectorVel,VectorVel>::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 
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1123
KDL::checkEqual
void checkEqual(const T &a, const T &b, double eps)
Definition: jacobiantests.hpp:34
KDL::checkFrameVelOps
void checkFrameVelOps()
Definition: jacobianframetests.cpp:132
KDL::checkEulerZYX
void checkEulerZYX()
Definition: jacobianframetests.cpp:50
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:469
KDL::checkUnary::check
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:45
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::checkDiffs
void checkDiffs()
Definition: jacobianframetests.cpp:13
KDL::checkUnaryVel::check
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:72
KDL::checkFrameOps
void checkFrameOps()
Definition: jacobianframetests.cpp:89
KDL::checkBinaryVel::check
static void check(double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:216
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
KDL::addDelta
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:1150
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
KDL::checkBinary::check
static void check(double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:112

orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36