orocos_kdl: jacobiandottest.cpp Source File

Go to the documentation of this file.
00001 #include "jacobiandottest.hpp"
00002 
00003 CPPUNIT_TEST_SUITE_REGISTRATION(JacobianDotTest);
00004 
00005 using namespace KDL;
00006 
00007 void JacobianDotTest::setUp(){}
00008 void JacobianDotTest::tearDown(){}
00009 
00010 
00011 namespace KDL{
00012     static const double L0 = 1.0;
00013     static const double L1 = 0.5;
00014     static const double L2 = 0.4;
00015     static const double L3 = 0;
00016     static const double L4 = 0;
00017     static const double L5 = 0;
00018     Chain d2(){
00019         Chain d2;
00020         d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
00021         d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L1,0,0))));
00022         return d2;
00023     }
00024     Chain d6(){
00025         Chain d6;
00026         d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
00027         d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L1,0,0))));
00028         d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L2,0,0))));
00029         d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L3,0,0))));
00030         d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L4,0,0))));
00031         d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L5,0,0))));
00032         return d6;
00033     }
00034     Chain KukaLWR_DHnew(){
00035         Chain kukaLWR_DHnew;
00036         
00037         //joint 0
00038         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None),
00039                                   Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0)
00040                                   ));
00041         //joint 1
00042         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00043                                   Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
00044                                   Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00045                                                                                                  Vector::Zero(),
00046                                                                                                  RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));
00047                                    
00048         //joint 2 
00049         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00050                                   Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
00051                                   Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
00052                                                                                                    Vector(0.0,-0.3120511,-0.0038871),
00053                                                                                                    RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));
00054                                   
00055         //joint 3
00056         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00057                                   Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
00058                                   Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00059                                                                                                    Vector(0.0,-0.0015515,0.0),
00060                                                                                                    RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));
00061                                   
00062         //joint 4
00063         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00064                                   Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
00065                                   Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
00066                                                                                                    Vector(0.0,0.5216809,0.0),
00067                                                                                                    RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));
00068                                   
00069         //joint 5
00070         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00071                                   Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
00072                                   Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00073                                                                                                    Vector(0.0,0.0119891,0.0),
00074                                                                                                    RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));
00075                                   
00076         //joint 6
00077         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00078                                   Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
00079                                   Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
00080                                                                                                    Vector(0.0,0.0080787,0.0),
00081                                                                                                    RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
00082         //joint 7
00083         kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
00084                                    Frame::Identity(),
00085                                    RigidBodyInertia(2,
00086                                                                                                    Vector::Zero(),
00087                                                                                                    RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));
00088         return kukaLWR_DHnew;
00089     }    
00090 }
00091 
00092 
00093 void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representation)
00094 {
00095     switch(representation)
00096     {
00097         case ChainJntToJacDotSolver::HYBRID:
00098             break;
00099         case ChainJntToJacDotSolver::BODYFIXED:
00100             // Ref Frame {ee}, Ref Point {ee}
00101             J.changeBase(F_bs_ee.M.Inverse());
00102             break;
00103         case ChainJntToJacDotSolver::INERTIAL:
00104             // Ref Frame {bs}, Ref Point {bs}
00105             J.changeRefPoint(-F_bs_ee.p);
00106             break;
00107     }
00108 }
00109 void Jdot_diff(const Jacobian& J_q,
00110                      const Jacobian& J_qdt,
00111                      const double& dt,
00112                      Jacobian& Jdot)
00113 {
00114     assert(J_q.columns() == J_qdt.columns());
00115     assert(J_q.columns() == Jdot.columns());
00116     for(unsigned int l=0;l<6;l++)
00117     for(unsigned int c=0;c<J_q.columns();c++)
00118         Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
00119 }
00120 
00121 Jacobian Jdot_d2_symbolic(const JntArray& q,const JntArray& qdot)
00122 {
00123     // Returns Jdot for the simple 2DOF arm 
00124     Jacobian Jdot(q.rows());
00125     SetToZero(Jdot);
00126     Jdot(0,0) =  -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1))-L0*cos(q(0))*qdot(0);
00127     Jdot(0,1) =  -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1));
00128     Jdot(1,0) =  -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1))-L0*sin(q(0))*qdot(0);
00129     Jdot(1,1) =  -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1));
00130     return Jdot;
00131 }
00132 
00133 Jacobian J_d2_symbolic(const JntArray& q,const JntArray& qdot)
00134 {
00135     // Returns J for the simple 2DOF arm
00136     Jacobian J(q.rows());
00137     SetToZero(J);
00138     J(0,0) =  -L1 * sin(q(0)+q(1))-L0*sin(q(0));
00139     J(0,1) =  -L1 * sin(q(0)+q(1));
00140     J(1,0) =   L1 * cos(q(0)+q(1))+L0*cos(q(0));
00141     J(1,1) =   L1 * cos(q(0)+q(1));
00142     J(5,0) = J(5,1) = 1;
00143     return J;
00144 }
00145 
00146 JntArray diff(const JntArray& q,const JntArray& qdot,const double& dt)
00147 {
00148     JntArray q_qdqt(q);
00149     for(unsigned int i=0; i<q.rows(); i++)
00150         q_qdqt(i) += dt*qdot(i);
00151     return q_qdqt;
00152 }
00153 
00154 void random(JntArray& q)
00155 {
00156     for(unsigned int i=0; i<q.rows(); i++)
00157         random(q(i));
00158 }
00159 
00160 double compare_Jdot_Diff_vs_Solver(const Chain& chain,const double& dt,const int& representation,bool verbose)
00161 {
00162     // This test verifies if the solvers gives approx. the same result as [ J(q+qdot*dot) - J(q) ]/dot
00163     JntArray q(chain.getNrOfJoints());
00164     JntArray qdot(chain.getNrOfJoints());
00165     JntArray q_dqdt(chain.getNrOfJoints());
00166 
00167 
00168     random(q);
00169     random(qdot);
00170     q_dqdt = diff(q,qdot,dt);
00171     
00172     ChainJntToJacDotSolver jdot_solver(chain);
00173     ChainJntToJacSolver j_solver(chain);
00174     ChainFkSolverPos_recursive fk_solver(chain);
00175 
00176     Frame F_bs_ee_q,F_bs_ee_q_dqdt;
00177     Jacobian jac_q(chain.getNrOfJoints()),
00178                 jac_q_dqdt(chain.getNrOfJoints()),
00179                 jdot_by_diff(chain.getNrOfJoints());
00180                 
00181     j_solver.JntToJac(q,jac_q);
00182     j_solver.JntToJac(q_dqdt,jac_q_dqdt);  
00183     
00184     fk_solver.JntToCart(q,F_bs_ee_q);
00185     fk_solver.JntToCart(q_dqdt,F_bs_ee_q_dqdt);
00186     
00187     changeRepresentation(jac_q,F_bs_ee_q,representation);
00188     changeRepresentation(jac_q_dqdt,F_bs_ee_q_dqdt,representation);
00189     
00190     Jdot_diff(jac_q,jac_q_dqdt,dt,jdot_by_diff);
00191 
00192     Jacobian jdot_by_solver(chain.getNrOfJoints());
00193     jdot_solver.setRepresentation(representation);
00194     jdot_solver.JntToJacDot(JntArrayVel(q_dqdt,qdot),jdot_by_solver);
00195 
00196     Twist jdot_qdot_by_solver;
00197     MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
00198 
00199     Twist jdot_qdot_by_diff;
00200     MultiplyJacobian(jdot_by_diff,qdot,jdot_qdot_by_diff);
00201     
00202     if(verbose){
00203         std::cout << "Jdot diff : \n" << jdot_by_diff<<std::endl;
00204         std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
00205         
00206         std::cout << "Error : " <<jdot_qdot_by_diff-jdot_qdot_by_solver<<q<<qdot<<std::endl;
00207     }
00208     double err = jdot_qdot_by_diff.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
00209                   + jdot_qdot_by_diff.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
00210     return std::abs(err);
00211 }
00212 
00213 double compare_d2_Jdot_Symbolic_vs_Solver(bool verbose)
00214 {
00215     Chain chain=d2();
00216     JntArray q(chain.getNrOfJoints());
00217     JntArray qdot(chain.getNrOfJoints());
00218 
00219     random(q);
00220     random(qdot);
00221     
00222     ChainJntToJacDotSolver jdot_solver(chain);
00223                
00224     Jacobian jdot_sym = Jdot_d2_symbolic(q,qdot);
00225 
00226     Jacobian jdot_by_solver(chain.getNrOfJoints());
00227     jdot_solver.JntToJacDot(JntArrayVel(q,qdot),jdot_by_solver);
00228 
00229     Twist jdot_qdot_by_solver;
00230     MultiplyJacobian(jdot_by_solver,qdot,jdot_qdot_by_solver);
00231 
00232     Twist jdot_qdot_sym;
00233     MultiplyJacobian(jdot_sym,qdot,jdot_qdot_sym);
00234     
00235     if(verbose){
00236         std::cout << "Jdot symbolic : \n" << jdot_sym<<std::endl;
00237         std::cout << "Jdot solver:\n"<<jdot_by_solver<<std::endl;
00238         std::cout << "Error : " <<jdot_qdot_sym-jdot_qdot_by_solver<<q<<qdot<<std::endl;
00239     }
00240     double err = jdot_qdot_sym.vel.Norm() - jdot_qdot_by_solver.vel.Norm()
00241                   + jdot_qdot_sym.rot.Norm() - jdot_qdot_by_solver.rot.Norm();
00242     return std::abs(err);    
00243 }
00244 
00245 bool runTest(const Chain& chain,const int& representation)
00246 {
00247     bool success=true;
00248     bool verbose = false;
00249     double err;
00250     bool print_err = false;
00251     
00252     for(double dt=1e-6;dt<0.1;dt*=10)
00253     {
00254         double eps_diff_vs_solver = 4.0*dt; // Apparently :)
00255 
00256         for(int i=0;i<100;i++)
00257         {
00258             err = compare_Jdot_Diff_vs_Solver(chain,dt,representation,verbose);
00259 
00260             success &= err<=eps_diff_vs_solver;
00261 
00262             if(!success || print_err){
00263                 std::cout<<" dt:"<< dt<<" err:"<<err
00264                 <<" eps_diff_vs_solver:"<<eps_diff_vs_solver
00265                 <<std::endl;
00266                 if(!success)
00267                     break;
00268             }
00269         }
00270     }
00271 
00272     return success;
00273 }
00274 
00275 void JacobianDotTest::testD2DiffHybrid(){
00276     CPPUNIT_ASSERT(runTest(d2(),0));
00277 }
00278 void JacobianDotTest::testD6DiffHybrid(){
00279     CPPUNIT_ASSERT(runTest(d6(),0));
00280 }
00281 void JacobianDotTest::testKukaDiffHybrid(){
00282     CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),0));
00283 }
00284 
00285 void JacobianDotTest::testD2DiffInertial(){
00286     CPPUNIT_ASSERT(runTest(d2(),2));
00287 }
00288 void JacobianDotTest::testD6DiffInertial(){
00289     CPPUNIT_ASSERT(runTest(d6(),2));
00290 }
00291 void JacobianDotTest::testKukaDiffInertial(){
00292     CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),2));
00293 }
00294 void JacobianDotTest::testD2DiffBodyFixed(){
00295     CPPUNIT_ASSERT(runTest(d2(),1));
00296 }
00297 void JacobianDotTest::testD6DiffBodyFixed(){
00298     CPPUNIT_ASSERT(runTest(d6(),1));
00299 }
00300 void JacobianDotTest::testKukaDiffBodyFixed(){
00301     CPPUNIT_ASSERT(runTest(KukaLWR_DHnew(),1));
00302 }
00303 
00304 void JacobianDotTest::testD2Symbolic(){
00305     // This test verifies if the solvers gives the same result as the symbolic Jdot (Hybrid only)
00306     bool success=true;
00307     bool verbose = false;
00308     double err_d2_sym;
00309     bool print_err = false;
00310     
00311     double eps_sym_vs_solver = 1e-10;
00312     
00313     for(int i=0;i<100;i++)
00314     {
00315         err_d2_sym =    compare_d2_Jdot_Symbolic_vs_Solver(verbose);
00316         
00317         success &= err_d2_sym<=eps_sym_vs_solver;
00318         
00319         if(!success || print_err){
00320             std::cout <<" err_d2_sym:"<<err_d2_sym
00321             <<" eps_sym_vs_solver:"<<eps_sym_vs_solver<<std::endl;
00322             if(!success)
00323             break;
00324         }
00325             
00326     }
00327     
00328     CPPUNIT_ASSERT(success);
00329 }