rframestest.cpp
Go to the documentation of this file.
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 }


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14