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
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
00089 Segment segment1 = Segment(rotJoint0, frame1);
00090 Segment segment2 = Segment(rotJoint1, frame2);
00091
00092
00093 RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
00094
00095
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
00102 chaindyn.addSegment(segment1);
00103 chaindyn.addSegment(segment2);
00104
00105 }
00106
00107 void SolverTest::tearDown()
00108 {
00109
00110
00111
00112
00113
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
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
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
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
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
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
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
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
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
00375
00376 Twist constraintForcesX(constrainXLinear, constrainXAngular);
00377 Twist constraintForcesY(constrainYLinear, constrainYAngular);
00378
00379 Jacobian alpha(1);
00380
00381 alpha.setColumn(0, constraintForcesX);
00382
00383
00384
00385 JntArray betha(1);
00386 betha(0) = 0.0;
00387
00388
00389
00390
00391 Vector linearAcc(0.0, 10, 0.0);
00392 Vector angularAcc(0.0, 0.0, 0.0);
00393 Twist twist1(linearAcc, angularAcc);
00394
00395
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
00406
00407
00408
00409
00410
00411 int numberOfConstraints = 1;
00412 ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
00413
00414
00415
00416
00417
00418
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
00434 JntArray jointInitialPose(chaindyn.getNrOfJoints());
00435 jointInitialPose(0) = 0.0;
00436 jointInitialPose(1) = M_PI/6.0;
00437
00438
00439
00440
00441 jointPoses[0](0) = jointInitialPose(0);
00442 jointPoses[0](1) = jointInitialPose(1);
00443
00444
00445
00446
00447
00448
00449
00450
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
00471 jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta;
00472 jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta;
00473 jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta;
00474 jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
00475
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 }