6 #include <kdl/jacobianexpr.hpp> 7 #include <kdl/test_macros.h> 18 for (
int i=0;i<rv.nrOfDeriv();++i) {
27 for (
int i=0;i<rv.nrOfDeriv();++i) {
34 inline void checkEqual(
const T& a,
const T& b,
double eps) {
37 assert(
Equal(a,b,eps));
41 template <
typename OpID,
typename A>
43 typedef UnaryOp<OpID,A>
myOp;
51 for (i=0;i<a.nrOfDeriv();++i) {
53 myOp::deriv(a.value(),a.deriv(i)),
55 myOp::value(a.value()),
67 template <
typename OpID,
typename A>
69 typedef UnaryOp<OpID,A>
myOp;
78 for (i=0;i<a.nrOfDeriv();++i) {
79 KDL_MSG(
"testing value() components of deriv ");
81 myOp::deriv(a.value(),a.deriv(i)).value(),
83 myOp::value(a.value()),
85 addDelta(a.value(),a.deriv(i).value(),dt)
91 addDelta(a.deriv(i).value(), a.deriv(i).deriv(),dt));
93 addDelta(a.value(),a.deriv(i).value(),dt)
95 KDL_MSG(
"testing deriv() components of deriv ");
97 myOp::deriv(a.value(),a.deriv(i)).deriv(),
99 myOp::deriv(a.value(),a.deriv(i)).value(),
100 myOp::deriv(a1, d1).value(),
108 template <
typename OpID,
typename A,
typename B>
110 typedef BinaryOp<OpID,A,B>
myOp;
112 inline static void check(
double dt=1E-8,
double eps=1E-4,
int size=1) {
120 for (i=0;i<a.nrOfDeriv();++i) {
122 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)),
124 myOp::value(a.value(),b.value()),
133 myOp::derivVC(a.value(),a.deriv(i),b.value()),
135 myOp::value(a.value(),b.value()),
144 myOp::derivCV(a.value(),b.value(),b.deriv(i)),
146 myOp::value(a.value(),b.value()),
160 template <
typename OpID,
typename A,
typename B>
162 typedef BinaryOp<OpID,A,B>
myOp;
164 inline static void check(
double dt=1E-8,
double eps=1E-4,
int size=1) {
172 for (i=0;i<a.nrOfDeriv();++i) {
174 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)),
176 myOp::value(a.value(),b.value()),
185 myOp::derivVC(a.value(),a.deriv(i),b.value()),
187 myOp::value(a.value(),b.value()),
196 myOp::derivCV(a.value(),b.value(),b.deriv(i)),
198 myOp::value(a.value(),b.value()),
212 template <
typename OpID,
typename A,
typename B>
214 typedef BinaryOp<OpID,A,B>
myOp;
216 inline static void check(
double dt=1E-8,
double eps=1E-4,
int size=1) {
224 for (i=0;i<a.nrOfDeriv();++i) {
227 KDL_MSG(
"testing value() component of derivVV ");
229 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).value(),
231 myOp::value(a.value(),b.value()),
233 addDelta(a.value(),a.deriv(i).value(),dt),
234 addDelta(b.value(),b.deriv(i).value(),dt)
240 addDelta(a.deriv(i).value(), a.deriv(i).deriv(),dt));
242 addDelta(a.value(),a.deriv(i).value(),dt)
245 addDelta(b.deriv(i).value(), b.deriv(i).deriv(),dt));
247 addDelta(b.value(),b.deriv(i).value(),dt)
250 KDL_MSG(
"testing deriv() components of derivVV ");
252 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).deriv(),
254 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).value(),
255 myOp::derivVV(a1, da1,b1,db1).value(),
259 KDL_MSG(
"testing deriv() components of derivVC ");
261 myOp::derivVC(a.value(),a.deriv(i),b.value()).deriv(),
263 myOp::derivVC(a.value(),a.deriv(i),b.value()).value(),
264 myOp::derivVC(a1, da1,b.value()).value(),
268 KDL_MSG(
"testing deriv() components of derivCV ");
270 myOp::derivCV(a.value(),b.value(),b.deriv(i)).deriv(),
272 myOp::derivCV(a.value(),b.value(),b.deriv(i)).value(),
273 myOp::derivCV(a.value(),b1,db1).value(),
278 KDL_MSG(
"testing value() components of derivVC ");
280 myOp::derivVC(a.value(),a.deriv(i),b.value()).value(),
282 myOp::value(a.value(),b.value()),
284 addDelta(a.value(),a.deriv(i).value(),dt),
290 KDL_MSG(
"testing value() components of derivCV ");
292 myOp::derivCV(a.value(),b.value(),b.deriv(i)).value(),
294 myOp::value(a.value(),b.value()),
297 addDelta(b.value(),b.deriv(i).value(),dt)
BinaryOp< OpID, A, B > myOp
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void checkEqual(const T &a, const T &b, double eps)
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
static void check(double dt=1E-8, double eps=1E-4, int size=1)
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
BinaryOp< OpID, A, B > myOp
static void check(double dt=1E-8, double eps=1E-4, int size=1)
IMETHOD void posrandom(Vector &a)
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.
BinaryOp< OpID, A, B > myOp
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
static void check(double dt=1E-8, double eps=1E-4, int size=1)