$search
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 // Twist 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 // Twist 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 }