solvertest.cpp
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     // Motoman SIA10 Chain with Mass Parameters (for forward dynamics tests)
00124 
00125     //  effective motor inertia is included as joint inertia
00126     static const double scale=1;
00127     static const double offset=0;
00128     static const double inertiamotorA=5.0;      // effective motor inertia kg-m^2
00129     static const double inertiamotorB=3.0;      // effective motor inertia kg-m^2
00130     static const double inertiamotorC=1.0;      // effective motor inertia kg-m^2
00131     static const double damping=0;
00132     static const double stiffness=0;
00133 
00134     //  Segment Inertias
00135     KDL::RigidBodyInertia inert1(15.0, KDL::Vector(0.0, -0.02, 0.0),                       // mass, CM
00136                                  KDL::RotationalInertia(0.1, 0.05, 0.1, 0.0, 0.0, 0.0));   // inertia
00137     KDL::RigidBodyInertia inert2(5.0, KDL::Vector(0.0, -0.02, -0.1),
00138                                  KDL::RotationalInertia(0.01, 0.1, 0.1, 0.0, 0.0, 0.0));
00139     KDL::RigidBodyInertia inert3(5.0, KDL::Vector(0.0, -0.05, 0.02),
00140                                  KDL::RotationalInertia(0.05, 0.01, 0.05, 0.0, 0.0, 0.0));
00141     KDL::RigidBodyInertia inert4(3.0, KDL::Vector(0.0, 0.02, -0.15),
00142                                  KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
00143     KDL::RigidBodyInertia inert5(3.0, KDL::Vector(0.0, -0.05, 0.01),
00144                                  KDL::RotationalInertia(0.02, 0.01, 0.02, 0.0, 0.0, 0.0));
00145     KDL::RigidBodyInertia inert6(3.0, KDL::Vector(0.0, -0.01, -0.1),
00146                                  KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
00147     KDL::RigidBodyInertia inert7(1.0, KDL::Vector(0.0, 0.0, 0.05),
00148                                  KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0));
00149 
00150     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
00151                                Frame::DH(0.0, M_PI_2, 0.36, 0.0),
00152                                inert1));
00153     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
00154                                Frame::DH(0.0, -M_PI_2, 0.0, 0.0),
00155                                inert2));
00156     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
00157                                Frame::DH(0.0, M_PI_2, 0.36, 0.0),
00158                                inert3));
00159     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
00160                                Frame::DH(0.0, -M_PI_2, 0.0, 0.0),
00161                                inert4));
00162     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
00163                                Frame::DH(0.0, M_PI_2, 0.36, 0.0),
00164                                inert5));
00165     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
00166                                Frame::DH(0.0, -M_PI_2, 0.0, 0.0),
00167                                inert6));
00168     motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
00169                                Frame::DH(0.0, 0.0, 0.0, 0.0),
00170                                inert7));
00171     motomansia10dyn.addSegment(Segment(Joint(Joint::None),
00172                                        Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
00173 }
00174 
00175 void SolverTest::tearDown()
00176 {
00177 //     delete fksolverpos;
00178 //     delete jacsolver;
00179 //     delete fksolvervel;
00180 //     delete iksolvervel;
00181 //     delete iksolverpos;
00182 }
00183 
00184 void SolverTest::UpdateChainTest()
00185 {
00186     ChainFkSolverPos_recursive fksolverpos(chain2);
00187     ChainFkSolverVel_recursive fksolvervel(chain2);
00188     ChainJntToJacSolver jacsolver1(chain2);
00189     ChainJntToJacDotSolver jacdotsolver1(chain2);
00190     ChainIkSolverVel_pinv iksolver2(chain2);
00191     ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00192     ChainIkSolverVel_pinv_nso  iksolver_pinv_nso(chain2);
00193     ChainIkSolverVel_wdls iksolver_wdls(chain2,1e-6,30);
00194     ChainIkSolverPos_NR iksolverpos(chain2,fksolverpos,iksolver2);
00195     ChainIkSolverPos_NR_JL iksolverpos2(chain2,fksolverpos,iksolver2);
00196     ChainIkSolverPos_LMA iksolverpos3(chain2);
00197     ChainDynParam dynparam(chain2, Vector::Zero());
00198     ChainIdSolver_RNE idsolver1(chain2, Vector::Zero());
00199     unsigned int nr_of_constraints = 4;
00200     ChainIdSolver_Vereshchagin idsolver2(chain2,Twist::Zero(),4);
00201 
00202     JntArray q_in(chain2.getNrOfJoints());
00203     JntArray q_in2(chain2.getNrOfJoints());
00204     JntArrayVel q_in3(chain2.getNrOfJoints());
00205     for(unsigned int i=0; i<chain2.getNrOfJoints(); i++)
00206     {
00207         random(q_in(i));
00208         random(q_in2(i));
00209     }
00210     JntArray q_out(chain2.getNrOfJoints());
00211     JntArray q_out2(chain2.getNrOfJoints());
00212     Jacobian jac(chain2.getNrOfJoints());
00213     Frame T;
00214     Twist t;
00215     FrameVel T2;
00216     Wrenches wrenches(chain2.getNrOfSegments());
00217     JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
00218 
00219     Jacobian alpha(nr_of_constraints - 1);
00220     JntArray beta(nr_of_constraints - 1);
00221     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()+1));
00222     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()+1));
00223     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()+1));
00224     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()+1));
00225     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()+1));
00226     chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX),
00227             Frame(Vector(0.0,0.0,0.1))));
00228     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00229     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00230     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
00231     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver2.CartToJnt(q_in,t,q_out));
00232     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00233     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00234     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.CartToJnt(q_in,t,q_out));
00235     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.CartToJnt(q_in,T,q_out));
00236     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out));
00237     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out));
00238     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00239     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00240     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00241     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
00242     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
00243 
00244     jacsolver1.updateInternalDataStructures();
00245     jacdotsolver1.updateInternalDataStructures();
00246     iksolver2.updateInternalDataStructures();
00247     iksolver_pinv_givens2.updateInternalDataStructures();
00248     iksolver_pinv_nso.updateInternalDataStructures();
00249     iksolver_wdls.updateInternalDataStructures();
00250     iksolverpos.updateInternalDataStructures();
00251     iksolverpos2.updateInternalDataStructures();
00252     iksolverpos3.updateInternalDataStructures();
00253     idsolver1.updateInternalDataStructures();
00254     idsolver2.updateInternalDataStructures();
00255     dynparam.updateInternalDataStructures();
00256 
00257     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
00258     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
00259     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00260     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
00261     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00262     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
00263     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00264     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00265     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
00266     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
00267     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
00268     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
00269     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00270     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00271     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00272     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
00273     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
00274 
00275     q_in.resize(chain2.getNrOfJoints());
00276     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00277     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver2.CartToJnt(q_in,t,q_out));
00278     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00279     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00280     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.CartToJnt(q_in,t,q_out));
00281     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos.CartToJnt(q_in,T,q_out));
00282     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
00283     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
00284     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos2.CartToJnt(q_in,T,q_out));
00285     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, iksolverpos3.CartToJnt(q_in,T,q_out));
00286     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00287     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00288     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00289     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
00290     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
00291     q_in2.resize(chain2.getNrOfJoints());
00292     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
00293     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00294     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00295     wrenches.resize(chain2.getNrOfSegments());
00296     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00297     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00298     q_out2.resize(chain2.getNrOfSegments());
00299     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00300     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00301     alpha.resize(nr_of_constraints);
00302     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00303     beta.resize(nr_of_constraints);
00304     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00305     jac.resize(chain2.getNrOfJoints());
00306     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00307     q_out.resize(chain2.getNrOfJoints());
00308     q_in3.resize(chain2.getNrOfJoints());
00309     m.resize(chain2.getNrOfJoints());
00310     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
00311     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
00312     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
00313     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
00314     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
00315     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver2.CartToJnt(q_in,t,q_out));
00316     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
00317     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
00318     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_wdls.CartToJnt(q_in,t,q_out));
00319     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos.CartToJnt(q_in,T,q_out));
00320     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
00321     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
00322     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
00323     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
00324     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
00325     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
00326     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
00327     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
00328     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
00329 }
00330 void SolverTest::FkPosAndJacTest()
00331 {
00332     ChainFkSolverPos_recursive fksolver1(chain1);
00333     ChainJntToJacSolver jacsolver1(chain1);
00334     FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
00335     ChainFkSolverPos_recursive fksolver2(chain2);
00336     ChainJntToJacSolver jacsolver2(chain2);
00337     FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
00338     ChainFkSolverPos_recursive fksolver3(chain3);
00339     ChainJntToJacSolver jacsolver3(chain3);
00340     FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
00341     ChainFkSolverPos_recursive fksolver4(chain4);
00342     ChainJntToJacSolver jacsolver4(chain4);
00343     FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
00344 }
00345 
00346 void SolverTest::FkVelAndJacTest()
00347 {
00348     ChainFkSolverVel_recursive fksolver1(chain1);
00349     ChainJntToJacSolver jacsolver1(chain1);
00350     FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
00351     ChainFkSolverVel_recursive fksolver2(chain2);
00352     ChainJntToJacSolver jacsolver2(chain2);
00353     FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
00354     ChainFkSolverVel_recursive fksolver3(chain3);
00355     ChainJntToJacSolver jacsolver3(chain3);
00356     FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
00357     ChainFkSolverVel_recursive fksolver4(chain4);
00358     ChainJntToJacSolver jacsolver4(chain4);
00359     FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
00360 }
00361 
00362 void SolverTest::FkVelAndIkVelTest()
00363 {
00364     //Chain1
00365     std::cout<<"square problem"<<std::endl;
00366     ChainFkSolverVel_recursive fksolver1(chain1);
00367     ChainIkSolverVel_pinv iksolver1(chain1);
00368     ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
00369     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00370     FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
00371     std::cout<<"KDL-SVD-Givens"<<std::endl;
00372     FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
00373 
00374     //Chain2
00375     std::cout<<"underdetermined problem"<<std::endl;
00376     ChainFkSolverVel_recursive fksolver2(chain2);
00377     ChainIkSolverVel_pinv iksolver2(chain2);
00378     ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00379     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00380     FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
00381     std::cout<<"KDL-SVD-Givens"<<std::endl;
00382     FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
00383 
00384     //Chain3
00385     std::cout<<"overdetermined problem"<<std::endl;
00386     ChainFkSolverVel_recursive fksolver3(chain3);
00387     ChainIkSolverVel_pinv iksolver3(chain3);
00388     ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
00389     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00390     FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
00391     std::cout<<"KDL-SVD-Givens"<<std::endl;
00392     FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
00393 
00394     //Chain4
00395     std::cout<<"overdetermined problem"<<std::endl;
00396     ChainFkSolverVel_recursive fksolver4(chain4);
00397     ChainIkSolverVel_pinv iksolver4(chain4);
00398     ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
00399     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00400     FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
00401     std::cout<<"KDL-SVD-Givens"<<std::endl;
00402     FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
00403 }
00404 
00405 void SolverTest::FkPosAndIkPosTest()
00406 {
00407     std::cout<<"square problem"<<std::endl;
00408     ChainFkSolverPos_recursive fksolver1(chain1);
00409     ChainIkSolverVel_pinv iksolver1v(chain1);
00410     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
00411     ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
00412     ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
00413 
00414     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00415     FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
00416     std::cout<<"KDL-SVD-Givens"<<std::endl;
00417     FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
00418 
00419     std::cout<<"underdetermined problem"<<std::endl;
00420     ChainFkSolverPos_recursive fksolver2(chain2);
00421     ChainIkSolverVel_pinv iksolverv2(chain2);
00422     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
00423     ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
00424     ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
00425 
00426     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00427     FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
00428     std::cout<<"KDL-SVD-Givens"<<std::endl;
00429     FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
00430 
00431     std::cout<<"overdetermined problem"<<std::endl;
00432     ChainFkSolverPos_recursive fksolver3(chain3);
00433     ChainIkSolverVel_pinv iksolverv3(chain3);
00434     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
00435     ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
00436     ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
00437 
00438     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00439     FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
00440     std::cout<<"KDL-SVD-Givens"<<std::endl;
00441     FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
00442 
00443     std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
00444     ChainFkSolverPos_recursive fksolver4(chain4);
00445     ChainIkSolverVel_pinv iksolverv4(chain4);
00446     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
00447     ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
00448     ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
00449 
00450     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00451     FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
00452     std::cout<<"KDL-SVD-Givens"<<std::endl;
00453     FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
00454 }
00455 
00456 void SolverTest::IkSingularValueTest()
00457 {
00458         unsigned int maxiter = 30;
00459         double  eps = 1e-6 ;
00460         int maxiter_vel = 30;
00461         double  eps_vel = 0.1 ;
00462     Frame F, dF, F_des,F_solved;
00463         KDL::Twist F_error ;
00464 
00465         std::cout<<"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
00466 
00467     ChainFkSolverPos_recursive fksolver(motomansia10);
00468     ChainIkSolverVel_pinv ikvelsolver1(motomansia10,eps_vel,maxiter_vel);
00469     ChainIkSolverPos_NR iksolver1(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00470         unsigned int nj = motomansia10.getNrOfJoints();
00471     JntArray q(nj), q_solved(nj) ;
00472 
00473 
00474         std::cout<<"norminal case:  convergence"<<std::endl;
00475 
00476         q(0) = 0. ;
00477         q(1) = 0.5 ;
00478         q(2) = 0.4 ;
00479         q(3) = -M_PI_2 ;
00480         q(4) = 0. ;
00481         q(5) = 0. ;
00482         q(6) = 0. ;
00483 
00484         dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00485         dF.p = KDL::Vector(0.01,0.01,0.01) ;
00486 
00487         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00488         F_des = F * dF ;
00489 
00490         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00491                          iksolver1.CartToJnt(q, F_des, q_solved));      // converges
00492     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00493                          ikvelsolver1.getError());
00494         CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00495                          ikvelsolver1.getNrZeroSigmas()) ;              //      1 singular value
00496 
00497         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q_solved,F_solved));
00498         F_error = KDL::diff(F_solved,F_des);
00499         CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
00500 
00501         std::cout<<"nonconvergence:  pseudoinverse singular"<<std::endl;
00502 
00503         q(0) = 0. ;
00504         q(1) = 0.2 ;
00505         q(2) = 0.4 ;
00506         q(3) = -M_PI_2 ;
00507         q(4) = 0. ;
00508         q(5) = 0. ;
00509         q(6) = 0. ;
00510 
00511         dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00512         dF.p = KDL::Vector(0.01,0.01,0.01) ;
00513 
00514         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00515         F_des = F * dF ;
00516 
00517         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00518                          iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
00519         CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00520                          ikvelsolver1.getError());              // truncated SV solution
00521         CPPUNIT_ASSERT_EQUAL((unsigned int)2,
00522                          ikvelsolver1.getNrZeroSigmas()) ;              //      2 singular values (jac pseudoinverse singular)
00523 
00524         std::cout<<"nonconvergence:  large displacement, low iterations"<<std::endl;
00525 
00526         q(0) = 0. ;
00527         q(1) = 0.5 ;
00528         q(2) = 0.4 ;
00529         q(3) = -M_PI_2 ;
00530         q(4) = 0. ;
00531         q(5) = 0. ;
00532         q(6) = 0. ;
00533 
00534         // big displacement
00535         dF.M = KDL::Rotation::RPY(0.2, 0.2, 0.2) ;
00536         dF.p = KDL::Vector(-0.2,-0.2, -0.2) ;
00537 
00538         // low iterations
00539         maxiter = 5 ;
00540     ChainIkSolverPos_NR iksolver2(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00541 
00542         CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00543         F_des = F * dF ;
00544 
00545     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00546                          iksolver2.CartToJnt(q,F_des,q_solved));        //  does not converge
00547     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00548                         ikvelsolver1.getError());
00549         CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00550                          ikvelsolver1.getNrZeroSigmas()) ;              //      1 singular value (jac pseudoinverse exists)
00551 
00552         std::cout<<"nonconvergence:  fully singular"<<std::endl;
00553 
00554     q(0) = 0. ;
00555     q(1) = 0. ;
00556     q(2) = 0. ;
00557     q(3) = 0. ;
00558     q(4) = 0. ;
00559     q(5) = 0. ;
00560     q(6) = 0. ;
00561 
00562     dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00563     dF.p = KDL::Vector(0.01,0.01,0.01) ;
00564 
00565     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolver.JntToCart(q,F));
00566     F_des = F * dF ;
00567 
00568     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
00569                          iksolver1.CartToJnt(q,F_des,q_solved)); // no converge
00570     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00571                          ikvelsolver1.getError());              // truncated SV solution
00572     CPPUNIT_ASSERT_EQUAL((unsigned int)3,
00573                          ikvelsolver1.getNrZeroSigmas());
00574 }
00575 
00576 
00577 void SolverTest::IkVelSolverWDLSTest()
00578 {
00579         int maxiter = 30;
00580         double  eps = 0.1 ;
00581         double lambda = 0.1 ;
00582 
00583         std::cout<<"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
00584 
00585         KDL::ChainIkSolverVel_wdls ikvelsolver(motomansia10,eps,maxiter) ;
00586         ikvelsolver.setLambda(lambda) ;
00587         unsigned int nj = motomansia10.getNrOfJoints();
00588     JntArray q(nj), dq(nj) ;
00589 
00590         KDL::Vector     v05(0.05,0.05,0.05) ;
00591         KDL::Twist dx(v05,v05) ;
00592 
00593         std::cout<<"smallest singular value is above threshold (no WDLS)"<<std::endl;
00594 
00595         q(0) = 0. ;
00596         q(1) = 0.5 ;
00597         q(2) = 0.4 ;
00598         q(3) = -M_PI_2 ;
00599         q(4) = 0. ;
00600         q(5) = 0. ;
00601         q(6) = 0. ;
00602 
00603         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00604                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00605         CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ;              //      1 singular value
00606 
00607 
00608         std::cout<<"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
00609 
00610         q(1) = 0.2 ;
00611 
00612         CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00613                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00614         CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;           //      2 singular values
00615         CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),
00616                          sqrt(1.0-(ikvelsolver.getSigmaMin()/eps)*(ikvelsolver.getSigmaMin()/eps))*lambda) ;
00617 
00618         std::cout<<"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
00619 
00620         q(1) = 0.0 ;
00621 
00622     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00623                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00624         CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;           //      2 singular values
00625         CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;    // full value
00626 
00627         // fully singular
00628         q(2) = 0.0 ;
00629         q(3) = 0.0 ;
00630 
00631     CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00632                          ikvelsolver.CartToJnt(q, dx, dq)) ;    // wdls mode
00633         CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
00634         CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;    // full value
00635 }
00636 
00637 
00638 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
00639 {
00640     double deltaq = 1E-4;
00641 
00642     Frame F1,F2;
00643 
00644     JntArray q(chain.getNrOfJoints());
00645     Jacobian jac(chain.getNrOfJoints());
00646 
00647     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00648     {
00649         random(q(i));
00650     }
00651 
00652     jacsolver.JntToJac(q,jac);
00653 
00654     for (unsigned int i=0; i< q.rows() ; i++)
00655     {
00656         // test the derivative of J towards qi
00657         double oldqi = q(i);
00658         q(i) = oldqi+deltaq;
00659         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F2));
00660         q(i) = oldqi-deltaq;
00661         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
00662         q(i) = oldqi;
00663         // check Jacobian :
00664         Twist Jcol1 = diff(F1,F2,2*deltaq);
00665         Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
00666                     Vector(jac(3,i),jac(4,i),jac(5,i)));
00667 
00668         //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
00669         CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
00670     }
00671 }
00672 
00673 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
00674 {
00675     JntArray q(chain.getNrOfJoints());
00676     JntArray qdot(chain.getNrOfJoints());
00677 
00678     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00679     {
00680         random(q(i));
00681         random(qdot(i));
00682     }
00683     JntArrayVel qvel(q,qdot);
00684     Jacobian jac(chain.getNrOfJoints());
00685 
00686     FrameVel cart;
00687     Twist t;
00688 
00689     jacsolver.JntToJac(qvel.q,jac);
00690     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
00691     MultiplyJacobian(jac,qvel.qdot,t);
00692     CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
00693 }
00694 
00695 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
00696 {
00697 
00698     JntArray q(chain.getNrOfJoints());
00699     JntArray qdot(chain.getNrOfJoints());
00700 
00701     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00702     {
00703         random(q(i));
00704         random(qdot(i));
00705     }
00706     JntArrayVel qvel(q,qdot);
00707     JntArray qdot_solved(chain.getNrOfJoints());
00708 
00709     FrameVel cart;
00710 
00711     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolvervel.JntToCart(qvel,cart));
00712 
00713     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved));
00714 
00715     qvel.deriv()=qdot_solved;
00716 
00717     if(chain.getNrOfJoints()<=6)
00718         CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
00719     else
00720     {
00721         FrameVel cart_solved;
00722         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(qvel,cart_solved));
00723         CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
00724     }
00725 }
00726 
00727 
00728 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
00729 {
00730     JntArray q(chain.getNrOfJoints());
00731     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00732     {
00733         random(q(i));
00734     }
00735     JntArray q_init(chain.getNrOfJoints());
00736     double tmp;
00737     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00738     {
00739         random(tmp);
00740         q_init(i)=q(i)+0.1*tmp;
00741     }
00742     JntArray q_solved(q);
00743 
00744     Frame F1,F2;
00745 
00746     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q,F1));
00747     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, iksolverpos.CartToJnt(q_init,F1,q_solved));
00748     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, fksolverpos.JntToCart(q_solved,F2));
00749 
00750     CPPUNIT_ASSERT_EQUAL(F1,F2);
00751     //CPPUNIT_ASSERT_EQUAL(q,q_solved);
00752 
00753 }
00754 
00755 
00756 void SolverTest::VereshchaginTest()
00757 {
00758 
00759     Vector constrainXLinear(1.0, 0.0, 0.0);
00760     Vector constrainXAngular(0.0, 0.0, 0.0);
00761     Vector constrainYLinear(0.0, 0.0, 0.0);
00762     Vector constrainYAngular(0.0, 0.0, 0.0);
00763     // Vector constrainZLinear(0.0, 0.0, 0.0);
00764     //Vector constrainZAngular(0.0, 0.0, 0.0);
00765     Twist constraintForcesX(constrainXLinear, constrainXAngular);
00766     Twist constraintForcesY(constrainYLinear, constrainYAngular);
00767     //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
00768     Jacobian alpha(1);
00769     //alpha.setColumn(0, constraintForcesX);
00770     alpha.setColumn(0, constraintForcesX);
00771     //alpha.setColumn(0, constraintForcesZ);
00772 
00773     //Acceleration energy at  the end-effector
00774     JntArray betha(1); //set to zero
00775     betha(0) = 0.0;
00776     //betha(1) = 0.0;
00777     //betha(2) = 0.0;
00778 
00779     //arm root acceleration
00780     Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
00781     Vector angularAcc(0.0, 0.0, 0.0);
00782     Twist twist1(linearAcc, angularAcc);
00783 
00784     //external forces on the arm
00785     Vector externalForce1(0.0, 0.0, 0.0);
00786     Vector externalTorque1(0.0, 0.0, 0.0);
00787     Vector externalForce2(0.0, 0.0, 0.0);
00788     Vector externalTorque2(0.0, 0.0, 0.0);
00789     Wrench externalNetForce1(externalForce1, externalTorque1);
00790     Wrench externalNetForce2(externalForce2, externalTorque2);
00791     Wrenches externalNetForce;
00792     externalNetForce.push_back(externalNetForce1);
00793     externalNetForce.push_back(externalNetForce2);
00794     //~Definition of constraints and external disturbances
00795     //-------------------------------------------------------------------------------------//
00796 
00797 
00798     //Definition of solver and initial configuration
00799     //-------------------------------------------------------------------------------------//
00800     int numberOfConstraints = 1;
00801     ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
00802 
00803     //These arrays of joint values contain actual and desired values
00804     //actual is generated by a solver and integrator
00805     //desired is given by an interpolator
00806     //error is the difference between desired-actual
00807     //in this test only the actual values are printed.
00808     const int k = 1;
00809     JntArray jointPoses[k];
00810     JntArray jointRates[k];
00811     JntArray jointAccelerations[k];
00812     JntArray jointTorques[k];
00813     for (int i = 0; i < k; i++)
00814     {
00815         JntArray jointValues(chaindyn.getNrOfJoints());
00816         jointPoses[i] = jointValues;
00817         jointRates[i] = jointValues;
00818         jointAccelerations[i] = jointValues;
00819         jointTorques[i] = jointValues;
00820     }
00821 
00822     // Initial arm position configuration/constraint
00823     JntArray jointInitialPose(chaindyn.getNrOfJoints());
00824     jointInitialPose(0) = 0.0; // initial joint0 pose
00825     jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
00826     //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
00827     //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
00828 
00829     //actual
00830     jointPoses[0](0) = jointInitialPose(0);
00831     jointPoses[0](1) = jointInitialPose(1);
00832 
00833     //~Definition of solver and initial configuration
00834     //-------------------------------------------------------------------------------------//
00835 
00836 
00837     //Definition of process main loop
00838     //-------------------------------------------------------------------------------------//
00839     //Time required to complete the task move(frameinitialPose, framefinalPose)
00840     double taskTimeConstant = 0.1;
00841     double simulationTime = 1*taskTimeConstant;
00842     double timeDelta = 0.01;
00843 
00844     const std::string msg = "Assertion failed, check matrix and array sizes";
00845 
00846     for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
00847     {
00848         CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]));
00849 
00850         //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
00851         jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
00852         jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
00853         jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
00854         jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
00855         //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
00856         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));
00857     }
00858 }
00859 
00860 void SolverTest::FkPosVectTest()
00861 {
00862     ChainFkSolverPos_recursive fksolver1(chain1);
00863     std::vector<Frame> v_out(chain1.getNrOfSegments());
00864     
00865     JntArray q(chain1.getNrOfJoints());
00866     JntArray qdot(chain1.getNrOfJoints());
00867 
00868     for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
00869     {
00870         random(q(i));
00871         random(qdot(i));
00872     }
00873     Frame f_out;
00874     fksolver1.JntToCart(q,v_out);
00875     fksolver1.JntToCart(q,f_out);
00876      
00877     CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
00878 }
00879 
00880 void SolverTest::FkVelVectTest()
00881 {
00882     ChainFkSolverVel_recursive fksolver1(chain1);
00883     std::vector<FrameVel> v_out(chain1.getNrOfSegments());
00884     
00885     JntArray q(chain1.getNrOfJoints());
00886     JntArray qdot(chain1.getNrOfJoints());
00887 
00888     for(unsigned int i=0; i<chain1.getNrOfJoints(); i++)
00889     {
00890         random(q(i));
00891         random(qdot(i));
00892     }
00893     JntArrayVel qvel(q,qdot);
00894     FrameVel f_out;
00895     fksolver1.JntToCart(qvel,v_out);
00896     fksolver1.JntToCart(qvel,f_out);
00897     
00898     CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
00899 }
00900 
00901 void SolverTest::FdSolverDevelopmentTest()
00902 {
00903     int ret;
00904     double eps=1.e-3;
00905 
00906     std::cout<<"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
00907 
00908     //  NOTE:  This is prototype code for the KDL forward dynamics solver class
00909     //         based on the Recurse Newton Euler Method:  ChainFdSolver_RNE
00910 
00911     //  Dynamics Solver
00912     Vector gravity(0.0, 0.0, -9.81);  // base frame
00913     ChainDynParam DynSolver = KDL::ChainDynParam(motomansia10dyn, gravity);
00914 
00915     unsigned int nj = motomansia10dyn.getNrOfJoints();
00916     unsigned int ns = motomansia10dyn.getNrOfSegments();
00917 
00918     // Joint position, velocity, and acceleration
00919     JntArray q(nj);
00920     JntArray qd(nj);
00921     JntArray qdd(nj);
00922 
00923     //  random
00924     q(0) = 0.2;
00925     q(1) = 0.6;
00926     q(2) = 1.;
00927     q(3) = 0.5;
00928     q(4) = -1.4;
00929     q(5) = 0.3;
00930     q(6) = -0.8;
00931 
00932     qd(0) = 1.;
00933     qd(1) = -2.;
00934     qd(2) = 3.;
00935     qd(3) = -4.;
00936     qd(4) = 5.;
00937     qd(5) = -6.;
00938     qd(6) = 7.;
00939 
00940     // Validate FK
00941     ChainFkSolverPos_recursive fksolver(motomansia10dyn);
00942     Frame f_out;
00943     fksolver.JntToCart(q,f_out);
00944     CPPUNIT_ASSERT(Equal(-0.547, f_out.p(0), eps));
00945     CPPUNIT_ASSERT(Equal(-0.301, f_out.p(1), eps));
00946     CPPUNIT_ASSERT(Equal(0.924, f_out.p(2), eps));
00947     CPPUNIT_ASSERT(Equal(0.503, f_out.M(0,0), eps));
00948     CPPUNIT_ASSERT(Equal(0.286, f_out.M(0,1), eps));
00949     CPPUNIT_ASSERT(Equal(-0.816, f_out.M(0,2), eps));
00950     CPPUNIT_ASSERT(Equal(-0.859, f_out.M(1,0), eps));
00951     CPPUNIT_ASSERT(Equal(0.269, f_out.M(1,1), eps));
00952     CPPUNIT_ASSERT(Equal(-0.436, f_out.M(1,2), eps));
00953     CPPUNIT_ASSERT(Equal(0.095, f_out.M(2,0), eps));
00954     CPPUNIT_ASSERT(Equal(0.920, f_out.M(2,1), eps));
00955     CPPUNIT_ASSERT(Equal(0.381, f_out.M(2,2), eps));
00956 
00957     // Validate Jacobian
00958     ChainJntToJacSolver jacsolver(motomansia10dyn);
00959     Jacobian jac(nj);
00960     jacsolver.JntToJac(q, jac);
00961     double Jac[6][7] =
00962         {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
00963         {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
00964         {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
00965         {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
00966         {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
00967         {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
00968     for ( int i=0; i<6; i++ ) {
00969         for ( int j=0; j<nj; j++ ) {
00970             CPPUNIT_ASSERT(Equal(jac(i,j), Jac[i][j], eps));
00971         }
00972     }
00973 
00974     // Return values
00975     JntArray taugrav(nj);
00976     JntArray taucor(nj);
00977     JntSpaceInertiaMatrix H(nj), Heff(nj);
00978 
00979     // Compute Dynamics (torque in N-m)
00980     ret = DynSolver.JntToGravity(q, taugrav);
00981     if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
00982     CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
00983     CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
00984     CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
00985     CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
00986     CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
00987     CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
00988     CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));
00989 
00990     ret = DynSolver.JntToCoriolis(q, qd, taucor);
00991     if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
00992     CPPUNIT_ASSERT(Equal(-15.523, taucor(0), eps));
00993     CPPUNIT_ASSERT(Equal(24.250, taucor(1), eps));
00994     CPPUNIT_ASSERT(Equal(-6.862, taucor(2), eps));
00995     CPPUNIT_ASSERT(Equal(6.303, taucor(3), eps));
00996     CPPUNIT_ASSERT(Equal(0.110, taucor(4), eps));
00997     CPPUNIT_ASSERT(Equal(-4.898, taucor(5), eps));
00998     CPPUNIT_ASSERT(Equal(-0.249, taucor(6), eps));
00999 
01000     ret = DynSolver.JntToMass(q, H);
01001     if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
01002     double Hexp[7][7] =
01003         {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
01004         {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
01005         {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
01006         {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
01007         {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
01008         {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
01009         {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
01010     for ( int i=0; i<nj; i++ ) {
01011         for ( int j=0; j<nj; j++ ) {
01012             CPPUNIT_ASSERT(Equal(H(i,j), Hexp[i][j], eps));
01013         }
01014     }
01015 
01016     // Inverse Dynamics:
01017     //   T = H * qdd + Tcor + Tgrav - J^T * Fext
01018     // Forward Dynamics
01019     //   1. Call JntToMass from ChainDynParam to get H
01020     //   2. Call ID with qdd=0 to get T=Tcor+Tgrav+J^T*Fext
01021     //   3. Calculate qdd = H^-1 * T
01022     KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
01023 
01024     // In tool coordinates
01025     Vector f(10,-20,30) ;
01026     Vector n(3,-4,5) ;
01027     Wrench f_tool(f,n);
01028     // In local link coordinates
01029     Wrenches f_ext(ns);
01030     for(int i=0;i<ns;i++){
01031         SetToZero(f_ext[i]);
01032     }
01033     f_ext[ns-1]=f_tool;
01034 
01035     JntArray Tnoninertial(nj);
01036     JntArray jntarraynull(nj);
01037     SetToZero(jntarraynull);
01038     IdSolver.CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
01039     CPPUNIT_ASSERT(Equal(-21.252, Tnoninertial(0), eps));
01040     CPPUNIT_ASSERT(Equal(-37.933, Tnoninertial(1), eps));
01041     CPPUNIT_ASSERT(Equal(-2.497, Tnoninertial(2), eps));
01042     CPPUNIT_ASSERT(Equal(-15.289, Tnoninertial(3), eps));
01043     CPPUNIT_ASSERT(Equal(-4.646, Tnoninertial(4), eps));
01044     CPPUNIT_ASSERT(Equal(-9.201, Tnoninertial(5), eps));
01045     CPPUNIT_ASSERT(Equal(-5.249, Tnoninertial(6), eps));
01046 
01047     // get acceleration using inverse symmetric matrix times vector
01048     Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
01049     Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
01050     for(int i=0;i<nj;i++){
01051         Tnon_eig(i) =  -Tnoninertial(i);
01052         for(int j=0;j<nj;j++){
01053             H_eig(i,j) =  H(i,j);
01054         }
01055     }
01056     ldl_solver_eigen(H_eig, Tnon_eig, L, D, r, acc_eig);
01057     for(int i=0;i<nj;i++){
01058         qdd(i) = acc_eig(i);
01059     }
01060     CPPUNIT_ASSERT(Equal(2.998, qdd(0), eps));
01061     CPPUNIT_ASSERT(Equal(4.289, qdd(1), eps));
01062     CPPUNIT_ASSERT(Equal(0.946, qdd(2), eps));
01063     CPPUNIT_ASSERT(Equal(2.518, qdd(3), eps));
01064     CPPUNIT_ASSERT(Equal(3.530, qdd(4), eps));
01065     CPPUNIT_ASSERT(Equal(8.150, qdd(5), eps));
01066     CPPUNIT_ASSERT(Equal(4.254, qdd(6), eps));
01067 }
01068 
01069 void SolverTest::FdSolverConsistencyTest()
01070 {
01071     int ret;
01072     double eps=1.e-3;
01073 
01074     std::cout<<"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
01075 
01076     //  NOTE:  Compute the forward and inverse dynamics and test for consistency
01077 
01078     //  Forward Dynamics Solver
01079     Vector gravity(0.0, 0.0, -9.81);  // base frame
01080     KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(motomansia10dyn, gravity);
01081 
01082     unsigned int nj = motomansia10dyn.getNrOfJoints();
01083     unsigned int ns = motomansia10dyn.getNrOfSegments();
01084 
01085     // Joint position, velocity, and acceleration
01086     KDL::JntArray q(nj);
01087     KDL::JntArray qd(nj);
01088     KDL::JntArray qdd(nj);
01089     KDL::JntArray tau(nj);
01090 
01091     //  random
01092     q(0) = 0.2;
01093     q(1) = 0.6;
01094     q(2) = 1.;
01095     q(3) = 0.5;
01096     q(4) = -1.4;
01097     q(5) = 0.3;
01098     q(6) = -0.8;
01099 
01100     qd(0) = 1.;
01101     qd(1) = -2.;
01102     qd(2) = 3.;
01103     qd(3) = -4.;
01104     qd(4) = 5.;
01105     qd(5) = -6.;
01106     qd(6) = 7.;
01107 
01108     // actuator torques
01109     tau(0) = 50.;
01110     tau(1) = -20.;
01111     tau(2) = 10.;
01112     tau(3) = 40.;
01113     tau(4) = -60.;
01114     tau(5) = 15.;
01115     tau(6) = -10.;
01116 
01117     KDL::Vector f(10,-20,30) ;
01118     KDL::Vector n(3,-4,5) ;
01119     KDL::Wrench f_tool(f,n);
01120     // In local link coordinates
01121     KDL::Wrenches f_ext(ns);
01122     for(int i=0;i<ns;i++){
01123         SetToZero(f_ext[i]);
01124     }
01125     f_ext[ns-1]=f_tool;
01126 
01127     // Call FD function
01128     ret = FdSolver.CartToJnt(q, qd, tau, f_ext, qdd);
01129     if (ret < 0) std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
01130     CPPUNIT_ASSERT(Equal(9.486, qdd(0), eps));
01131     CPPUNIT_ASSERT(Equal(1.830, qdd(1), eps));
01132     CPPUNIT_ASSERT(Equal(4.726, qdd(2), eps));
01133     CPPUNIT_ASSERT(Equal(11.665, qdd(3), eps));
01134     CPPUNIT_ASSERT(Equal(-50.108, qdd(4), eps));
01135     CPPUNIT_ASSERT(Equal(21.403, qdd(5), eps));
01136     CPPUNIT_ASSERT(Equal(-0.381, qdd(6), eps));
01137 
01138     // Check against ID solver for consistency
01139     KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
01140     KDL::JntArray torque(nj);
01141     IdSolver.CartToJnt(q, qd, qdd, f_ext, torque);
01142     for ( int i=0; i<nj; i++ )
01143     {
01144         CPPUNIT_ASSERT(Equal(torque(i), tau(i), eps));
01145     }
01146 
01147     return;
01148 }
01149 
01150 void SolverTest::LDLdecompTest()
01151 {
01152     std::cout<<"LDL Solver Test"<<std::endl;
01153     double eps=1.e-6;
01154 
01155     //  Given A and b, solve Ax=b for x, where A is a symmetric real matrix
01156     //  https://en.wikipedia.org/wiki/Cholesky_decomposition
01157     Eigen::MatrixXd A(3,3), Aout(3,3);
01158     Eigen::VectorXd b(3);
01159     Eigen::MatrixXd L(3,3), Lout(3,3);
01160     Eigen::VectorXd d(3), dout(3);
01161     Eigen::VectorXd x(3), xout(3);
01162     Eigen::VectorXd r(3);  // temp variable used internally by ldl solver
01163     Eigen::MatrixXd Dout(3,3);  // diagonal matrix
01164 
01165     // Given
01166     A <<  4, 12, -16,
01167          12, 37, -43,
01168         -16, -43, 98;
01169     b << 28, 117, 98;
01170     // Results to verify
01171     L <<  1, 0, 0,
01172           3, 1, 0,
01173           -4, 5, 1;
01174     d << 4, 1, 9;
01175     x << 3, 8, 5;
01176 
01177     ldl_solver_eigen(A, b, Lout, dout, r, xout);
01178 
01179     for(int i=0;i<3;i++){
01180         for(int j=0;j<3;j++){
01181             CPPUNIT_ASSERT(Equal(L(i,j), Lout(i,j), eps));
01182         }
01183     }
01184 
01185     Dout.setZero();
01186     for(int i=0;i<3;i++){
01187         Dout(i,i) = dout(i);
01188     }
01189 
01190     // Verify solution for x
01191     for(int i=0;i<3;i++){
01192         CPPUNIT_ASSERT(Equal(xout(i), x(i), eps));
01193     }
01194 
01195     // Test reconstruction of A from LDL^T decomposition
01196     Aout = Lout * Dout * Lout.transpose();
01197     for(int i=0;i<3;i++){
01198         for(int j=0;j<3;j++){
01199             CPPUNIT_ASSERT(Equal(A(i,j), Aout(i,j), eps));
01200         }
01201     }
01202 
01203     return;
01204 }


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