serialchaintest.cpp
Go to the documentation of this file.
00001 #include <kdl/kinfam/serialchain.hpp>
00002 #include <kdl/frames.hpp>
00003 #include <kdl/framevel.hpp>
00004 #include <kdl/frames_io.hpp>
00005 #include <kdl/kinfam/crs450.hpp>
00006 #include <kdl/kinfam/kuka160.hpp>
00007 #include <kdl/kinfam/serialchaincartpos2jnt.hpp>
00008 #include <kdl/kinfam/lineartransmission.hpp>
00009 
00010 using namespace KDL;
00011 
00012 
00013 
00014 void CompareFamilies(KinematicFamily* KF1,KinematicFamily* KF2) {
00015         Jnt2CartPos* jnt2cartpos1 = KF1->createJnt2CartPos();
00016         Jnt2CartPos* jnt2cartpos2 = KF2->createJnt2CartPos();
00017         JointVector q(6);
00018         q[0] = 0*KDL::deg2rad;
00019         q[1] = 10*KDL::deg2rad;
00020         q[2] = 20*KDL::deg2rad;
00021         q[3] = 30*KDL::deg2rad;
00022         q[4] = 40*KDL::deg2rad;
00023         q[5] = 50*KDL::deg2rad;
00024         Frame F1,F2;
00025         jnt2cartpos1->evaluate(q);
00026         jnt2cartpos1->getFrame(F1);
00027         jnt2cartpos2->evaluate(q);
00028         jnt2cartpos2->getFrame(F2);
00029         if (!Equal(F1,F2,1E-7)) {
00030           std::cout << "the two kinematic families do not give the same result." << std::endl;
00031           std::cout << "Result of first kinematic family " << std::endl;
00032           std::cout << F1 << std::endl;
00033           std::cout << "Result of second kinematic family " << std::endl;
00034           std::cout << F2 << std::endl;
00035           exit(1);
00036         }
00037     delete jnt2cartpos1;
00038     delete jnt2cartpos2;
00039 }
00040 
00041 //
00042 // Test whether Jnt2CartPos and CartPos2Jnt give a consistent result.
00043 //
00044 class TestForwardAndInverse {
00045         KinematicFamily*      family;
00046         Jnt2CartPos*          jnt2cartpos;
00047         Frame                         F_base_ee;
00048         Frame                         F_base_ee2;
00049         JointVector           q_solved;
00050         JointVector           q_initial;
00051 public:
00052         CartPos2Jnt*          cartpos2jnt;
00053 public:
00054         static void TestFamily(KinematicFamily* _family) {
00055                 JointVector q_initial(6);
00056                 q_initial[0] = 0*KDL::deg2rad;
00057                 q_initial[1] = 0*KDL::deg2rad;
00058                 q_initial[2] = 90*KDL::deg2rad;
00059                 q_initial[3] = 0*KDL::deg2rad;
00060                 q_initial[4] = 90*KDL::deg2rad;
00061                 q_initial[5] = 0*KDL::deg2rad;
00062                 TestForwardAndInverse testobj(_family,q_initial);
00063                 JointVector q(6);
00064                 q[0] = 0*KDL::deg2rad;
00065                 q[1] = 10*KDL::deg2rad;
00066                 q[2] = 20*KDL::deg2rad;
00067                 q[3] = 30*KDL::deg2rad;
00068                 q[4] = 40*KDL::deg2rad;
00069                 q[5] = 50*KDL::deg2rad;
00070                 testobj.test(q);
00071                 //std::cout << "number of iterations " << ((SerialChainCartPos2Jnt*)testobj.cartpos2jnt)->iter << std::endl;
00072                 q[0] = -10*KDL::deg2rad;
00073                 q[1] = -10*KDL::deg2rad;
00074                 q[2] = 40*KDL::deg2rad;
00075                 q[3] = -30*KDL::deg2rad;
00076                 q[4] = 20*KDL::deg2rad;
00077                 q[5] = 60*KDL::deg2rad;
00078                 testobj.test(q);
00079                 //std::cout << "number of iterations " << ((SerialChainCartPos2Jnt*)testobj.cartpos2jnt)->iter << std::endl;
00080         }
00081 
00082 
00083 //
00084 // Test whether Jnt2CartPos and Jnt2Jac give consistent
00085 // results.
00086 //
00087 class TestForwardPosAndJac {
00088         KinematicFamily* family;
00089         Jnt2CartPos*     jnt2cartpos;
00090         Jnt2Jac*         jnt2jac;
00091     Jacobian<Frame>  FJ_base_ee;
00092         Frame                    F_base_ee1;
00093         Frame                    F_base_ee2;
00094 public:
00095         static void TestFamily(KinematicFamily* _family) {
00096                 TestForwardPosAndJac testobj(_family);
00097                 JointVector q(6);
00098                 q[0] = 0*KDL::deg2rad;
00099                 q[1] = 10*KDL::deg2rad;
00100                 q[2] = 20*KDL::deg2rad;
00101                 q[3] = 30*KDL::deg2rad;
00102                 q[4] = 40*KDL::deg2rad;
00103                 q[5] = 50*KDL::deg2rad;
00104                 testobj.test(q);
00105                 q[0] = -50*KDL::deg2rad;
00106                 q[1] = -10*KDL::deg2rad;
00107                 q[2] = 20*KDL::deg2rad;
00108                 q[3] = -30*KDL::deg2rad;
00109                 q[4] = 20*KDL::deg2rad;
00110                 q[5] = 110*KDL::deg2rad;
00111                 testobj.test(q);
00112         }
00113 
00114     TestForwardPosAndJac(KinematicFamily* _family) :
00115                 family(_family),
00116                 jnt2cartpos(_family->createJnt2CartPos()),
00117                 jnt2jac(_family->createJnt2Jac()),
00118         FJ_base_ee(_family->nrOfJoints())
00119     {
00120                 // the transformations should exist :
00121                 assert( jnt2jac != 0);
00122                 assert( jnt2cartpos != 0);
00123     }
00124 
00125     int test(JointVector& q) {
00126         double deltaq = 1E-4;
00127         double epsJ   = 1E-4;
00128                 if (jnt2jac->evaluate(q)!=0) return 1;
00129                 jnt2jac->getJacobian(FJ_base_ee);
00130         for (int i=0; i< q.size() ;i++) {
00131             // test the derivative of J towards qi
00132             double oldqi = q[i];
00133             q[i] = oldqi+deltaq;
00134                         if (jnt2cartpos->evaluate(q)!=0) return 1;
00135                         jnt2cartpos->getFrame(F_base_ee2);
00136             q[i] = oldqi-deltaq;
00137                         if (jnt2cartpos->evaluate(q)!=0) return 1;
00138                         jnt2cartpos->getFrame(F_base_ee1);
00139             q[i] = oldqi;
00140             // check Jacobian :
00141             Twist Jcol = diff(F_base_ee1,F_base_ee2,2*deltaq);
00142             if (!Equal(Jcol,FJ_base_ee.deriv(i),epsJ)) {
00143                 std::cout << "Difference between symbolic and numeric calculation of Jacobian for column "
00144                           << i << std::endl;
00145                 std::cout << "Numeric " << Jcol << std::endl;
00146                 std::cout << "Symbolic " << FJ_base_ee.deriv(i) << std::endl;
00147                 exit(1);
00148             }
00149         }
00150     }
00151 
00152     ~TestForwardPosAndJac() {
00153                 delete jnt2cartpos;
00154                 delete jnt2jac;
00155     }
00156 };
00157 
00158 
00159 //
00160 // Test whether Jnt2CartVel and Jnt2Jac give consistent
00161 // results.
00162 //
00163 class TestCartVelAndJac {
00164         KinematicFamily* family;
00165         Jnt2CartVel*     jnt2cartvel;
00166         Jnt2Jac*         jnt2jac;
00167     Jacobian<Frame>  FJ_base_ee;
00168         FrameVel                 F_base_ee;
00169 public:
00170         static void TestFamily(KinematicFamily* _family) {
00171                 TestCartVelAndJac testobj(_family);
00172                 JointVector qdot(6);
00173         qdot[0] = 0.1;
00174         qdot[1] = 0.2;
00175         qdot[2] = -0.3;
00176         qdot[3] = 0.4;
00177         qdot[4] = -0.5;
00178         qdot[5] = 0.6;
00179                 JointVector q(6);
00180                 q[0] = 0*KDL::deg2rad;
00181                 q[1] = 10*KDL::deg2rad;
00182                 q[2] = 20*KDL::deg2rad;
00183                 q[3] = 30*KDL::deg2rad;
00184                 q[4] = 40*KDL::deg2rad;
00185                 q[5] = 50*KDL::deg2rad;
00186         std::cout << "q[1] = " << q[1] << std::endl;
00187                 testobj.test(q,qdot);
00188                 q[0] = -50*KDL::deg2rad;
00189                 q[1] = -10*KDL::deg2rad;
00190                 q[2] = 20*KDL::deg2rad;
00191                 q[3] = -30*KDL::deg2rad;
00192                 q[4] = 20*KDL::deg2rad;
00193                 q[5] = 110*KDL::deg2rad;
00194                 testobj.test(q,qdot);
00195         }
00196 
00197     TestCartVelAndJac(KinematicFamily* _family) :
00198                 family(_family),
00199                 jnt2cartvel(_family->createJnt2CartVel()),
00200                 jnt2jac(_family->createJnt2Jac()),
00201         FJ_base_ee(_family->nrOfJoints())
00202     {
00203                 // the transformations should exist :
00204                 assert( jnt2jac != 0);
00205                 assert( jnt2cartvel != 0);
00206     }
00207 
00208     int test(JointVector& q,JointVector& qdot) {
00209         std::cout << "Testing whether Jnt2CartVel and Jnt2Jac are consistent " << std::endl;
00210         std::cout << "q[1] = " << q[1] << std::endl;
00211         double deltaq = 1E-4;
00212         double epsJ   = 1E-4;
00213                 int result;
00214         result = jnt2jac->evaluate(q);
00215         assert(result==0);
00216                 jnt2jac->getJacobian(FJ_base_ee);
00217         result = jnt2cartvel->evaluate(q,qdot);
00218         jnt2cartvel->getFrameVel(F_base_ee);
00219         assert(result==0);
00220         Twist t = Twist::Zero();
00221         for (int i=0; i< q.size() ;i++) {
00222             t += FJ_base_ee.deriv(i)*qdot[i];
00223         }
00224         if (!Equal(t,F_base_ee.GetTwist(),1E-6)) {
00225                 std::cout << "Difference between the resuls"<< std::endl;
00226                 std::cout << "via the Jacobian  " << t << std::endl;
00227                 std::cout << "via the jnt2cartvel transformation  " << F_base_ee.GetTwist() << std::endl;
00228                 exit(1);
00229         }
00230     }
00231 
00232     ~TestCartVelAndJac() {
00233                 delete jnt2cartvel;
00234                 delete jnt2jac;
00235     }
00236 };
00237 
00238 
00239 
00240 //
00241 // Test whether Jnt2CartVel and CartVel2Jnt give consistent
00242 // results.
00243 //
00244 class TestCartVelAndInverse {
00245         KinematicFamily* family;
00246         Jnt2CartVel*     jnt2cartvel;
00247         CartVel2Jnt*     cartvel2jnt;
00248         FrameVel                 F_base_ee;
00249     JointVector      qdot2;
00250 public:
00251         static void TestFamily(KinematicFamily* _family) {
00252                 TestCartVelAndInverse testobj(_family);
00253                 JointVector qdot(6);
00254         qdot[0] = 0.1;
00255         qdot[1] = 0.2;
00256         qdot[2] = -0.3;
00257         qdot[3] = 0.4;
00258         qdot[4] = -0.5;
00259         qdot[5] = 0.6;
00260         JointVector q(6);
00261                 q[0] = 0*KDL::deg2rad;
00262                 q[1] = 10*KDL::deg2rad;
00263                 q[2] = 20*KDL::deg2rad;
00264                 q[3] = 30*KDL::deg2rad;
00265                 q[4] = 40*KDL::deg2rad;
00266                 q[5] = 50*KDL::deg2rad;
00267         testobj.test(q,qdot);
00268                 q[0] = -50*KDL::deg2rad;
00269                 q[1] = -10*KDL::deg2rad;
00270                 q[2] = 20*KDL::deg2rad;
00271                 q[3] = -30*KDL::deg2rad;
00272                 q[4] = 20*KDL::deg2rad;
00273                 q[5] = 110*KDL::deg2rad;
00274                 testobj.test(q,qdot);
00275         }
00276 
00277     TestCartVelAndInverse(KinematicFamily* _family) :
00278                 family(_family),
00279                 jnt2cartvel(_family->createJnt2CartVel()),
00280                 cartvel2jnt(_family->createCartVel2Jnt()),
00281         qdot2(_family->nrOfJoints())
00282     {
00283                 // the transformations should exist :
00284                 assert( cartvel2jnt != 0);
00285                 assert( jnt2cartvel != 0);
00286     }
00287 
00288     int test(const JointVector& q,const JointVector& qdot) {
00289         std::cout << "Testing whether Jnt2CartVel and CartVel2Jnt are consistent " << std::endl;
00290         double epsJ   = 1E-7;
00291                 int result;
00292 
00293         result = jnt2cartvel->evaluate(q,qdot);
00294         assert(result==0);
00295         jnt2cartvel->getFrameVel(F_base_ee);
00296 
00297         cartvel2jnt->setTwist(F_base_ee.GetTwist());
00298         result = cartvel2jnt->evaluate(q, qdot2);
00299         assert(result==0);
00300 
00301         for (int i=0;i<qdot.size();++i) {
00302            if (fabs(qdot[i]-qdot2[i])>epsJ)  {
00303                std::cout << " original joint velocities and calculated joint velocities do not match" << std::endl;
00304                //std::cerr << " original joint velocities and calculated joint velocities do not match" << std::endl;
00305                for (int j=0;j<qdot.size();j++) {
00306                     std::cout << "qdot["<<j<<"]="<<qdot[j]<<"  and qdot2["<<j<<"]="<<qdot2[j] << std::endl;
00307                 }
00308                 std::cout << "Frame : " << F_base_ee.GetFrame() << std::endl;
00309                 std::cout << "Twist : " << F_base_ee.GetTwist() << std::endl;
00310                exit(1);
00311            }
00312         }
00313     }
00314 
00315     ~TestCartVelAndInverse() {
00316                 delete jnt2cartvel;
00317                 delete cartvel2jnt;
00318     }
00319 };
00320 
00321 
00322 
00323 
00324 
00325 
00326 
00327 
00331 class CRS450_exp: public SerialChain {
00332         public:
00333         explicit CRS450_exp(int jointoffset=0) :
00334                 SerialChain("CRS450", 6, jointoffset,new LinearTransmission(6) )
00335         {
00336                 double L1 = 0.33;
00337                 double L2 = 0.305;
00338                 double L3 = 0.33;
00339                 double L4 = 0.176;
00340                 LinearTransmission* tr = (LinearTransmission*)transmission;
00341                 tr->setTransmission(2,2.0,1.0);
00342                 tr->setTransmission(3,3.0,2.0);
00343         addJoint(new JointRotZ(Frame::Identity())); // j1
00344         addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L1))));// j2
00345         addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L2))));// j3
00346         addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0)))); // j4
00347         addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L3))));// j5
00348         addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0)))); // j6
00349         setLastJointToEE(Frame(Rotation::Identity(),Vector(0,0,L4)));
00350         };
00351 };
00352 
00353 
00354 int main(int argc,char* argv[]) {
00355         KinematicFamily* family1,*family2,*family3,*family4;
00356         std::cout << std::endl << "Tests on CRS450 " << std::endl;
00357         family1 = new CRS450();
00358         TestForwardAndInverse::TestFamily(family1);
00359         TestForwardPosAndJac::TestFamily(family1);
00360     TestCartVelAndJac::TestFamily(family1);
00361     TestCartVelAndInverse::TestFamily(family1);//
00362         std::cout <<std::endl <<  "Tests on CRS450_exp " << std::endl;
00363         family2 = new CRS450_exp();
00364         TestForwardAndInverse::TestFamily(family2);
00365         TestForwardPosAndJac::TestFamily(family2);
00366     TestCartVelAndJac::TestFamily(family2);
00367     TestCartVelAndInverse::TestFamily(family2);//
00368         std::cout << std::endl << "Tests on CRS450Feath " << std::endl;
00369         family3 = new CRS450Feath();
00370         TestForwardAndInverse::TestFamily(family3);
00371         TestForwardPosAndJac::TestFamily(family3);
00372     TestCartVelAndJac::TestFamily(family3);
00373     TestCartVelAndInverse::TestFamily(family3);//
00374         std::cout << std::endl << "Comparing the kinematic families "<< std::endl;
00375         CompareFamilies(family1,family3);
00376         std::cout << std::endl << "Tests on CRS450Feath->createSerialChain " << std::endl;
00377     family4 = ((ZXXZXZ*)family3)->createSerialChain();
00378         TestForwardAndInverse::TestFamily(family4);
00379         TestForwardPosAndJac::TestFamily(family4);
00380     TestCartVelAndJac::TestFamily(family4);
00381     TestCartVelAndInverse::TestFamily(family4);//
00382 
00383 
00384 
00385     // testing the clone functionality (valgrind)
00386         KinematicFamily* family1b,*family2b,*family3b,*family4b;
00387     family1b = family1->clone();
00388     family2b = family2->clone();
00389     family3b = family3->clone();
00390     family4b = family4->clone();
00391 
00392     delete family4;
00393         delete family3;
00394         delete family2;
00395         delete family1;
00396 
00397     delete family4b;
00398         delete family3b;
00399         delete family2b;
00400         delete family1b;
00401 
00402         return 0;
00403 }
00404 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:23