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 motomansia10.addSegment(Segment(Joint(Joint::None),
00107 Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0)));
00108 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00109 Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00110 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00111 Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
00112 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00113 Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00114 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00115 Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
00116 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00117 Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
00118 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00119 Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0)));
00120 motomansia10.addSegment(Segment(Joint(Joint::RotZ),
00121 Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
00122 }
00123
00124 void SolverTest::tearDown()
00125 {
00126
00127
00128
00129
00130
00131 }
00132
00133 void SolverTest::FkPosAndJacTest()
00134 {
00135 ChainFkSolverPos_recursive fksolver1(chain1);
00136 ChainJntToJacSolver jacsolver1(chain1);
00137 FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
00138 ChainFkSolverPos_recursive fksolver2(chain2);
00139 ChainJntToJacSolver jacsolver2(chain2);
00140 FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
00141 ChainFkSolverPos_recursive fksolver3(chain3);
00142 ChainJntToJacSolver jacsolver3(chain3);
00143 FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
00144 ChainFkSolverPos_recursive fksolver4(chain4);
00145 ChainJntToJacSolver jacsolver4(chain4);
00146 FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
00147 }
00148
00149 void SolverTest::FkVelAndJacTest()
00150 {
00151 ChainFkSolverVel_recursive fksolver1(chain1);
00152 ChainJntToJacSolver jacsolver1(chain1);
00153 FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
00154 ChainFkSolverVel_recursive fksolver2(chain2);
00155 ChainJntToJacSolver jacsolver2(chain2);
00156 FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
00157 ChainFkSolverVel_recursive fksolver3(chain3);
00158 ChainJntToJacSolver jacsolver3(chain3);
00159 FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
00160 ChainFkSolverVel_recursive fksolver4(chain4);
00161 ChainJntToJacSolver jacsolver4(chain4);
00162 FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
00163 }
00164
00165 void SolverTest::FkVelAndIkVelTest()
00166 {
00167
00168 std::cout<<"square problem"<<std::endl;
00169 ChainFkSolverVel_recursive fksolver1(chain1);
00170 ChainIkSolverVel_pinv iksolver1(chain1);
00171 ChainIkSolverVel_pinv_givens iksolver_pinv_givens1(chain1);
00172 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00173 FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
00174 std::cout<<"KDL-SVD-Givens"<<std::endl;
00175 FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
00176
00177
00178 std::cout<<"underdetermined problem"<<std::endl;
00179 ChainFkSolverVel_recursive fksolver2(chain2);
00180 ChainIkSolverVel_pinv iksolver2(chain2);
00181 ChainIkSolverVel_pinv_givens iksolver_pinv_givens2(chain2);
00182 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00183 FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
00184 std::cout<<"KDL-SVD-Givens"<<std::endl;
00185 FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
00186
00187
00188 std::cout<<"overdetermined problem"<<std::endl;
00189 ChainFkSolverVel_recursive fksolver3(chain3);
00190 ChainIkSolverVel_pinv iksolver3(chain3);
00191 ChainIkSolverVel_pinv_givens iksolver_pinv_givens3(chain3);
00192 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00193 FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
00194 std::cout<<"KDL-SVD-Givens"<<std::endl;
00195 FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
00196
00197
00198 std::cout<<"overdetermined problem"<<std::endl;
00199 ChainFkSolverVel_recursive fksolver4(chain4);
00200 ChainIkSolverVel_pinv iksolver4(chain4);
00201 ChainIkSolverVel_pinv_givens iksolver_pinv_givens4(chain4);
00202 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00203 FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
00204 std::cout<<"KDL-SVD-Givens"<<std::endl;
00205 FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
00206 }
00207
00208 void SolverTest::FkPosAndIkPosTest()
00209 {
00210 std::cout<<"square problem"<<std::endl;
00211 ChainFkSolverPos_recursive fksolver1(chain1);
00212 ChainIkSolverVel_pinv iksolver1v(chain1);
00213 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens1(chain1);
00214 ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v);
00215 ChainIkSolverPos_NR iksolver1_givens(chain1,fksolver1,iksolverv_pinv_givens1,1000);
00216
00217 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00218 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
00219 std::cout<<"KDL-SVD-Givens"<<std::endl;
00220 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
00221
00222 std::cout<<"underdetermined problem"<<std::endl;
00223 ChainFkSolverPos_recursive fksolver2(chain2);
00224 ChainIkSolverVel_pinv iksolverv2(chain2);
00225 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens2(chain2);
00226 ChainIkSolverPos_NR iksolver2(chain2,fksolver2,iksolverv2);
00227 ChainIkSolverPos_NR iksolver2_givens(chain2,fksolver2,iksolverv_pinv_givens2,1000);
00228
00229 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00230 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
00231 std::cout<<"KDL-SVD-Givens"<<std::endl;
00232 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
00233
00234 std::cout<<"overdetermined problem"<<std::endl;
00235 ChainFkSolverPos_recursive fksolver3(chain3);
00236 ChainIkSolverVel_pinv iksolverv3(chain3);
00237 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens3(chain3);
00238 ChainIkSolverPos_NR iksolver3(chain3,fksolver3,iksolverv3);
00239 ChainIkSolverPos_NR iksolver3_givens(chain3,fksolver3,iksolverv_pinv_givens3,1000);
00240
00241 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00242 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
00243 std::cout<<"KDL-SVD-Givens"<<std::endl;
00244 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
00245
00246 std::cout<<"underdetermined problem with WGs segment constructor"<<std::endl;
00247 ChainFkSolverPos_recursive fksolver4(chain4);
00248 ChainIkSolverVel_pinv iksolverv4(chain4);
00249 ChainIkSolverVel_pinv_givens iksolverv_pinv_givens4(chain4);
00250 ChainIkSolverPos_NR iksolver4(chain4,fksolver4,iksolverv4,1000);
00251 ChainIkSolverPos_NR iksolver4_givens(chain4,fksolver4,iksolverv_pinv_givens4,1000);
00252
00253 std::cout<<"KDL-SVD-HouseHolder"<<std::endl;
00254 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
00255 std::cout<<"KDL-SVD-Givens"<<std::endl;
00256 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
00257 }
00258
00259 void SolverTest::IkSingularValueTest()
00260 {
00261 unsigned int maxiter = 30;
00262 double eps = 1e-6 ;
00263 int maxiter_vel = 30;
00264 double eps_vel = 0.1 ;
00265 Frame F, dF, F_des,F_solved;
00266 KDL::Twist F_error ;
00267
00268 std::cout<<"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
00269
00270 ChainFkSolverPos_recursive fksolver(motomansia10);
00271 ChainIkSolverVel_pinv ikvelsolver1(motomansia10,eps_vel,maxiter_vel);
00272 ChainIkSolverPos_NR iksolver1(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00273 unsigned int nj = motomansia10.getNrOfJoints();
00274 JntArray q(nj), q_solved(nj) ;
00275
00276
00277 std::cout<<"norminal case: convergence"<<std::endl;
00278
00279 q(0) = 0. ;
00280 q(1) = 0.5 ;
00281 q(2) = 0.4 ;
00282 q(3) = -M_PI_2 ;
00283 q(4) = 0. ;
00284 q(5) = 0. ;
00285 q(6) = 0. ;
00286
00287 dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00288 dF.p = KDL::Vector(0.01,0.01,0.01) ;
00289
00290 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00291 F_des = F * dF ;
00292
00293 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00294 iksolver1.CartToJnt(q, F_des, q_solved));
00295 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00296 ikvelsolver1.getError());
00297 CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00298 ikvelsolver1.getNrZeroSigmas()) ;
00299
00300 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q_solved,F_solved));
00301 F_error = KDL::diff(F_solved,F_des);
00302 CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
00303
00304 std::cout<<"nonconvergence: pseudoinverse singular"<<std::endl;
00305
00306 q(0) = 0. ;
00307 q(1) = 0.2 ;
00308 q(2) = 0.4 ;
00309 q(3) = -M_PI_2 ;
00310 q(4) = 0. ;
00311 q(5) = 0. ;
00312 q(6) = 0. ;
00313
00314 dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00315 dF.p = KDL::Vector(0.01,0.01,0.01) ;
00316
00317 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00318 F_des = F * dF ;
00319
00320 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NO_CONVERGE,
00321 iksolver1.CartToJnt(q,F_des,q_solved));
00322 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00323 ikvelsolver1.getError());
00324 CPPUNIT_ASSERT_EQUAL((unsigned int)2,
00325 ikvelsolver1.getNrZeroSigmas()) ;
00326
00327 std::cout<<"nonconvergence: large displacement, low iterations"<<std::endl;
00328
00329 q(0) = 0. ;
00330 q(1) = 0.5 ;
00331 q(2) = 0.4 ;
00332 q(3) = -M_PI_2 ;
00333 q(4) = 0. ;
00334 q(5) = 0. ;
00335 q(6) = 0. ;
00336
00337
00338 dF.M = KDL::Rotation::RPY(0.2, 0.2, 0.2) ;
00339 dF.p = KDL::Vector(-0.2,-0.2, -0.2) ;
00340
00341
00342 maxiter = 5 ;
00343 ChainIkSolverPos_NR iksolver2(motomansia10,fksolver,ikvelsolver1,maxiter,eps);
00344
00345 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00346 F_des = F * dF ;
00347
00348 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NO_CONVERGE,
00349 iksolver2.CartToJnt(q,F_des,q_solved));
00350 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00351 ikvelsolver1.getError());
00352 CPPUNIT_ASSERT_EQUAL((unsigned int)1,
00353 ikvelsolver1.getNrZeroSigmas()) ;
00354
00355 std::cout<<"nonconvergence: fully singular"<<std::endl;
00356
00357 q(0) = 0. ;
00358 q(1) = 0. ;
00359 q(2) = 0. ;
00360 q(3) = 0. ;
00361 q(4) = 0. ;
00362 q(5) = 0. ;
00363 q(6) = 0. ;
00364
00365 dF.M = KDL::Rotation::RPY(0.1, 0.1, 0.1) ;
00366 dF.p = KDL::Vector(0.01,0.01,0.01) ;
00367
00368 CPPUNIT_ASSERT_EQUAL(0, fksolver.JntToCart(q,F));
00369 F_des = F * dF ;
00370
00371 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NO_CONVERGE,
00372 iksolver1.CartToJnt(q,F_des,q_solved));
00373 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
00374 ikvelsolver1.getError());
00375 CPPUNIT_ASSERT_EQUAL((unsigned int)3,
00376 ikvelsolver1.getNrZeroSigmas());
00377 }
00378
00379
00380 void SolverTest::IkVelSolverWDLSTest()
00381 {
00382 int maxiter = 30;
00383 double eps = 0.1 ;
00384 double lambda = 0.1 ;
00385
00386 std::cout<<"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
00387
00388 KDL::ChainIkSolverVel_wdls ikvelsolver(motomansia10,eps,maxiter) ;
00389 ikvelsolver.setLambda(lambda) ;
00390 unsigned int nj = motomansia10.getNrOfJoints();
00391 JntArray q(nj), dq(nj) ;
00392
00393 KDL::Vector v05(0.05,0.05,0.05) ;
00394 KDL::Twist dx(v05,v05) ;
00395
00396 std::cout<<"smallest singular value is above threshold (no WDLS)"<<std::endl;
00397
00398 q(0) = 0. ;
00399 q(1) = 0.5 ;
00400 q(2) = 0.4 ;
00401 q(3) = -M_PI_2 ;
00402 q(4) = 0. ;
00403 q(5) = 0. ;
00404 q(6) = 0. ;
00405
00406 CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,
00407 ikvelsolver.CartToJnt(q, dx, dq)) ;
00408 CPPUNIT_ASSERT(1==ikvelsolver.getNrZeroSigmas()) ;
00409
00410
00411 std::cout<<"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
00412
00413 q(1) = 0.2 ;
00414
00415 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00416 ikvelsolver.CartToJnt(q, dx, dq)) ;
00417 CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;
00418 CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),
00419 sqrt(1.0-(ikvelsolver.getSigmaMin()/eps)*(ikvelsolver.getSigmaMin()/eps))*lambda) ;
00420
00421 std::cout<<"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
00422
00423 q(1) = 0.0 ;
00424
00425 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00426 ikvelsolver.CartToJnt(q, dx, dq)) ;
00427 CPPUNIT_ASSERT_EQUAL((unsigned int)2,ikvelsolver.getNrZeroSigmas()) ;
00428 CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;
00429
00430
00431 q(2) = 0.0 ;
00432 q(3) = 0.0 ;
00433
00434 CPPUNIT_ASSERT_EQUAL((int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
00435 ikvelsolver.CartToJnt(q, dx, dq)) ;
00436 CPPUNIT_ASSERT_EQUAL(4,(int)ikvelsolver.getNrZeroSigmas()) ;
00437 CPPUNIT_ASSERT_EQUAL(ikvelsolver.getLambdaScaled(),lambda) ;
00438 }
00439
00440
00441 void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
00442 {
00443 double deltaq = 1E-4;
00444
00445 Frame F1,F2;
00446
00447 JntArray q(chain.getNrOfJoints());
00448 Jacobian jac(chain.getNrOfJoints());
00449
00450 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00451 {
00452 random(q(i));
00453 }
00454
00455 jacsolver.JntToJac(q,jac);
00456
00457 for (unsigned int i=0; i< q.rows() ; i++)
00458 {
00459
00460 double oldqi = q(i);
00461 q(i) = oldqi+deltaq;
00462 CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F2));
00463 q(i) = oldqi-deltaq;
00464 CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
00465 q(i) = oldqi;
00466
00467 Twist Jcol1 = diff(F1,F2,2*deltaq);
00468 Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
00469 Vector(jac(3,i),jac(4,i),jac(5,i)));
00470
00471
00472 CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
00473 }
00474 }
00475
00476 void SolverTest::FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver)
00477 {
00478 JntArray q(chain.getNrOfJoints());
00479 JntArray qdot(chain.getNrOfJoints());
00480
00481 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00482 {
00483 random(q(i));
00484 random(qdot(i));
00485 }
00486 JntArrayVel qvel(q,qdot);
00487 Jacobian jac(chain.getNrOfJoints());
00488
00489 FrameVel cart;
00490 Twist t;
00491
00492 jacsolver.JntToJac(qvel.q,jac);
00493 CPPUNIT_ASSERT(fksolvervel.JntToCart(qvel,cart)==0);
00494 MultiplyJacobian(jac,qvel.qdot,t);
00495 CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
00496 }
00497
00498 void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
00499 {
00500
00501 JntArray q(chain.getNrOfJoints());
00502 JntArray qdot(chain.getNrOfJoints());
00503
00504 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00505 {
00506 random(q(i));
00507 random(qdot(i));
00508 }
00509 JntArrayVel qvel(q,qdot);
00510 JntArray qdot_solved(chain.getNrOfJoints());
00511
00512 FrameVel cart;
00513
00514 CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart));
00515
00516 int ret = iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved);
00517 CPPUNIT_ASSERT(0<=ret);
00518
00519 qvel.deriv()=qdot_solved;
00520
00521 if(chain.getNrOfJoints()<=6)
00522 CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
00523 else
00524 {
00525 FrameVel cart_solved;
00526 CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart_solved));
00527 CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
00528 }
00529 }
00530
00531
00532 void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
00533 {
00534 JntArray q(chain.getNrOfJoints());
00535 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00536 {
00537 random(q(i));
00538 }
00539 JntArray q_init(chain.getNrOfJoints());
00540 double tmp;
00541 for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
00542 {
00543 random(tmp);
00544 q_init(i)=q(i)+0.1*tmp;
00545 }
00546 JntArray q_solved(q);
00547
00548 Frame F1,F2;
00549
00550 CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
00551 CPPUNIT_ASSERT(0 <= iksolverpos.CartToJnt(q_init,F1,q_solved));
00552 CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q_solved,F2));
00553
00554 CPPUNIT_ASSERT_EQUAL(F1,F2);
00555
00556
00557 }
00558
00559
00560 void SolverTest::VereshchaginTest()
00561 {
00562
00563 Vector constrainXLinear(1.0, 0.0, 0.0);
00564 Vector constrainXAngular(0.0, 0.0, 0.0);
00565 Vector constrainYLinear(0.0, 0.0, 0.0);
00566 Vector constrainYAngular(0.0, 0.0, 0.0);
00567
00568
00569 Twist constraintForcesX(constrainXLinear, constrainXAngular);
00570 Twist constraintForcesY(constrainYLinear, constrainYAngular);
00571
00572 Jacobian alpha(1);
00573
00574 alpha.setColumn(0, constraintForcesX);
00575
00576
00577
00578 JntArray betha(1);
00579 betha(0) = 0.0;
00580
00581
00582
00583
00584 Vector linearAcc(0.0, 10, 0.0);
00585 Vector angularAcc(0.0, 0.0, 0.0);
00586 Twist twist1(linearAcc, angularAcc);
00587
00588
00589 Vector externalForce1(0.0, 0.0, 0.0);
00590 Vector externalTorque1(0.0, 0.0, 0.0);
00591 Vector externalForce2(0.0, 0.0, 0.0);
00592 Vector externalTorque2(0.0, 0.0, 0.0);
00593 Wrench externalNetForce1(externalForce1, externalTorque1);
00594 Wrench externalNetForce2(externalForce2, externalTorque2);
00595 Wrenches externalNetForce;
00596 externalNetForce.push_back(externalNetForce1);
00597 externalNetForce.push_back(externalNetForce2);
00598
00599
00600
00601
00602
00603
00604 int numberOfConstraints = 1;
00605 ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
00606
00607
00608
00609
00610
00611
00612 const int k = 1;
00613 JntArray jointPoses[k];
00614 JntArray jointRates[k];
00615 JntArray jointAccelerations[k];
00616 JntArray jointTorques[k];
00617 for (int i = 0; i < k; i++)
00618 {
00619 JntArray jointValues(chaindyn.getNrOfJoints());
00620 jointPoses[i] = jointValues;
00621 jointRates[i] = jointValues;
00622 jointAccelerations[i] = jointValues;
00623 jointTorques[i] = jointValues;
00624 }
00625
00626
00627 JntArray jointInitialPose(chaindyn.getNrOfJoints());
00628 jointInitialPose(0) = 0.0;
00629 jointInitialPose(1) = M_PI/6.0;
00630
00631
00632
00633
00634 jointPoses[0](0) = jointInitialPose(0);
00635 jointPoses[0](1) = jointInitialPose(1);
00636
00637
00638
00639
00640
00641
00642
00643
00644 double taskTimeConstant = 0.1;
00645 double simulationTime = 1*taskTimeConstant;
00646 double timeDelta = 0.01;
00647 int status;
00648
00649 const std::string msg = "Assertion failed, check matrix and array sizes";
00650
00651 for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
00652 {
00653 status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]);
00654
00655 CPPUNIT_ASSERT((status == 0));
00656 if (status != 0)
00657 {
00658 std::cout << "Check matrix and array sizes. Something does not match " << std::endl;
00659 exit(1);
00660 }
00661 else
00662 {
00663
00664 jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta;
00665 jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta;
00666 jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta;
00667 jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
00668
00669 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));
00670 }
00671 }
00672 }