1 #include <kdl/framevel.hpp>
2 #include <kdl/frameacc.hpp>
3 #include <kdl/framevel_io.hpp>
4 #include <kdl/frameacc_io.hpp>
5 #include <kdl/test_macros.h>
17 KDL_DIFF( v+v+v-2*v, v );
26 KDL_DIFF( v*vt,-vt*v);
28 KDL_DIFF( v*v2,-v2*v);
38 KDL_DIFF( v+v+v-2*v, v );
47 KDL_DIFF( v*vt,-vt*v);
49 KDL_DIFF( v*v2,-v2*v);
59 KDL_DIFF( v+v+v-2*v, v );
68 KDL_DIFF( v*vt,-vt*v);
70 KDL_DIFF( v*v2,-v2*v);
89 KDL_DIFF( (R*v).
Norm(), v.Norm() );
94 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
95 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
98 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
120 KDL_DIFF( (R*v).
Norm(), v.Norm() );
125 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
126 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
129 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
150 KDL_DIFF( (R*v).
Norm(), v.Norm() );
155 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
156 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
159 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
176 KDL_DIFF( F.
Inverse(F*vt), vt );
178 KDL_DIFF( F*F.
Inverse(vt), vt );
181 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
182 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
185 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
202 KDL_DIFF( F.
Inverse(F*vt), vt );
204 KDL_DIFF( F*F.
Inverse(vt), vt );
207 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
208 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
211 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
229 KDL_DIFF( F.
Inverse(F*vt), vt );
231 KDL_DIFF( F*F.
Inverse(vt), vt );
234 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
235 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
238 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
267 KDL_DIFF(t+t+t-2.0*t,t);
277 KDL_DIFF(R*t,R*R.
Inverse(R*t));
279 KDL_DIFF(F*t,F*F.
Inverse(F*t));
282 KDL_DIFF(t/3.14,t*(1.0/3.14));
286 KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
287 KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
309 KDL_DIFF(t+t+t-2.0*t,t);
319 KDL_DIFF(R*t,R*R.
Inverse(R*t));
321 KDL_DIFF(F*t,F*F.
Inverse(F*t));
324 KDL_DIFF(t/3.14,t*(1.0/3.14));
328 KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
329 KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));