00001 #include <kdl/framevel.hpp>
00002 #include <kdl/frameacc.hpp>
00003 #include <kdl/framevel_io.hpp>
00004 #include <kdl/frameacc_io.hpp>
00005 #include <kdl/test_macros.h>
00006
00007
00008 using namespace KDL;
00009
00010 void TestVector() {
00011 KDL_CTX;
00012 Vector v(3,4,5);
00013 Vector v2;
00014 Vector vt(-5,-6,-3);
00015 KDL_DIFF( 2*v-v, v );
00016 KDL_DIFF( v*2-v, v );
00017 KDL_DIFF( v+v+v-2*v, v );
00018 v2=v;
00019 KDL_DIFF( v, v2 );
00020 v2+=v;
00021 KDL_DIFF( 2*v, v2 );
00022 v2-=v;
00023 KDL_DIFF( v,v2);
00024 v2.ReverseSign();
00025 KDL_DIFF( v,-v2);
00026 KDL_DIFF( v*vt,-vt*v);
00027 v2 = Vector(-5,-6,-3);
00028 KDL_DIFF( v*v2,-v2*v);
00029 }
00030
00031 void TestVectorVel() {
00032 KDL_CTX;
00033 VectorVel v(Vector(3,-4,5),Vector(6,3,-5));
00034 VectorVel v2;
00035 Vector vt(-4,-6,-8);
00036 KDL_DIFF( 2*v-v, v );
00037 KDL_DIFF( v*2-v, v );
00038 KDL_DIFF( v+v+v-2*v, v );
00039 v2=v;
00040 KDL_DIFF( v, v2 );
00041 v2+=v;
00042 KDL_DIFF( 2*v, v2 );
00043 v2-=v;
00044 KDL_DIFF( v,v2);
00045 v2.ReverseSign();
00046 KDL_DIFF( v,-v2);
00047 KDL_DIFF( v*vt,-vt*v);
00048 v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5));
00049 KDL_DIFF( v*v2,-v2*v);
00050 }
00051
00052 void TestVectorAcc() {
00053 KDL_CTX;
00054 VectorAcc v(Vector(3,-4,5),Vector(6,3,-5),Vector(-4,-3,-6));
00055 VectorAcc v2;
00056 Vector vt(-4,-6,-8);
00057 KDL_DIFF( 2*v-v, v );
00058 KDL_DIFF( v*2-v, v );
00059 KDL_DIFF( v+v+v-2*v, v );
00060 v2=v;
00061 KDL_DIFF( v, v2 );
00062 v2+=v;
00063 KDL_DIFF( 2*v, v2 );
00064 v2-=v;
00065 KDL_DIFF( v,v2);
00066 v2.ReverseSign();
00067 KDL_DIFF( v,-v2);
00068 KDL_DIFF( v*vt,-vt*v);
00069 v2 = VectorAcc(Vector(-5,-6,-3),Vector(-3,-4,-1),Vector(10,12,9));
00070 KDL_DIFF( v*v2,-v2*v);
00071 }
00072
00073 void TestRotation() {
00074 KDL_CTX;
00075 Vector v(Vector(9,4,-2));
00076 Vector v2;
00077 Vector vt(2,3,4);
00078 Rotation R;
00079 Rotation R2;
00080 double a;
00081 double b;
00082 double c;
00083 a= -15*deg2rad;
00084 b= 20*deg2rad;
00085 c= -80*deg2rad;
00086 R = Rotation(Rotation::RPY(a,b,c));
00087 R2=R;
00088 KDL_DIFF( R,R2 );
00089 KDL_DIFF( (R*v).Norm(), v.Norm() );
00090 KDL_DIFF( R.Inverse(R*v), v );
00091 KDL_DIFF( R*R.Inverse(v), v );
00092 KDL_DIFF( R*Rotation::Identity(), R );
00093 KDL_DIFF( Rotation::Identity()*R, R );
00094 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
00095 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
00096 KDL_DIFF( R*R.Inverse(), Rotation::Identity() );
00097 KDL_DIFF( R.Inverse()*R, Rotation::Identity() );
00098 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
00099 v2=v*v-2*v;
00100 KDL_DIFF( (v2).Norm(), ::sqrt(dot(v2,v2)) );
00101 }
00102
00103
00104 void TestRotationVel() {
00105 KDL_CTX;
00106 VectorVel v(Vector(9,4,-2),Vector(-5,6,-2));
00107 VectorVel v2;
00108 Vector vt(2,3,4);
00109 RotationVel R;
00110 RotationVel R2;
00111 double a;
00112 double b;
00113 double c;
00114 a= -15*deg2rad;
00115 b= 20*deg2rad;
00116 c= -80*deg2rad;
00117 R = RotationVel(Rotation::RPY(a,b,c),Vector(2,4,1));
00118 R2=R;
00119 KDL_DIFF( R,R2 );
00120 KDL_DIFF( (R*v).Norm(), v.Norm() );
00121 KDL_DIFF( R.Inverse(R*v), v );
00122 KDL_DIFF( R*R.Inverse(v), v );
00123 KDL_DIFF( R*Rotation::Identity(), R );
00124 KDL_DIFF( Rotation::Identity()*R, R );
00125 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
00126 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
00127 KDL_DIFF( R*R.Inverse(), RotationVel::Identity() );
00128 KDL_DIFF( R.Inverse()*R, RotationVel::Identity() );
00129 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
00130 v2=v*v-2*v;
00131 KDL_DIFF( (v2).Norm(), sqrt(dot(v2,v2)) );
00132 }
00133
00134 void TestRotationAcc() {
00135 KDL_CTX;
00136 VectorAcc v(Vector(9,4,-2),Vector(-5,6,-2),Vector(2,-3,-3));
00137 VectorAcc v2;
00138 Vector vt(2,3,4);
00139 RotationAcc R;
00140 RotationAcc R2;
00141 double a;
00142 double b;
00143 double c;
00144 a= -15*deg2rad;
00145 b= 20*deg2rad;
00146 c= -80*deg2rad;
00147 R = RotationAcc(Rotation::RPY(a,b,c),Vector(2,4,1),Vector(-3,-2,-1));
00148 R2=R;
00149 KDL_DIFF( R,R2 );
00150 KDL_DIFF( (R*v).Norm(), v.Norm() );
00151 KDL_DIFF( R.Inverse(R*v), v );
00152 KDL_DIFF( R*R.Inverse(v), v );
00153 KDL_DIFF( R*Rotation::Identity(), R );
00154 KDL_DIFF( Rotation::Identity()*R, R );
00155 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
00156 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
00157 KDL_DIFF( R*R.Inverse(), RotationAcc::Identity() );
00158 KDL_DIFF( R.Inverse()*R, RotationAcc::Identity() );
00159 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
00160 v2=v*v-2*v;
00161 KDL_DIFF( (v2).Norm(), sqrt(dot(v2,v2)) );
00162 }
00163
00164 void TestFrame() {
00165 KDL_CTX;
00166 Vector v(3,4,5);
00167 Vector vt(-1,0,-10);
00168 Rotation R;
00169 Frame F;
00170 Frame F2;
00171 F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1));
00172
00173 F2=F;
00174 KDL_DIFF( F, F2 );
00175 KDL_DIFF( F.Inverse(F*v), v );
00176 KDL_DIFF( F.Inverse(F*vt), vt );
00177 KDL_DIFF( F*F.Inverse(v), v );
00178 KDL_DIFF( F*F.Inverse(vt), vt );
00179 KDL_DIFF( F*Frame::Identity(), F );
00180 KDL_DIFF( Frame::Identity()*F, F );
00181 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
00182 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
00183 KDL_DIFF( F*F.Inverse(), Frame::Identity() );
00184 KDL_DIFF( F.Inverse()*F, Frame::Identity() );
00185 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
00186 }
00187
00188 void TestFrameVel() {
00189 KDL_CTX;
00190 VectorVel v(Vector(3,4,5),Vector(-2,-4,-1));
00191 Vector vt(-1,0,-10);
00192 RotationVel R;
00193 FrameVel F;
00194 FrameVel F2; ;
00195 F = FrameVel(
00196 Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)),
00197 Twist(Vector(2,-2,-2),Vector(-5,-3,-2))
00198 );
00199 F2=F;
00200 KDL_DIFF( F, F2 );
00201 KDL_DIFF( F.Inverse(F*v), v );
00202 KDL_DIFF( F.Inverse(F*vt), vt );
00203 KDL_DIFF( F*F.Inverse(v), v );
00204 KDL_DIFF( F*F.Inverse(vt), vt );
00205 KDL_DIFF( F*Frame::Identity(), F );
00206 KDL_DIFF( Frame::Identity()*F, F );
00207 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
00208 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
00209 KDL_DIFF( F*F.Inverse(), FrameVel::Identity() );
00210 KDL_DIFF( F.Inverse()*F, Frame::Identity() );
00211 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
00212 }
00213
00214 void TestFrameAcc() {
00215 KDL_CTX;
00216 VectorAcc v(Vector(3,4,5),Vector(-2,-4,-1),Vector(6,4,2));
00217 Vector vt(-1,0,-10);
00218 RotationAcc R;
00219 FrameAcc F;
00220 FrameAcc F2;
00221 F = FrameAcc(
00222 Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)),
00223 Twist(Vector(2,-2,-2),Vector(-5,-3,-2)),
00224 Twist(Vector(5,6,2),Vector(-2,-3,1))
00225 );
00226 F2=F;
00227 KDL_DIFF( F, F2 );
00228 KDL_DIFF( F.Inverse(F*v), v );
00229 KDL_DIFF( F.Inverse(F*vt), vt );
00230 KDL_DIFF( F*F.Inverse(v), v );
00231 KDL_DIFF( F*F.Inverse(vt), vt );
00232 KDL_DIFF( F*Frame::Identity(), F );
00233 KDL_DIFF( Frame::Identity()*F, F );
00234 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
00235 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
00236 KDL_DIFF( F*F.Inverse(), FrameAcc::Identity() );
00237 KDL_DIFF( F.Inverse()*F, Frame::Identity() );
00238 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
00239 }
00240
00241 void TestTwistVel() {
00242 KDL_CTX;
00243
00244 TwistVel t(VectorVel(
00245 Vector(6,3,5),
00246 Vector(1,4,2)
00247 ),VectorVel(
00248 Vector(4,-2,7),
00249 Vector(-1,-2,-3)
00250 )
00251 );
00252 TwistVel t2;
00253 RotationVel R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad),Vector(-1,5,3));
00254 FrameVel F = FrameVel(
00255 Frame(
00256 Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad),
00257 Vector(4,-2,1)
00258 ),
00259 Twist(
00260 Vector(2,-2,-2),
00261 Vector(-5,-3,-2)
00262 )
00263 );
00264
00265 KDL_DIFF(2.0*t-t,t);
00266 KDL_DIFF(t*2.0-t,t);
00267 KDL_DIFF(t+t+t-2.0*t,t);
00268 t2=t;
00269 KDL_DIFF(t,t2);
00270 t2+=t;
00271 KDL_DIFF(2.0*t,t2);
00272 t2-=t;
00273 KDL_DIFF(t,t2);
00274 t.ReverseSign();
00275 KDL_DIFF(t,-t2);
00276 KDL_DIFF(R.Inverse(R*t),t);
00277 KDL_DIFF(R*t,R*R.Inverse(R*t));
00278 KDL_DIFF(F.Inverse(F*t),t);
00279 KDL_DIFF(F*t,F*F.Inverse(F*t));
00280 KDL_DIFF(doubleVel(3.14,2)*t,t*doubleVel(3.14,2));
00281 KDL_DIFF(t/doubleVel(3.14,2),t*(1.0/doubleVel(3.14,2)));
00282 KDL_DIFF(t/3.14,t*(1.0/3.14));
00283 KDL_DIFF(-t,-1.0*t);
00284 VectorVel p1(Vector(5,1,2),Vector(4,2,1)) ;
00285 VectorVel p2(Vector(2,0,5),Vector(-2,7,-1)) ;
00286 KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
00287 KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
00288 }
00289
00290 void TestTwistAcc() {
00291 KDL_CTX;
00292
00293 TwistAcc t( VectorAcc(Vector(6,3,5),Vector(1,4,2),Vector(5,2,1)),
00294 VectorAcc(Vector(4,-2,7),Vector(-1,-2,-3),Vector(5,2,9) )
00295 );
00296 TwistAcc t2;
00297 RotationAcc R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad),
00298 Vector(-1,5,3),
00299 Vector(2,1,3)
00300 ) ;
00301 FrameAcc F = FrameAcc(
00302 Frame(Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad),Vector(4,-2,1)),
00303 Twist(Vector(2,-2,-2),Vector(-5,-3,-2)),
00304 Twist(Vector(5,4,-5),Vector(12,13,17))
00305 );
00306
00307 KDL_DIFF(2.0*t-t,t);
00308 KDL_DIFF(t*2.0-t,t);
00309 KDL_DIFF(t+t+t-2.0*t,t);
00310 t2=t;
00311 KDL_DIFF(t,t2);
00312 t2+=t;
00313 KDL_DIFF(2.0*t,t2);
00314 t2-=t;
00315 KDL_DIFF(t,t2);
00316 t.ReverseSign();
00317 KDL_DIFF(t,-t2);
00318 KDL_DIFF(R.Inverse(R*t),t);
00319 KDL_DIFF(R*t,R*R.Inverse(R*t));
00320 KDL_DIFF(F.Inverse(F*t),t);
00321 KDL_DIFF(F*t,F*F.Inverse(F*t));
00322 KDL_DIFF(doubleAcc(3.14,2,3)*t,t*doubleAcc(3.14,2,3));
00323 KDL_DIFF(t/doubleAcc(3.14,2,7),t*(1.0/doubleAcc(3.14,2,7)));
00324 KDL_DIFF(t/3.14,t*(1.0/3.14));
00325 KDL_DIFF(-t,-1.0*t);
00326 VectorAcc p1(Vector(5,1,2),Vector(4,2,1),Vector(2,1,3));
00327 VectorAcc p2(Vector(2,0,5),Vector(-2,7,-1),Vector(-3,2,-1));
00328 KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
00329 KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
00330 }
00331
00332 int main() {
00333 KDL_CTX;
00334 TestVector();
00335 TestRotation();
00336 TestFrame();
00337 TestVectorVel();
00338 TestRotationVel();
00339 TestFrameVel();
00340 TestVectorAcc();
00341 TestRotationAcc();
00342 TestFrameAcc();
00343 TestTwistVel();
00344 TestTwistAcc();
00345 return 0;
00346 }