orocos_kdl: orocos_kdl: solvertest.cpp Source 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 }
00106 
00107 void SolverTest::tearDown()
00108 {
00109 //     delete fksolverpos;
00110 //     delete jacsolver;
00111 //     delete fksolvervel;
00112 //     delete iksolvervel;
00113 //     delete iksolverpos;
00114 }
00115 
00116 void SolverTest::FkPosAndJacTest()
00117 {
00118     ChainFkSolverPos_recursive fksolver1(chain1);
00119     ChainJntToJacSolver jacsolver1(chain1);
00120     FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
00121     ChainFkSolverPos_recursive fksolver2(chain2);
00122     ChainJntToJacSolver jacsolver2(chain2);
00123     FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
00124     ChainFkSolverPos_recursive fksolver3(chain3);
00125     ChainJntToJacSolver jacsolver3(chain3);
00126     FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
00127     ChainFkSolverPos_recursive fksolver4(chain4);
00128     ChainJntToJacSolver jacsolver4(chain4);
00129     FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
00130 }
00131 
00132 void SolverTest::FkVelAndJacTest()
00133 {
00134     ChainFkSolverVel_recursive fksolver1(chain1);
00135     ChainJntToJacSolver jacsolver1(chain1);
00136     FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
00137     ChainFkSolverVel_recursive fksolver2(chain2);
00138     ChainJntToJacSolver jacsolver2(chain2);
00139     FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
00140     ChainFkSolverVel_recursive fksolver3(chain3);
00141     ChainJntToJacSolver jacsolver3(chain3);
00142     FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
00143     ChainFkSolverVel_recursive fksolver4(chain4);
00144     ChainJntToJacSolver jacsolver4(chain4);
00145     FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
00146 }
00147 
00148 void SolverTest::FkVelAndIkVelTest()
00149 {
00150     //Chain1
00151     std::cout<<"square problem"<<std::endl;
00152     ChainFkSolverVel_recursive fksolver1(chain1);
00153     ChainIkSolverVel_pinv iksolver1(chain1);
00154     ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
00155     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00156     FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
00157     std::cout<<"KDL-SVD-Givens"<<std::endl;
00158     FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
00159 
00160     //Chain2
00161     std::cout<<"underdetermined problem"<<std::endl;
00162     ChainFkSolverVel_recursive fksolver2(chain2);
00163     ChainIkSolverVel_pinv iksolver2(chain2);
00164     ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00165     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00166     FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
00167     std::cout<<"KDL-SVD-Givens"<<std::endl;
00168     FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
00169 
00170     //Chain3
00171     std::cout<<"overdetermined problem"<<std::endl;
00172     ChainFkSolverVel_recursive fksolver3(chain3);
00173     ChainIkSolverVel_pinv iksolver3(chain3);
00174     ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
00175     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00176     FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
00177     std::cout<<"KDL-SVD-Givens"<<std::endl;
00178     FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
00179 
00180     //Chain4
00181     std::cout<<"overdetermined problem"<<std::endl;
00182     ChainFkSolverVel_recursive fksolver4(chain4);
00183     ChainIkSolverVel_pinv iksolver4(chain4);
00184     ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
00185     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00186     FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
00187     std::cout<<"KDL-SVD-Givens"<<std::endl;
00188     FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
00189 }
00190 
00191 void SolverTest::FkPosAndIkPosTest()
00192 {
00193     std::cout<<"square problem"<<std::endl;
00194     ChainFkSolverPos_recursive fksolver1(chain1);
00195     ChainIkSolverVel_pinv iksolver1v(chain1);
00196     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
00197     ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
00198     ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
00199 
00200     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00201     FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
00202     std::cout<<"KDL-SVD-Givens"<<std::endl;
00203     FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
00204 
00205     std::cout<<"underdetermined problem"<<std::endl;
00206     ChainFkSolverPos_recursive fksolver2(chain2);
00207     ChainIkSolverVel_pinv iksolverv2(chain2);
00208     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
00209     ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
00210     ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
00211 
00212     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00213     FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
00214     std::cout<<"KDL-SVD-Givens"<<std::endl;
00215     FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
00216 
00217     std::cout<<"overdetermined problem"<<std::endl;
00218     ChainFkSolverPos_recursive fksolver3(chain3);
00219     ChainIkSolverVel_pinv iksolverv3(chain3);
00220     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
00221     ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
00222     ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
00223 
00224     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00225     FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
00226     std::cout<<"KDL-SVD-Givens"<<std::endl;
00227     FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
00228 
00229     std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
00230     ChainFkSolverPos_recursive fksolver4(chain4);
00231     ChainIkSolverVel_pinv iksolverv4(chain4);
00232     ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
00233     ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
00234     ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
00235 
00236     std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00237     FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
00238     std::cout<<"KDL-SVD-Givens"<<std::endl;
00239     FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
00240 }
00241 
00242 
00243 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
00244 {
00245     double deltaq = 1E-4;
00246     double epsJ   = 1E-4;
00247 
00248     Frame F1,F2;
00249 
00250     JntArray q(chain.getNrOfJoints());
00251     Jacobian jac(chain.getNrOfJoints());
00252 
00253     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00254     {
00255         random(q(i));
00256     }
00257 
00258     jacsolver.JntToJac(q,jac);
00259 
00260     for (int i=0; i< q.rows() ; i++)
00261     {
00262         // test the derivative of J towards qi
00263         double oldqi = q(i);
00264         q(i) = oldqi+deltaq;
00265         CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F2));
00266         q(i) = oldqi-deltaq;
00267         CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
00268         q(i) = oldqi;
00269         // check Jacobian :
00270         Twist Jcol1 = diff(F1,F2,2*deltaq);
00271         Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
00272                     Vector(jac(3,i),jac(4,i),jac(5,i)));
00273 
00274         //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
00275         CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
00276     }
00277 }
00278 
00279 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
00280 {
00281     double deltaq = 1E-4;
00282     double epsJ   = 1E-4;
00283 
00284     JntArray q(chain.getNrOfJoints());
00285     JntArray qdot(chain.getNrOfJoints());
00286 
00287     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00288     {
00289         random(q(i));
00290         random(qdot(i));
00291     }
00292     JntArrayVel qvel(q,qdot);
00293     Jacobian jac(chain.getNrOfJoints());
00294 
00295     FrameVel cart;
00296     Twist t;
00297 
00298     jacsolver.JntToJac(qvel.q,jac);
00299     CPPUNIT_ASSERT(fksolvervel.JntToCart(qvel,cart)==0);
00300     MultiplyJacobian(jac,qvel.qdot,t);
00301     CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
00302 }
00303 
00304 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
00305 {
00306     double epsJ   = 1E-7;
00307 
00308     JntArray q(chain.getNrOfJoints());
00309     JntArray qdot(chain.getNrOfJoints());
00310 
00311     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00312     {
00313         random(q(i));
00314         random(qdot(i));
00315     }
00316     JntArrayVel qvel(q,qdot);
00317     JntArray qdot_solved(chain.getNrOfJoints());
00318 
00319     FrameVel cart;
00320 
00321     CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart));
00322 
00323     int ret = iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved);
00324     CPPUNIT_ASSERT(0<=ret);
00325 
00326     qvel.deriv()=qdot_solved;
00327 
00328     if(chain.getNrOfJoints()<=6)
00329         CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
00330     else
00331     {
00332         FrameVel cart_solved;
00333         CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart_solved));
00334         CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
00335     }
00336 }
00337 
00338 
00339 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
00340 {
00341     JntArray q(chain.getNrOfJoints());
00342     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00343     {
00344         random(q(i));
00345     }
00346     JntArray q_init(chain.getNrOfJoints());
00347     double tmp;
00348     for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00349     {
00350         random(tmp);
00351         q_init(i)=q(i)+0.1*tmp;
00352     }
00353     JntArray q_solved(q);
00354 
00355     Frame F1,F2;
00356 
00357     CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
00358     CPPUNIT_ASSERT(0==iksolverpos.CartToJnt(q_init,F1,q_solved));
00359     CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q_solved,F2));
00360 
00361     CPPUNIT_ASSERT_EQUAL(F1,F2);
00362     //CPPUNIT_ASSERT_EQUAL(q,q_solved);
00363 
00364 }
00365 
00366 
00367 void SolverTest::VereshchaginTest()
00368 {
00369 
00370     Vector constrainXLinear(1.0, 0.0, 0.0);
00371     Vector constrainXAngular(0.0, 0.0, 0.0);
00372     Vector constrainYLinear(0.0, 0.0, 0.0);
00373     Vector constrainYAngular(0.0, 0.0, 0.0);
00374     // Vector constrainZLinear(0.0, 0.0, 0.0);
00375     //Vector constrainZAngular(0.0, 0.0, 0.0);
00376     Twist constraintForcesX(constrainXLinear, constrainXAngular);
00377     Twist constraintForcesY(constrainYLinear, constrainYAngular);
00378     //Twist constraintForcesZ(constrainZLinear, constrainZAngular);
00379     Jacobian alpha(1);
00380     //alpha.setColumn(0, constraintForcesX);
00381     alpha.setColumn(0, constraintForcesX);
00382     //alpha.setColumn(0, constraintForcesZ);
00383 
00384     //Acceleration energy at  the end-effector
00385     JntArray betha(1); //set to zero
00386     betha(0) = 0.0;
00387     //betha(1) = 0.0;
00388     //betha(2) = 0.0;
00389 
00390     //arm root acceleration
00391     Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
00392     Vector angularAcc(0.0, 0.0, 0.0);
00393     Twist twist1(linearAcc, angularAcc);
00394 
00395     //external forces on the arm
00396     Vector externalForce1(0.0, 0.0, 0.0);
00397     Vector externalTorque1(0.0, 0.0, 0.0);
00398     Vector externalForce2(0.0, 0.0, 0.0);
00399     Vector externalTorque2(0.0, 0.0, 0.0);
00400     Wrench externalNetForce1(externalForce1, externalTorque1);
00401     Wrench externalNetForce2(externalForce2, externalTorque2);
00402     Wrenches externalNetForce;
00403     externalNetForce.push_back(externalNetForce1);
00404     externalNetForce.push_back(externalNetForce2);
00405     //~Definition of constraints and external disturbances
00406     //-------------------------------------------------------------------------------------//
00407 
00408 
00409     //Definition of solver and initial configuration
00410     //-------------------------------------------------------------------------------------//
00411     int numberOfConstraints = 1;
00412     ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
00413 
00414     //These arrays of joint values contain actual and desired values
00415     //actual is generated by a solver and integrator
00416     //desired is given by an interpolator
00417     //error is the difference between desired-actual
00418     //in this test only the actual values are printed.
00419     int k = 1;
00420     JntArray jointPoses[k];
00421     JntArray jointRates[k];
00422     JntArray jointAccelerations[k];
00423     JntArray jointTorques[k];
00424     for (int i = 0; i < k; i++)
00425     {
00426         JntArray jointValues(chaindyn.getNrOfJoints());
00427         jointPoses[i] = jointValues;
00428         jointRates[i] = jointValues;
00429         jointAccelerations[i] = jointValues;
00430         jointTorques[i] = jointValues;
00431     }
00432 
00433     // Initial arm position configuration/constraint
00434     JntArray jointInitialPose(chaindyn.getNrOfJoints());
00435     jointInitialPose(0) = 0.0; // initial joint0 pose
00436     jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
00437     //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
00438     //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
00439 
00440     //actual
00441     jointPoses[0](0) = jointInitialPose(0);
00442     jointPoses[0](1) = jointInitialPose(1);
00443 
00444     //~Definition of solver and initial configuration
00445     //-------------------------------------------------------------------------------------//
00446 
00447 
00448     //Definition of process main loop
00449     //-------------------------------------------------------------------------------------//
00450     //Time required to complete the task move(frameinitialPose, framefinalPose)
00451     double taskTimeConstant = 0.1;
00452     double simulationTime = 1*taskTimeConstant;
00453     double timeDelta = 0.01;
00454     int status;
00455 
00456     const std::string msg = "Assertion failed, check matrix and array sizes";
00457 
00458     for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
00459     {
00460         status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]);
00461 
00462         CPPUNIT_ASSERT((status == 0));
00463         if (status != 0)
00464         {
00465             std::cout << "Check matrix and array sizes. Something does not match " << std::endl;
00466             exit(1);
00467         }
00468         else
00469         {
00470             //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
00471             jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
00472             jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
00473             jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
00474             jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
00475             //printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
00476             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));
00477         }
00478     }
00479 }