orocos_kdl: solvertest.cpp Source File

Go to the documentation of this file.
00001 #include "solvertest.hpp"
00002 #include <frames_io.hpp>
00003 #include <framevel_io.hpp>
00004 #include <kinfam_io.hpp>
00005 #include <time.h>
00006 
00007 
00008 CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest );
00009 
00010 using namespace KDL;
00011 
00012 void SolverTest::setUp()
00013 {
00014     srand( (unsigned)time( NULL ));
00015 
00016     chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
00017                               Frame(Vector(0.0,0.0,0.0))));
00018     chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00019                               Frame(Vector(0.0,0.0,0.9))));
00020     chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::None),
00021                               Frame(Vector(-0.4,0.0,0.0))));
00022     chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00023                               Frame(Vector(0.0,0.0,1.2))));
00024     chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
00025                               Frame(Vector(0.4,0.0,0.0))));
00026     chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
00027                               Frame(Vector(0.0,0.0,1.4))));
00028     chain1.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
00029                               Frame(Vector(0.0,0.0,0.0))));
00030     chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
00031                               Frame(Vector(0.0,0.0,0.4))));
00032     chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::None),
00033                               Frame(Vector(0.0,0.0,0.0))));
00034 
00035     chain2.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
00036                               Frame(Vector(0.0,0.0,0.5))));
00037     chain2.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00038                               Frame(Vector(0.0,0.0,0.4))));
00039     chain2.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),
00040                               Frame(Vector(0.0,0.0,0.3))));
00041     chain2.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00042                               Frame(Vector(0.0,0.0,0.2))));
00043     chain2.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
00044                               Frame(Vector(0.0,0.0,0.1))));
00045 
00046 
00047     chain3.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
00048                               Frame(Vector(0.0,0.0,0.0))));
00049     chain3.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
00050                               Frame(Vector(0.0,0.0,0.9))));
00051     chain3.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
00052                               Frame(Vector(-0.4,0.0,0.0))));
00053     chain3.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
00054                               Frame(Vector(0.0,0.0,1.2))));
00055     chain3.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
00056                               Frame(Vector(0.4,0.0,0.0))));
00057     chain3.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
00058                               Frame(Vector(0.0,0.0,1.4))));
00059     chain3.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
00060                               Frame(Vector(0.0,0.0,0.0))));
00061     chain3.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
00062                               Frame(Vector(0.0,0.0,0.4))));
00063     chain3.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotY),
00064                               Frame(Vector(0.0,0.0,0.0))));
00065 
00066 
00067     chain4.addSegment(Segment("Segment 1", Joint("Joint 1", Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
00068                               Frame(Vector(0.0,0.0,0.5))));
00069     chain4.addSegment(Segment("Segment 2", Joint("Joint 2", Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
00070                               Frame(Vector(0.0,0.0,0.4))));
00071     chain4.addSegment(Segment("Segment 3", Joint("Joint 3", Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
00072                               Frame(Vector(0.0,0.0,0.3))));
00073     chain4.addSegment(Segment("Segment 4", Joint("Joint 4", Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
00074                               Frame(Vector(0.0,0.0,0.2))));
00075     chain4.addSegment(Segment("Segment 5", Joint("Joint 5", Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
00076                               Frame(Vector(0.0,0.0,0.1))));
00077 
00078 
00079 
00080     //chain definition for vereshchagin's dynamic solver
00081     Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
00082     Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
00083 
00084     Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
00085     Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
00086     Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
00087 
00088     //chain segments
00089     Segment segment1 = Segment(rotJoint0, frame1);
00090     Segment segment2 = Segment(rotJoint1, frame2);
00091 
00092     //rotational inertia around symmetry axis of rotation
00093     RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
00094 
00095     //spatial inertia
00096     RigidBodyInertia inerSegment1(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
00097     RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
00098     segment1.setInertia(inerSegment1);
00099     segment2.setInertia(inerSegment2);
00100 
00101     //chain
00102     chaindyn.addSegment(segment1);
00103     chaindyn.addSegment(segment2);
00104 
00105         // Motoman SIA10 Chain (for IK singular value tests)
00106         motomansia10.addSegment(Segment(Joint(Joint::None),
00107                                                                         Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0)));
00108         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00109                                                                         Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00110         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00111                                                                         Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
00112         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00113                                                                         Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00114         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00115                                                                         Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
00116         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00117                                                                         Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00118         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00119                                                                         Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0)));
00120         motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00121                                                                         Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
00122 }
00123 
00124 void SolverTest::tearDown()
00125 {
00126 //     delete fksolverpos;
00127 //     delete jacsolver;
00128 //     delete fksolvervel;
00129 //     delete iksolvervel;
00130 //     delete iksolverpos;
00131 }
00132 
00133 void SolverTest::FkPosAndJacTest()
00134 {
00135     ChainFkSolverPos_recursive fksolver1(chain1);
00136     ChainJntToJacSolver jacsolver1(chain1);
00137     FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
00138     ChainFkSolverPos_recursive fksolver2(chain2);
00139     ChainJntToJacSolver jacsolver2(chain2);
00140     FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
00141     ChainFkSolverPos_recursive fksolver3(chain3);
00142     ChainJntToJacSolver jacsolver3(chain3);
00143     FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
00144     ChainFkSolverPos_recursive fksolver4(chain4);
00145     ChainJntToJacSolver jacsolver4(chain4);
00146     FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
00147 }
00148 
00149 void SolverTest::FkVelAndJacTest()
00150 {
00151     ChainFkSolverVel_recursive fksolver1(chain1);
00152     ChainJntToJacSolver jacsolver1(chain1);
00153     FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
00154     ChainFkSolverVel_recursive fksolver2(chain2);
00155     ChainJntToJacSolver jacsolver2(chain2);
00156     FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
00157     ChainFkSolverVel_recursive fksolver3(chain3);
00158     ChainJntToJacSolver jacsolver3(chain3);
00159     FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
00160     ChainFkSolverVel_recursive fksolver4(chain4);
00161     ChainJntToJacSolver jacsolver4(chain4);
00162     FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
00163 }
00164 
00165 void SolverTest::FkVelAndIkVelTest()
00166 {
00167     //Chain1
00168     std::cout<<"square problem"<<std::endl;
00169     ChainFkSolverVel_recursive fksolver1(chain1);
00170     ChainIkSolverVel_pinv iksolver1(chain1);
00171     ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
00172     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00173     FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
00174     std::cout<<"KDL-SVD-Givens"<<std::endl;
00175     FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
00176 
00177     //Chain2
00178     std::cout<<"underdetermined problem"<<std::endl;
00179     ChainFkSolverVel_recursive fksolver2(chain2);
00180     ChainIkSolverVel_pinv iksolver2(chain2);
00181     ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00182     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00183     FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
00184     std::cout<<"KDL-SVD-Givens"<<std::endl;
00185     FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
00186 
00187     //Chain3
00188     std::cout<<"overdetermined problem"<<std::endl;
00189     ChainFkSolverVel_recursive fksolver3(chain3);
00190     ChainIkSolverVel_pinv iksolver3(chain3);
00191     ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
00192     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00193     FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
00194     std::cout<<"KDL-SVD-Givens"<<std::endl;
00195     FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
00196 
00197     //Chain4
00198     std::cout<<"overdetermined problem"<<std::endl;
00199     ChainFkSolverVel_recursive fksolver4(chain4);
00200     ChainIkSolverVel_pinv iksolver4(chain4);
00201     ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
00202     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00203     FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
00204     std::cout<<"KDL-SVD-Givens"<<std::endl;
00205     FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
00206 }
00207 
00208 void SolverTest::FkPosAndIkPosTest()
00209 {
00210     std::cout<<"square problem"<<std::endl;
00211     ChainFkSolverPos_recursive fksolver1(chain1);
00212     ChainIkSolverVel_pinv iksolver1v(chain1);
00213     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
00214     ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
00215     ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
00216 
00217     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00218     FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
00219     std::cout<<"KDL-SVD-Givens"<<std::endl;
00220     FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
00221 
00222     std::cout<<"underdetermined problem"<<std::endl;
00223     ChainFkSolverPos_recursive fksolver2(chain2);
00224     ChainIkSolverVel_pinv iksolverv2(chain2);
00225     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
00226     ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
00227     ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
00228 
00229     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00230     FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
00231     std::cout<<"KDL-SVD-Givens"<<std::endl;
00232     FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
00233 
00234     std::cout<<"overdetermined problem"<<std::endl;
00235     ChainFkSolverPos_recursive fksolver3(chain3);
00236     ChainIkSolverVel_pinv iksolverv3(chain3);
00237     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
00238     ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
00239     ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
00240 
00241     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00242     FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
00243     std::cout<<"KDL-SVD-Givens"<<std::endl;
00244     FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
00245 
00246     std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
00247     ChainFkSolverPos_recursive fksolver4(chain4);
00248     ChainIkSolverVel_pinv iksolverv4(chain4);
00249     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
00250     ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
00251     ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
00252 
00253     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00254     FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
00255     std::cout<<"KDL-SVD-Givens"<<std::endl;
00256     FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
00257 }
00258 
00259 void SolverTest::IkSingularValueTest()
00260 {
00261         unsigned int maxiter = 30;
00262         double  eps = 1e-6 ;
00263         int maxiter_vel = 30;
00264         double  eps_vel = 0.1 ;
00265     Frame F, dF, F_des,F_solved;
00266         KDL::Twist F_error ;
00267 
00268         std::cout<<"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
00269 
00270     ChainFkSolverPos_recursive fksolver(motomansia10);
00271     ChainIkSolverVel_pinv ikvelsolver1(motomansia10,eps_vel,maxiter_vel);
00272     ChainIkSolverPos_NR iksolver1(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00273         unsigned int nj = motomansia10.getNrOfJoints();
00274     JntArray q(nj), q_solved(nj) ;
00275 
00276 
00277         std::cout<<"norminal case:  convergence"<<std::endl;
00278 
00279         q(0) = 0. ;
00280         q(1) = 0.5 ;
00281         q(2) = 0.4 ;
00282         q(3) = -M_PI_2 ;
00283         q(4) = 0. ;
00284         q(5) = 0. ;
00285         q(6) = 0. ;
00286 
00287         dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00288         dF.p = KDL::Vector(0.01,0.01,0.01) ;
00289 
00290         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00291         F_des = F * dF ;
00292 
00293         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00294                          iksolver1.CartToJnt(q, F_des, q_solved));      // converges
00295     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00296                          ikvelsolver1.getError());
00297         CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00298                          ikvelsolver1.getNrZeroSigmas()) ;              //      1 singular value
00299 
00300         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q_solved,F_solved));
00301         F_error = KDL::diff(F_solved,F_des);
00302         CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
00303 
00304         std::cout<<"nonconvergence:  pseudoinverse singular"<<std::endl;
00305 
00306         q(0) = 0. ;
00307         q(1) = 0.2 ;
00308         q(2) = 0.4 ;
00309         q(3) = -M_PI_2 ;
00310         q(4) = 0. ;
00311         q(5) = 0. ;
00312         q(6) = 0. ;
00313 
00314         dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00315         dF.p = KDL::Vector(0.01,0.01,0.01) ;
00316 
00317         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00318         F_des = F * dF ;
00319 
00320         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00321                          iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
00322         CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00323                          ikvelsolver1.getError());              // truncated SV solution
00324         CPPUNIT_ASSERT_EQUAL((unsigned int)2,
00325                          ikvelsolver1.getNrZeroSigmas()) ;              //      2 singular values (jac pseudoinverse singular)
00326 
00327         std::cout<<"nonconvergence:  large displacement, low iterations"<<std::endl;
00328 
00329         q(0) = 0. ;
00330         q(1) = 0.5 ;
00331         q(2) = 0.4 ;
00332         q(3) = -M_PI_2 ;
00333         q(4) = 0. ;
00334         q(5) = 0. ;
00335         q(6) = 0. ;
00336 
00337         // big displacement
00338         dF.M = KDL::Rotation::RPY(0.2, 0.2, 0.2) ;
00339         dF.p = KDL::Vector(-0.2,-0.2, -0.2) ;
00340 
00341         // low iterations
00342         maxiter = 5 ;
00343     ChainIkSolverPos_NR iksolver2(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00344 
00345         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00346         F_des = F * dF ;
00347 
00348     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00349                          iksolver2.CartToJnt(q,F_des,q_solved));        //  does not converge
00350     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00351                         ikvelsolver1.getError());
00352         CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00353                          ikvelsolver1.getNrZeroSigmas()) ;              //      1 singular value (jac pseudoinverse exists)
00354 
00355         std::cout<<"nonconvergence:  fully singular"<<std::endl;
00356 
00357     q(0) = 0. ;
00358     q(1) = 0. ;
00359     q(2) = 0. ;
00360     q(3) = 0. ;
00361     q(4) = 0. ;
00362     q(5) = 0. ;
00363     q(6) = 0. ;
00364 
00365     dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00366     dF.p = KDL::Vector(0.01,0.01,0.01) ;
00367 
00368     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolver.JntToCart(q,F));
00369     F_des = F * dF ;
00370 
00371     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00372                          iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
00373     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00374                          ikvelsolver1.getError());              // truncated SV solution
00375     CPPUNIT_ASSERT_EQUAL((unsigned int)3,
00376                          ikvelsolver1.getNrZeroSigmas());
00377 }
00378 
00379 
00380 void SolverTest::IkVelSolverWDLSTest()
00381 {
00382         int maxiter = 30;
00383         double  eps = 0.1 ;
00384         double lambda = 0.1 ;
00385 
00386         std::cout<<"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
00387 
00388         KDL::ChainIkSolverVel_wdls ikvelsolver(motomansia10,eps,maxiter) ;
00389         ikvelsolver.setLambda(lambda) ;
00390         unsigned int nj = motomansia10.getNrOfJoints();
00391     JntArray q(nj), dq(nj) ;
00392 
00393         KDL::Vector     v05(0.05,0.05,0.05) ;
00394         KDL::Twist dx(v05,v05) ;
00395 
00396         std::cout<<"smallest singular value is above threshold (no WDLS)"<<std::endl;
00397 
00398         q(0) = 0. ;
00399         q(1) = 0.5 ;
00400         q(2) = 0.4 ;
00401         q(3) = -M_PI_2 ;
00402         q(4) = 0. ;
00403         q(5) = 0. ;
00404         q(6) = 0. ;
00405 
00406         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00407                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00408         CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ;              //      1 singular value
00409 
00410 
00411         std::cout<<"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
00412 
00413         q(1) = 0.2 ;
00414 
00415         CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00416                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00417         CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;           //      2 singular values
00418         CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),
00419                          sqrt(1.0-(ikvelsolver.getSigmaMin()/eps)*(ikvelsolver.getSigmaMin()/eps))*lambda) ;
00420 
00421         std::cout<<"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
00422 
00423         q(1) = 0.0 ;
00424 
00425     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00426                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00427         CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;           //      2 singular values
00428         CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;    // full value
00429 
00430         // fully singular
00431         q(2) = 0.0 ;
00432         q(3) = 0.0 ;
00433 
00434     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00435                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00436         CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
00437         CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;    // full value
00438 }
00439 
00440 
00441 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
00442 {
00443     double deltaq = 1E-4;
00444 
00445     Frame F1,F2;
00446 
00447     JntArray q(chain.getNrOfJoints());
00448     Jacobian jac(chain.getNrOfJoints());
00449 
00450     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00451     {
00452         random(q(i));
00453     }
00454 
00455     jacsolver.JntToJac(q,jac);
00456 
00457     for (unsigned int i=0; i< q.rows() ; i++)
00458     {
00459         // test the derivative of J towards qi
00460         double oldqi = q(i);
00461         q(i) = oldqi+deltaq;
00462         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F2));
00463         q(i) = oldqi-deltaq;
00464         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
00465         q(i) = oldqi;
00466         // check Jacobian :
00467         Twist Jcol1 = diff(F1,F2,2*deltaq);
00468         Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
00469                     Vector(jac(3,i),jac(4,i),jac(5,i)));
00470 
00471         //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
00472         CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
00473     }
00474 }
00475 
00476 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
00477 {
00478     JntArray q(chain.getNrOfJoints());
00479     JntArray qdot(chain.getNrOfJoints());
00480 
00481     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00482     {
00483         random(q(i));
00484         random(qdot(i));
00485     }
00486     JntArrayVel qvel(q,qdot);
00487     Jacobian jac(chain.getNrOfJoints());
00488 
00489     FrameVel cart;
00490     Twist t;
00491 
00492     jacsolver.JntToJac(qvel.q,jac);
00493     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
00494     MultiplyJacobian(jac,qvel.qdot,t);
00495     CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
00496 }
00497 
00498 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
00499 {
00500 
00501     JntArray q(chain.getNrOfJoints());
00502     JntArray qdot(chain.getNrOfJoints());
00503 
00504     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00505     {
00506         random(q(i));
00507         random(qdot(i));
00508     }
00509     JntArrayVel qvel(q,qdot);
00510     JntArray qdot_solved(chain.getNrOfJoints());
00511 
00512     FrameVel cart;
00513 
00514     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
00515 
00516     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved));
00517 
00518     qvel.deriv()=qdot_solved;
00519 
00520     if(chain.getNrOfJoints()<=6)
00521         CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
00522     else
00523     {
00524         FrameVel cart_solved;
00525         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(qvel,cart_solved));
00526         CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
00527     }
00528 }
00529 
00530 
00531 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
00532 {
00533     JntArray q(chain.getNrOfJoints());
00534     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00535     {
00536         random(q(i));
00537     }
00538     JntArray q_init(chain.getNrOfJoints());
00539     double tmp;
00540     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00541     {
00542         random(tmp);
00543         q_init(i)=q(i)+0.1*tmp;
00544     }
00545     JntArray q_solved(q);
00546 
00547     Frame F1,F2;
00548 
00549     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
00550     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolverpos.CartToJnt(q_init,F1,q_solved));
00551     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q_solved,F2));
00552 
00553     CPPUNIT_ASSERT_EQUAL(F1,F2);
00554     //CPPUNIT_ASSERT_EQUAL(q,q_solved);
00555 
00556 }
00557 
00558 
00559 void SolverTest::VereshchaginTest()
00560 {
00561 
00562     Vector constrainXLinear(1.0, 0.0, 0.0);
00563     Vector constrainXAngular(0.0, 0.0, 0.0);
00564     Vector constrainYLinear(0.0, 0.0, 0.0);
00565     Vector constrainYAngular(0.0, 0.0, 0.0);
00566     // Vector constrainZLinear(0.0, 0.0, 0.0);
00567     //Vector constrainZAngular(0.0, 0.0, 0.0);
00568     Twist constraintForcesX(constrainXLinear, constrainXAngular);
00569     Twist constraintForcesY(constrainYLinear, constrainYAngular);
00570     //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
00571     Jacobian alpha(1);
00572     //alpha.setColumn(0, constraintForcesX);
00573     alpha.setColumn(0, constraintForcesX);
00574     //alpha.setColumn(0, constraintForcesZ);
00575 
00576     //Acceleration energy at  the end-effector
00577     JntArray betha(1); //set to zero
00578     betha(0) = 0.0;
00579     //betha(1) = 0.0;
00580     //betha(2) = 0.0;
00581 
00582     //arm root acceleration
00583     Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
00584     Vector angularAcc(0.0, 0.0, 0.0);
00585     Twist twist1(linearAcc, angularAcc);
00586 
00587     //external forces on the arm
00588     Vector externalForce1(0.0, 0.0, 0.0);
00589     Vector externalTorque1(0.0, 0.0, 0.0);
00590     Vector externalForce2(0.0, 0.0, 0.0);
00591     Vector externalTorque2(0.0, 0.0, 0.0);
00592     Wrench externalNetForce1(externalForce1, externalTorque1);
00593     Wrench externalNetForce2(externalForce2, externalTorque2);
00594     Wrenches externalNetForce;
00595     externalNetForce.push_back(externalNetForce1);
00596     externalNetForce.push_back(externalNetForce2);
00597     //~Definition of constraints and external disturbances
00598     //-------------------------------------------------------------------------------------//
00599 
00600 
00601     //Definition of solver and initial configuration
00602     //-------------------------------------------------------------------------------------//
00603     int numberOfConstraints = 1;
00604     ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
00605 
00606     //These arrays of joint values contain actual and desired values
00607     //actual is generated by a solver and integrator
00608     //desired is given by an interpolator
00609     //error is the difference between desired-actual
00610     //in this test only the actual values are printed.
00611     const int k = 1;
00612     JntArray jointPoses[k];
00613     JntArray jointRates[k];
00614     JntArray jointAccelerations[k];
00615     JntArray jointTorques[k];
00616     for (int i = 0; i < k; i++)
00617     {
00618         JntArray jointValues(chaindyn.getNrOfJoints());
00619         jointPoses[i] = jointValues;
00620         jointRates[i] = jointValues;
00621         jointAccelerations[i] = jointValues;
00622         jointTorques[i] = jointValues;
00623     }
00624 
00625     // Initial arm position configuration/constraint
00626     JntArray jointInitialPose(chaindyn.getNrOfJoints());
00627     jointInitialPose(0) = 0.0; // initial joint0 pose
00628     jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
00629     //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
00630     //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
00631 
00632     //actual
00633     jointPoses[0](0) = jointInitialPose(0);
00634     jointPoses[0](1) = jointInitialPose(1);
00635 
00636     //~Definition of solver and initial configuration
00637     //-------------------------------------------------------------------------------------//
00638 
00639 
00640     //Definition of process main loop
00641     //-------------------------------------------------------------------------------------//
00642     //Time required to complete the task move(frameinitialPose, framefinalPose)
00643     double taskTimeConstant = 0.1;
00644     double simulationTime = 1*taskTimeConstant;
00645     double timeDelta = 0.01;
00646     int status;
00647 
00648     const std::string msg = "Assertion failed, check matrix and array sizes";
00649 
00650     for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
00651     {
00652         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]));
00653 
00654         //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
00655         jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
00656         jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
00657         jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
00658         jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
00659         //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
00660         printf("%f          %f      %f       %f     %f       %f      %f     %f      %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1));
00661     }
00662 }
00663 
00664 void SolverTest::FkPosVectTest()
00665 {
00666     ChainFkSolverPos_recursive fksolver1(chain1);
00667     std::vector<Frame> v_out(chain1.getNrOfSegments());
00668     
00669     JntArray q(chain1.getNrOfJoints());
00670     JntArray qdot(chain1.getNrOfJoints());
00671 
00672     for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
00673     {
00674         random(q(i));
00675         random(qdot(i));
00676     }
00677     Frame f_out;
00678     fksolver1.JntToCart(q,v_out);
00679     fksolver1.JntToCart(q,f_out);
00680      
00681     CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
00682 }
00683 
00684 void SolverTest::FkVelVectTest()
00685 {
00686     ChainFkSolverVel_recursive fksolver1(chain1);
00687     std::vector<FrameVel> v_out(chain1.getNrOfSegments());
00688     
00689     JntArray q(chain1.getNrOfJoints());
00690     JntArray qdot(chain1.getNrOfJoints());
00691 
00692     for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
00693     {
00694         random(q(i));
00695         random(qdot(i));
00696     }
00697     JntArrayVel qvel(q,qdot);
00698     FrameVel f_out;
00699     fksolver1.JntToCart(qvel,v_out);
00700     fksolver1.JntToCart(qvel,f_out);
00701     
00702     CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
00703 }