orocos_kdl
jacobiantests.hpp
Go to the documentation of this file.
1 #ifndef PVTESTS_H
2 #define PVTESTS_H
3 
4 
5 
6 #include <kdl/jacobianexpr.hpp>
7 #include <kdl/test_macros.h>
8 #include <iostream>
9 
10 #include <string>
11 #include <iomanip>
12 
13 namespace KDL {
14 
15 template <typename T>
16 void random(Jacobian<T>& rv) {
17  random(rv.value());
18  for (int i=0;i<rv.nrOfDeriv();++i) {
19  random(rv.deriv(i));
20  }
21 }
22 
23 
24 template <typename T>
25 void posrandom(Jacobian<T>& rv) {
26  posrandom(rv.value());
27  for (int i=0;i<rv.nrOfDeriv();++i) {
28  posrandom(rv.deriv(i));
29  }
30 }
31 
32 
33 template <typename T>
34 inline void checkEqual(const T& a,const T& b,double eps) {
35  KDL_CTX;
36  KDL_DIFF(a,b);
37  assert(Equal(a,b,eps));
38 }
39 
40 
41 template <typename OpID,typename A>
42 class checkUnary {
43  typedef UnaryOp<OpID,A> myOp;
44 public:
45  inline static void check(void (*rnd)(Jacobian<A>&) = &random,double dt=1E-8,double eps=1E-4,int size=1) {
46  KDL_CTX;
47  Jacobian<A> a(size);
48  rnd(a);
49  KDL_ARG1(a);
50  int i;
51  for (i=0;i<a.nrOfDeriv();++i) {
52  checkEqual(
53  myOp::deriv(a.value(),a.deriv(i)),
54  diff(
55  myOp::value(a.value()),
56  myOp::value(
57  addDelta(a.value(),a.deriv(i),dt)
58  ),
59  dt),
60  eps
61  );
62  }
63  }
64 };
65 
66 
67 template <typename OpID,typename A>
68 class checkUnaryVel {
69  typedef UnaryOp<OpID,A> myOp;
70 
71 public:
72  inline static void check(void (*rnd)(Jacobian<A>&) = &random,double dt=1E-8,double eps=1E-4,int size=1) {
73  KDL_CTX;
74  Jacobian<A> a(size);
75  rnd(a);
76  KDL_ARG1(a);
77  int i;
78  for (i=0;i<a.nrOfDeriv();++i) {
79  KDL_MSG("testing value() components of deriv ");
80  checkEqual(
81  myOp::deriv(a.value(),a.deriv(i)).value(),
82  diff(
83  myOp::value(a.value()),
84  myOp::value(
85  addDelta(a.value(),a.deriv(i).value(),dt)
86  ),
87  dt),
88  eps
89  );
90  typename Traits<A>::derivType d1(
91  addDelta(a.deriv(i).value(), a.deriv(i).deriv(),dt));
92  typename Traits<A>::valueType a1(
93  addDelta(a.value(),a.deriv(i).value(),dt)
94  );
95  KDL_MSG("testing deriv() components of deriv ");
96  checkEqual(
97  myOp::deriv(a.value(),a.deriv(i)).deriv(),
98  diff(
99  myOp::deriv(a.value(),a.deriv(i)).value(),
100  myOp::deriv(a1, d1).value(),
101  dt),
102  eps
103  );
104  }
105  }
106 };
107 
108 template <typename OpID,typename A,typename B>
109 class checkBinary {
110  typedef BinaryOp<OpID,A,B> myOp;
111 public:
112  inline static void check(double dt=1E-8,double eps=1E-4,int size=1) {
113  KDL_CTX;
114  Jacobian<A> a(size);
115  random(a);
116  Jacobian<B> b(size);
117  random(b);
118  KDL_ARG2(a,b);
119  int i;
120  for (i=0;i<a.nrOfDeriv();++i) {
121  checkEqual(
122  myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)),
123  diff(
124  myOp::value(a.value(),b.value()),
125  myOp::value(
126  addDelta(a.value(),a.deriv(i),dt),
127  addDelta(b.value(),b.deriv(i),dt)
128  ),
129  dt),
130  eps
131  );
132  checkEqual(
133  myOp::derivVC(a.value(),a.deriv(i),b.value()),
134  diff(
135  myOp::value(a.value(),b.value()),
136  myOp::value(
137  addDelta(a.value(),a.deriv(i),dt),
138  b.value()
139  ),
140  dt),
141  eps
142  );
143  checkEqual(
144  myOp::derivCV(a.value(),b.value(),b.deriv(i)),
145  diff(
146  myOp::value(a.value(),b.value()),
147  myOp::value(
148  a.value(),
149  addDelta(b.value(),b.deriv(i),dt)
150  ),
151  dt),
152  eps
153  );
154 
155  }
156  }
157 };
158 
159 
160 template <typename OpID,typename A,typename B>
161 class checkBinary_displ {
162  typedef BinaryOp<OpID,A,B> myOp;
163 public:
164  inline static void check(double dt=1E-8,double eps=1E-4,int size=1) {
165  KDL_CTX;
166  Jacobian<A> a(size);
167  random(a);
168  Jacobian<B> b(size);
169  random(b);
170  KDL_ARG2(a,b);
171  int i;
172  for (i=0;i<a.nrOfDeriv();++i) {
173  checkEqual(
174  myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)),
175  diff_displ(
176  myOp::value(a.value(),b.value()),
177  myOp::value(
178  addDelta(a.value(),a.deriv(i),dt),
179  addDelta(b.value(),b.deriv(i),dt)
180  ),
181  dt),
182  eps
183  );
184  checkEqual(
185  myOp::derivVC(a.value(),a.deriv(i),b.value()),
186  diff_displ(
187  myOp::value(a.value(),b.value()),
188  myOp::value(
189  addDelta(a.value(),a.deriv(i),dt),
190  b.value()
191  ),
192  dt),
193  eps
194  );
195  checkEqual(
196  myOp::derivCV(a.value(),b.value(),b.deriv(i)),
197  diff_displ(
198  myOp::value(a.value(),b.value()),
199  myOp::value(
200  a.value(),
201  addDelta(b.value(),b.deriv(i),dt)
202  ),
203  dt),
204  eps
205  );
206 
207  }
208  }
209 };
210 
211 
212 template <typename OpID,typename A,typename B>
213 class checkBinaryVel {
214  typedef BinaryOp<OpID,A,B> myOp;
215 public:
216  inline static void check(double dt=1E-8,double eps=1E-4,int size=1) {
217  KDL_CTX;
218  Jacobian<A> a(size);
219  random(a);
220  Jacobian<B> b(size);
221  random(b);
222  KDL_ARG2(a,b);
223  int i;
224  for (i=0;i<a.nrOfDeriv();++i) {
225  //A Avalue = A(a.value(),a.deriv(i).value());
226  //B Bvalue = B(b.value(),b.deriv(i).value());
227  KDL_MSG("testing value() component of derivVV ");
228  checkEqual(
229  myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).value(),
230  diff(
231  myOp::value(a.value(),b.value()),
232  myOp::value(
233  addDelta(a.value(),a.deriv(i).value(),dt),
234  addDelta(b.value(),b.deriv(i).value(),dt)
235  ),
236  dt),
237  eps
238  );
239  typename Traits<A>::derivType da1(
240  addDelta(a.deriv(i).value(), a.deriv(i).deriv(),dt));
241  typename Traits<A>::valueType a1(
242  addDelta(a.value(),a.deriv(i).value(),dt)
243  );
244  typename Traits<B>::derivType db1(
245  addDelta(b.deriv(i).value(), b.deriv(i).deriv(),dt));
246  typename Traits<B>::valueType b1(
247  addDelta(b.value(),b.deriv(i).value(),dt)
248  );
249 
250  KDL_MSG("testing deriv() components of derivVV ");
251  checkEqual(
252  myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).deriv(),
253  diff(
254  myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).value(),
255  myOp::derivVV(a1, da1,b1,db1).value(),
256  dt),
257  eps
258  );
259  KDL_MSG("testing deriv() components of derivVC ");
260  checkEqual(
261  myOp::derivVC(a.value(),a.deriv(i),b.value()).deriv(),
262  diff(
263  myOp::derivVC(a.value(),a.deriv(i),b.value()).value(),
264  myOp::derivVC(a1, da1,b.value()).value(),
265  dt),
266  eps
267  );
268  KDL_MSG("testing deriv() components of derivCV ");
269  checkEqual(
270  myOp::derivCV(a.value(),b.value(),b.deriv(i)).deriv(),
271  diff(
272  myOp::derivCV(a.value(),b.value(),b.deriv(i)).value(),
273  myOp::derivCV(a.value(),b1,db1).value(),
274  dt),
275  eps
276  );
277 
278  KDL_MSG("testing value() components of derivVC ");
279  checkEqual(
280  myOp::derivVC(a.value(),a.deriv(i),b.value()).value(),
281  diff(
282  myOp::value(a.value(),b.value()),
283  myOp::value(
284  addDelta(a.value(),a.deriv(i).value(),dt),
285  b.value()
286  ),
287  dt),
288  eps
289  );
290  KDL_MSG("testing value() components of derivCV ");
291  checkEqual(
292  myOp::derivCV(a.value(),b.value(),b.deriv(i)).value(),
293  diff(
294  myOp::value(a.value(),b.value()),
295  myOp::value(
296  a.value(),
297  addDelta(b.value(),b.deriv(i).value(),dt)
298  ),
299  dt),
300  eps
301  );
302  }
303  }
304 };
305 
306 
307 } // namespace
308 #endif
309 
310 
311 
312 
Traits::valueType
T valueType
Definition: traits.h:39
Traits::derivType
T derivType
Definition: traits.h:40
KDL::checkBinary_displ::check
static void check(double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:164
KDL::checkBinary::check
static void check(double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:112
KDL::posrandom
IMETHOD void posrandom(doubleVel &F)
Definition: framevel.hpp:69
KDL::random
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:65
KDL::checkBinaryVel::myOp
BinaryOp< OpID, A, B > myOp
Definition: jacobiantests.hpp:214
KDL::checkBinary_displ::myOp
BinaryOp< OpID, A, B > myOp
Definition: jacobiantests.hpp:162
iostream
KDL::checkUnary
Definition: jacobiantests.hpp:42
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.
KDL
Definition: kukaLWR_DHnew.cpp:25
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::checkBinaryVel::check
static void check(double dt=1E-8, double eps=1E-4, int size=1)
Definition: jacobiantests.hpp:216
KDL::checkBinary
Definition: jacobiantests.hpp:109
KDL::checkUnaryVel::myOp
UnaryOp< OpID, A > myOp
Definition: jacobiantests.hpp:69
KDL::checkBinary_displ
Definition: jacobiantests.hpp:161
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
iomanip
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::checkBinaryVel
Definition: jacobiantests.hpp:213
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::checkUnaryVel
Definition: jacobiantests.hpp:68
KDL::checkEqual
void checkEqual(const T &a, const T &b, double eps)
Definition: jacobiantests.hpp:34
KDL::checkBinary::myOp
BinaryOp< OpID, A, B > myOp
Definition: jacobiantests.hpp:110
KDL::checkUnary::myOp
UnaryOp< OpID, A > myOp
Definition: jacobiantests.hpp:43
KDL::Jacobian
Definition: jacobian.hpp:36
string
Generated on Thu Nov 14 2024 04:37:48 for orocos_kdl by   doxygen 1.8.17