15 srand( (
unsigned)time( NULL ));
103 chaindyn.addSegment(segment1);
104 chaindyn.addSegment(segment2);
127 static const double scale=1;
128 static const double offset=0;
129 static const double inertiamotorA=5.0;
130 static const double inertiamotorB=3.0;
131 static const double inertiamotorC=1.0;
132 static const double damping=0;
133 static const double stiffness=0;
193 Vector(0.0,-0.3120511,-0.0038871),
199 Vector(0.0,-0.0015515,0.0),
205 Vector(0.0,0.5216809,0.0),
211 Vector(0.0,0.0119891,0.0),
217 Vector(0.0,0.0080787,0.0),
249 unsigned int nr_of_constraints = 4;
253 JntArray q_in(chain2.getNrOfJoints());
254 JntArray q_in2(chain2.getNrOfJoints());
256 for(
unsigned int i=0; i<chain2.getNrOfJoints(); i++)
261 JntArray q_out(chain2.getNrOfJoints());
262 JntArray q_out2(chain2.getNrOfJoints());
263 JntArray ff_tau(chain2.getNrOfJoints());
264 JntArray constraint_tau(chain2.getNrOfJoints());
265 Jacobian jac(chain2.getNrOfJoints());
269 Wrenches wrenches(chain2.getNrOfSegments());
270 JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
273 Jacobian alpha(nr_of_constraints - 1);
274 JntArray beta(nr_of_constraints - 1);
276 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_OUT_OF_RANGE,fksolvervel.
JntToCart(q_in3, T2, chain2.getNrOfSegments()+1));
277 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_OUT_OF_RANGE, jacsolver1.
JntToJac(q_in, jac, chain2.getNrOfSegments()+1));
278 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.
JntToJacDot(q_in3, t, chain2.getNrOfSegments()+1));
279 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.
JntToJacDot(q_in3, jac, chain2.getNrOfSegments()+1));
283 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.
JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
284 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.
JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
285 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolver2.
CartToJnt(q_in,t,q_out));
286 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.
CartToJnt(q_in,t,q_out));
287 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.
CartToJnt(q_in,t,q_out));
288 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.
CartToJnt(q_in,t,q_out));
289 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.
CartToJnt(q_in,T,q_out));
290 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.
CartToJnt(q_in,T,q_out));
291 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.
CartToJnt(q_in,T,q_out));
292 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
293 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
294 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, dynparam.
JntToCoriolis(q_in, q_in2, q_out));
295 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, dynparam.
JntToGravity(q_in, q_out));
296 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, dynparam.
JntToMass(q_in, m));
297 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOT_UP_TO_DATE, extwrench_estimator.
JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
314 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH,fksolvervel.
JntToCart(q_in3, T2, chain2.getNrOfSegments()));
315 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, jacsolver1.
JntToJac(q_in, jac, chain2.getNrOfSegments()));
316 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.
JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
317 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.
JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
318 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver2.
CartToJnt(q_in,t,q_out));
319 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.
CartToJnt(q_in,t,q_out));
320 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.
CartToJnt(q_in,t,q_out));
321 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.
CartToJnt(q_in,t,q_out));
322 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos.
CartToJnt(q_in,T,q_out));
323 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos2.
CartToJnt(q_in,T,q_out));
324 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos3.
CartToJnt(q_in,T,q_out));
325 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
326 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
327 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToCoriolis(q_in, q_in2, q_out));
328 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToGravity(q_in, q_out));
329 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToMass(q_in, m));
330 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.
JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
332 q_in.resize(chain2.getNrOfJoints());
333 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, jacsolver1.
JntToJac(q_in, jac, chain2.getNrOfSegments()));
334 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver2.
CartToJnt(q_in,t,q_out));
335 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_givens2.
CartToJnt(q_in,t,q_out));
336 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver_pinv_nso.
CartToJnt(q_in,t,q_out));
337 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolver_wdls.
CartToJnt(q_in,t,q_out));
338 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos.
CartToJnt(q_in,T,q_out));
339 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos2.
CartToJnt(q_in,T,q_out));
340 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos3.
CartToJnt(q_in,T,q_out));
341 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos2.
CartToJnt(q_in,T,q_out));
342 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, iksolverpos3.
CartToJnt(q_in,T,q_out));
343 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
344 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
345 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToCoriolis(q_in, q_in2, q_out));
346 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToGravity(q_in, q_out));
347 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToMass(q_in, m));
348 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.
JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
349 q_in2.resize(chain2.getNrOfJoints());
350 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, dynparam.
JntToCoriolis(q_in, q_in2, q_out));
351 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
352 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
353 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.
JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
354 wrenches.resize(chain2.getNrOfSegments());
355 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
356 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
357 q_out2.resize(chain2.getNrOfSegments());
358 ff_tau.resize(chain2.getNrOfSegments());
359 constraint_tau.resize(chain2.getNrOfSegments());
360 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
361 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
362 alpha.
resize(nr_of_constraints);
363 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
364 beta.
resize(nr_of_constraints);
365 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
366 jac.resize(chain2.getNrOfJoints());
367 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, jacdotsolver1.
JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
368 q_out.resize(chain2.getNrOfJoints());
369 q_in3.resize(chain2.getNrOfJoints());
370 m.resize(chain2.getNrOfJoints());
372 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR,fksolvervel.
JntToCart(q_in3, T2, chain2.getNrOfSegments()));
373 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, jacsolver1.
JntToJac(q_in, jac, chain2.getNrOfSegments()));
374 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, jacdotsolver1.
JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
375 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, jacdotsolver1.
JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
376 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolver2.
CartToJnt(q_in,t,q_out));
377 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolver_pinv_givens2.
CartToJnt(q_in,t,q_out));
378 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolver_pinv_nso.
CartToJnt(q_in,t,q_out));
379 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolver_wdls.
CartToJnt(q_in,t,q_out));
380 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolverpos.
CartToJnt(q_in,T,q_out));
381 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolverpos2.
CartToJnt(q_in,T,q_out));
382 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolverpos3.
CartToJnt(q_in,T,q_out));
383 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolverpos2.
CartToJnt(q_in,T,q_out));
384 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= iksolverpos3.
CartToJnt(q_in,T,q_out));
385 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= idsolver1.
CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
386 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= hdsolver.
CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
387 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= dynparam.
JntToCoriolis(q_in, q_in2, q_out));
388 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= dynparam.
JntToGravity(q_in, q_out));
389 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= dynparam.
JntToMass(q_in, m));
390 CPPUNIT_ASSERT((
int)SolverI::E_NOERROR <= extwrench_estimator.
JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
396 FkPosAndJacLocal(chain1,fksolver1,jacsolver1);
399 FkPosAndJacLocal(chain2,fksolver2,jacsolver2);
402 FkPosAndJacLocal(chain3,fksolver3,jacsolver3);
405 FkPosAndJacLocal(chain4,fksolver4,jacsolver4);
412 FkVelAndJacLocal(chain1,fksolver1,jacsolver1);
415 FkVelAndJacLocal(chain2,fksolver2,jacsolver2);
418 FkVelAndJacLocal(chain3,fksolver3,jacsolver3);
421 FkVelAndJacLocal(chain4,fksolver4,jacsolver4);
427 std::cout<<
"square problem"<<std::endl;
431 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
432 FkVelAndIkVelLocal(chain1,fksolver1,iksolver1);
433 std::cout<<
"KDL-SVD-Givens"<<std::endl;
434 FkVelAndIkVelLocal(chain1,fksolver1,iksolver_pinv_givens1);
437 std::cout<<
"underdetermined problem"<<std::endl;
441 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
442 FkVelAndIkVelLocal(chain2,fksolver2,iksolver2);
443 std::cout<<
"KDL-SVD-Givens"<<std::endl;
444 FkVelAndIkVelLocal(chain2,fksolver2,iksolver_pinv_givens2);
447 std::cout<<
"overdetermined problem"<<std::endl;
451 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
452 FkVelAndIkVelLocal(chain3,fksolver3,iksolver3);
453 std::cout<<
"KDL-SVD-Givens"<<std::endl;
454 FkVelAndIkVelLocal(chain3,fksolver3,iksolver_pinv_givens3);
457 std::cout<<
"overdetermined problem"<<std::endl;
461 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
462 FkVelAndIkVelLocal(chain4,fksolver4,iksolver4);
463 std::cout<<
"KDL-SVD-Givens"<<std::endl;
464 FkVelAndIkVelLocal(chain4,fksolver4,iksolver_pinv_givens4);
469 std::cout<<
"square problem"<<std::endl;
476 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
477 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1);
478 std::cout<<
"KDL-SVD-Givens"<<std::endl;
479 FkPosAndIkPosLocal(chain1,fksolver1,iksolver1_givens);
481 std::cout<<
"underdetermined problem"<<std::endl;
488 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
489 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2);
490 std::cout<<
"KDL-SVD-Givens"<<std::endl;
491 FkPosAndIkPosLocal(chain2,fksolver2,iksolver2_givens);
493 std::cout<<
"overdetermined problem"<<std::endl;
500 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
501 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3);
502 std::cout<<
"KDL-SVD-Givens"<<std::endl;
503 FkPosAndIkPosLocal(chain3,fksolver3,iksolver3_givens);
505 std::cout<<
"underdetermined problem with WGs segment constructor"<<std::endl;
512 std::cout<<
"KDL-SVD-HouseHolder"<<std::endl;
513 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4);
514 std::cout<<
"KDL-SVD-Givens"<<std::endl;
515 FkPosAndIkPosLocal(chain4,fksolver4,iksolver4_givens);
520 unsigned int maxiter = 30;
522 int maxiter_vel = 30;
523 double eps_vel = 0.1 ;
524 Frame F, dF, F_des,F_solved;
527 std::cout<<
"KDL-IK Solver Tests for Near Zero SVs"<<std::endl;
532 unsigned int nj = motomansia10.getNrOfJoints();
536 std::cout<<
"norminal case: convergence"<<std::endl;
549 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q,F));
553 iksolver1.
CartToJnt(q, F_des, q_solved));
554 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR,
556 CPPUNIT_ASSERT_EQUAL((
unsigned int)1,
559 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q_solved,F_solved));
561 CPPUNIT_ASSERT_EQUAL(F_des,F_solved);
563 std::cout<<
"nonconvergence: pseudoinverse singular"<<std::endl;
576 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q,F));
583 CPPUNIT_ASSERT_EQUAL((
unsigned int)2,
586 std::cout<<
"nonconvergence: large displacement, low iterations"<<std::endl;
604 CPPUNIT_ASSERT_EQUAL(0, fksolver.
JntToCart(q,F));
607 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
609 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR,
611 CPPUNIT_ASSERT_EQUAL((
unsigned int)1,
614 std::cout<<
"nonconvergence: fully singular"<<std::endl;
627 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, fksolver.
JntToCart(q,F));
630 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_MAX_ITERATIONS_EXCEEDED,
632 CPPUNIT_ASSERT_EQUAL((
int)ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR,
634 CPPUNIT_ASSERT_EQUAL((
unsigned int)3,
643 double lambda = 0.1 ;
645 std::cout<<
"KDL-IK WDLS Vel Solver Tests for Near Zero SVs"<<std::endl;
649 unsigned int nj = motomansia10.getNrOfJoints();
655 std::cout<<
"smallest singular value is above threshold (no WDLS)"<<std::endl;
670 std::cout<<
"smallest singular value is below threshold (lambda is scaled)"<<std::endl;
680 std::cout<<
"smallest singular value is zero (lambda_scaled=lambda)"<<std::endl;
684 CPPUNIT_ASSERT_EQUAL((
int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
693 CPPUNIT_ASSERT_EQUAL((
int)ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR,
702 double deltaq = 1E-4;
716 for (
unsigned int i=0; i< q.rows() ; i++)
723 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, fksolverpos.
JntToCart(q,F1));
728 Vector(jac(3,i),jac(4,i),jac(5,i)));
731 CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
754 CPPUNIT_ASSERT_EQUAL(cart.deriv(),t);
775 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, iksolvervel.
CartToJnt(qvel.
q,cart.deriv(),qdot_solved));
777 qvel.
deriv()=qdot_solved;
780 CPPUNIT_ASSERT(
Equal(qvel.
qdot,qdot_solved,1e-5));
784 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR,fksolvervel.
JntToCart(qvel,cart_solved));
785 CPPUNIT_ASSERT(
Equal(cart.deriv(),cart_solved.
deriv(),1e-5));
802 q_init(i)=q(i)+0.1*tmp;
809 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, iksolverpos.
CartToJnt(q_init,F1,q_solved));
810 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, fksolverpos.
JntToCart(q_solved,F2));
812 CPPUNIT_ASSERT_EQUAL(F1,F2);
820 std::cout <<
"KDL Vereshchagin Hybrid Dynamics Tests" <<std::endl;
842 int solver_return = 0;
845 unsigned int nj = kukaLWR.getNrOfJoints();
846 unsigned int ns = kukaLWR.getNrOfSegments();
850 CPPUNIT_ASSERT(
Equal(nj, ns));
891 f_ext[ns - 1] = f_tool;
901 int number_of_constraints = 6;
904 Jacobian alpha_unit_force(number_of_constraints);
907 Twist unit_force_x_l(
910 alpha_unit_force.
setColumn(0, unit_force_x_l);
912 Twist unit_force_y_l(
915 alpha_unit_force.
setColumn(1, unit_force_y_l);
917 Twist unit_force_z_l(
920 alpha_unit_force.
setColumn(2, unit_force_z_l);
922 Twist unit_force_x_a(
925 alpha_unit_force.
setColumn(3, unit_force_x_a);
927 Twist unit_force_y_a(
930 alpha_unit_force.
setColumn(4, unit_force_y_a);
932 Twist unit_force_z_a(
935 alpha_unit_force.
setColumn(5, unit_force_z_a);
938 JntArray beta_energy(number_of_constraints);
939 beta_energy(0) = -0.5;
940 beta_energy(1) = -0.5;
941 beta_energy(2) = 0.0;
942 beta_energy(3) = 0.0;
943 beta_energy(4) = 0.0;
944 beta_energy(5) = 0.2;
951 solver_return = vereshchaginSolver.
CartToJnt(q, qd, qdd, alpha_unit_force, beta_energy, f_ext, ff_tau, constraint_tau);
952 if (solver_return < 0) std::cout <<
"KDL: Vereshchagin solver ERROR: " << solver_return << std::endl;
959 std::vector<Twist> xDotdot(ns + 1);
962 CPPUNIT_ASSERT(
Equal(beta_energy(0), xDotdot[ns].vel(0), eps));
963 CPPUNIT_ASSERT(
Equal(beta_energy(1), xDotdot[ns].vel(1), eps));
964 CPPUNIT_ASSERT(
Equal(beta_energy(2), xDotdot[ns].vel(2), eps));
965 CPPUNIT_ASSERT(
Equal(beta_energy(5), xDotdot[ns].rot(2), eps));
969 Eigen::VectorXd nu(number_of_constraints);
971 CPPUNIT_ASSERT(
Equal(nu(0), 669693.30355, eps));
972 CPPUNIT_ASSERT(
Equal(nu(1), 5930.60826, eps));
973 CPPUNIT_ASSERT(
Equal(nu(2), -639.5238, eps));
974 CPPUNIT_ASSERT(
Equal(nu(3), 0.000, eps));
975 CPPUNIT_ASSERT(
Equal(nu(4), 0.000, eps));
976 CPPUNIT_ASSERT(
Equal(nu(5), 573.90485, eps));
981 CPPUNIT_ASSERT(
Equal(total_tau(0), 2013.3541, eps));
982 CPPUNIT_ASSERT(
Equal(total_tau(1), -6073.4999, eps));
983 CPPUNIT_ASSERT(
Equal(total_tau(2), 2227.4487, eps));
984 CPPUNIT_ASSERT(
Equal(total_tau(3), 56.87456, eps));
985 CPPUNIT_ASSERT(
Equal(total_tau(4), -11.3789, eps));
986 CPPUNIT_ASSERT(
Equal(total_tau(5), -6.05957, eps));
987 CPPUNIT_ASSERT(
Equal(total_tau(6), 569.0776, eps));
992 Vector constrainXLinear(1.0, 0.0, 0.0);
993 Vector constrainXAngular(0.0, 0.0, 0.0);
994 Vector constrainYLinear(0.0, 0.0, 0.0);
995 Vector constrainYAngular(0.0, 0.0, 0.0);
998 Twist constraintForcesX(constrainXLinear, constrainXAngular);
999 Twist constraintForcesY(constrainYLinear, constrainYAngular);
1013 Vector linearAcc(0.0, 10, 0.0);
1014 Vector angularAcc(0.0, 0.0, 0.0);
1015 Twist twist1(linearAcc, angularAcc);
1018 Vector externalForce1(0.0, 0.0, 0.0);
1019 Vector externalTorque1(0.0, 0.0, 0.0);
1020 Vector externalForce2(0.0, 0.0, 0.0);
1021 Vector externalTorque2(0.0, 0.0, 0.0);
1022 Wrench externalNetForce1(externalForce1, externalTorque1);
1023 Wrench externalNetForce2(externalForce2, externalTorque2);
1025 externalNetForce.push_back(externalNetForce1);
1026 externalNetForce.push_back(externalNetForce2);
1033 int numberOfConstraints = 1;
1046 JntArray jointConstraintTorques[k];
1047 for (
int i = 0; i < k; i++)
1049 JntArray jointValues(chaindyn.getNrOfJoints());
1050 jointPoses[i] = jointValues;
1051 jointRates[i] = jointValues;
1052 jointAccelerations[i] = jointValues;
1053 jointFFTorques[i] = jointValues;
1054 jointConstraintTorques[i] = jointValues;
1058 JntArray jointInitialPose(chaindyn.getNrOfJoints());
1059 jointInitialPose(0) = 0.0;
1060 jointInitialPose(1) =
PI/6.0;
1065 jointPoses[0](0) = jointInitialPose(0);
1066 jointPoses[0](1) = jointInitialPose(1);
1075 double taskTimeConstant = 0.1;
1076 double simulationTime = 1*taskTimeConstant;
1077 double timeDelta = 0.01;
1079 const std::string msg =
"Assertion failed, check matrix and array sizes";
1081 for (
double t = 0.0; t <=simulationTime; t = t + timeDelta)
1083 CPPUNIT_ASSERT_EQUAL((
int)
SolverI::E_NOERROR, constraintSolver.
CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointFFTorques[0], jointConstraintTorques[0]));
1086 jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta;
1087 jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta;
1088 jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta;
1089 jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
1090 jointFFTorques[0] = jointConstraintTorques[0];
1092 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), jointConstraintTorques[0](0), jointConstraintTorques[0](1));
1099 std::vector<Frame> v_out(chain1.getNrOfSegments());
1101 JntArray q(chain1.getNrOfJoints());
1102 JntArray qdot(chain1.getNrOfJoints());
1104 for(
unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1113 CPPUNIT_ASSERT(
Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1119 std::vector<FrameVel> v_out(chain1.getNrOfSegments());
1121 JntArray q(chain1.getNrOfJoints());
1122 JntArray qdot(chain1.getNrOfJoints());
1124 for(
unsigned int i=0; i<chain1.getNrOfJoints(); i++)
1134 CPPUNIT_ASSERT(
Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
1142 std::cout<<
"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
1148 Vector gravity(0.0, 0.0, -9.81);
1151 unsigned int nj = motomansia10dyn.getNrOfJoints();
1152 unsigned int ns = motomansia10dyn.getNrOfSegments();
1180 CPPUNIT_ASSERT(
Equal(-0.547, f_out.
p(0), eps));
1181 CPPUNIT_ASSERT(
Equal(-0.301, f_out.
p(1), eps));
1182 CPPUNIT_ASSERT(
Equal(0.924, f_out.
p(2), eps));
1183 CPPUNIT_ASSERT(
Equal(0.503, f_out.
M(0,0), eps));
1184 CPPUNIT_ASSERT(
Equal(0.286, f_out.
M(0,1), eps));
1185 CPPUNIT_ASSERT(
Equal(-0.816, f_out.
M(0,2), eps));
1186 CPPUNIT_ASSERT(
Equal(-0.859, f_out.
M(1,0), eps));
1187 CPPUNIT_ASSERT(
Equal(0.269, f_out.
M(1,1), eps));
1188 CPPUNIT_ASSERT(
Equal(-0.436, f_out.
M(1,2), eps));
1189 CPPUNIT_ASSERT(
Equal(0.095, f_out.
M(2,0), eps));
1190 CPPUNIT_ASSERT(
Equal(0.920, f_out.
M(2,1), eps));
1191 CPPUNIT_ASSERT(
Equal(0.381, f_out.
M(2,2), eps));
1198 {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
1199 {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
1200 {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
1201 {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
1202 {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
1203 {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
1204 for (
unsigned int i=0; i<6; i++ ) {
1205 for (
unsigned int j=0; j<nj; j++ ) {
1206 CPPUNIT_ASSERT(
Equal(jac(i,j), Jac[i][j], eps));
1213 JntSpaceInertiaMatrix H(nj), Heff(nj);
1217 if (ret < 0) std::cout <<
"KDL: inverse dynamics ERROR: " << ret << std::endl;
1218 CPPUNIT_ASSERT(
Equal(0.000, taugrav(0), eps));
1219 CPPUNIT_ASSERT(
Equal(-36.672, taugrav(1), eps));
1220 CPPUNIT_ASSERT(
Equal(4.315, taugrav(2), eps));
1221 CPPUNIT_ASSERT(
Equal(-11.205, taugrav(3), eps));
1222 CPPUNIT_ASSERT(
Equal(0.757, taugrav(4), eps));
1223 CPPUNIT_ASSERT(
Equal(1.780, taugrav(5), eps));
1224 CPPUNIT_ASSERT(
Equal(0.000, taugrav(6), eps));
1227 if (ret < 0) std::cout <<
"KDL: inverse dynamics ERROR: " << ret << std::endl;
1228 CPPUNIT_ASSERT(
Equal(-15.523, taucor(0), eps));
1229 CPPUNIT_ASSERT(
Equal(24.250, taucor(1), eps));
1230 CPPUNIT_ASSERT(
Equal(-6.862, taucor(2), eps));
1231 CPPUNIT_ASSERT(
Equal(6.303, taucor(3), eps));
1232 CPPUNIT_ASSERT(
Equal(0.110, taucor(4), eps));
1233 CPPUNIT_ASSERT(
Equal(-4.898, taucor(5), eps));
1234 CPPUNIT_ASSERT(
Equal(-0.249, taucor(6), eps));
1237 if (ret < 0) std::cout <<
"KDL: inverse dynamics ERROR: " << ret << std::endl;
1239 {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
1240 {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
1241 {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
1242 {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
1243 {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
1244 {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
1245 {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
1246 for (
unsigned int i=0; i<nj; i++ ) {
1247 for (
unsigned int j=0; j<nj; j++ ) {
1248 CPPUNIT_ASSERT(
Equal(H(i,j), Hexp[i][j], eps));
1266 for(
unsigned int i=0;i<ns;i++){
1274 IdSolver.
CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
1275 CPPUNIT_ASSERT(
Equal(-21.252, Tnoninertial(0), eps));
1276 CPPUNIT_ASSERT(
Equal(-37.933, Tnoninertial(1), eps));
1277 CPPUNIT_ASSERT(
Equal(-2.497, Tnoninertial(2), eps));
1278 CPPUNIT_ASSERT(
Equal(-15.289, Tnoninertial(3), eps));
1279 CPPUNIT_ASSERT(
Equal(-4.646, Tnoninertial(4), eps));
1280 CPPUNIT_ASSERT(
Equal(-9.201, Tnoninertial(5), eps));
1281 CPPUNIT_ASSERT(
Equal(-5.249, Tnoninertial(6), eps));
1284 Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
1285 Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
1286 for(
unsigned int i=0;i<nj;i++){
1287 Tnon_eig(i) = -Tnoninertial(i);
1288 for(
unsigned int j=0;j<nj;j++){
1289 H_eig(i,j) = H(i,j);
1293 for(
unsigned int i=0;i<nj;i++){
1294 qdd(i) = acc_eig(i);
1296 CPPUNIT_ASSERT(
Equal(2.998, qdd(0), eps));
1297 CPPUNIT_ASSERT(
Equal(4.289, qdd(1), eps));
1298 CPPUNIT_ASSERT(
Equal(0.946, qdd(2), eps));
1299 CPPUNIT_ASSERT(
Equal(2.518, qdd(3), eps));
1300 CPPUNIT_ASSERT(
Equal(3.530, qdd(4), eps));
1301 CPPUNIT_ASSERT(
Equal(8.150, qdd(5), eps));
1302 CPPUNIT_ASSERT(
Equal(4.254, qdd(6), eps));
1310 std::cout<<
"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
1315 Vector gravity(0.0, 0.0, -9.81);
1318 unsigned int nj = motomansia10dyn.getNrOfJoints();
1319 unsigned int ns = motomansia10dyn.getNrOfSegments();
1358 for(
unsigned int i=0;i<ns;i++){
1364 ret = FdSolver.
CartToJnt(q, qd, tau, f_ext, qdd);
1365 if (ret < 0) std::cout <<
"KDL: forward dynamics ERROR: " << ret << std::endl;
1366 CPPUNIT_ASSERT(
Equal(9.486, qdd(0), eps));
1367 CPPUNIT_ASSERT(
Equal(1.830, qdd(1), eps));
1368 CPPUNIT_ASSERT(
Equal(4.726, qdd(2), eps));
1369 CPPUNIT_ASSERT(
Equal(11.665, qdd(3), eps));
1370 CPPUNIT_ASSERT(
Equal(-50.108, qdd(4), eps));
1371 CPPUNIT_ASSERT(
Equal(21.403, qdd(5), eps));
1372 CPPUNIT_ASSERT(
Equal(-0.381, qdd(6), eps));
1377 IdSolver.
CartToJnt(q, qd, qdd, f_ext, torque);
1378 for (
unsigned int i=0; i<nj; i++ )
1380 CPPUNIT_ASSERT(
Equal(torque(i), tau(i), eps));
1388 std::cout<<
"LDL Solver Test"<<std::endl;
1393 Eigen::MatrixXd A(3,3), Aout(3,3);
1394 Eigen::VectorXd b(3);
1395 Eigen::MatrixXd L(3,3), Lout(3,3);
1396 Eigen::VectorXd d(3), dout(3);
1397 Eigen::VectorXd x(3), xout(3);
1398 Eigen::VectorXd r(3);
1399 Eigen::MatrixXd Dout(3,3);
1415 for(
int i=0;i<3;i++){
1416 for(
int j=0;j<3;j++){
1417 CPPUNIT_ASSERT(
Equal(L(i,j), Lout(i,j), eps));
1422 for(
int i=0;i<3;i++){
1423 Dout(i,i) = dout(i);
1427 for(
int i=0;i<3;i++){
1428 CPPUNIT_ASSERT(
Equal(xout(i), x(i), eps));
1432 Aout = Lout * Dout * Lout.transpose();
1433 for(
int i=0;i<3;i++){
1434 for(
int j=0;j<3;j++){
1435 CPPUNIT_ASSERT(
Equal(A(i,j), Aout(i,j), eps));
1447 Frame end_effector_pose;
1453 std::cout <<
"KDL FD (inverse-inertia version) and Vereshchagin Solvers Consistency Test for KUKA LWR 4 robot" << std::endl;
1457 unsigned int nj = kukaLWR.getNrOfJoints();
1458 unsigned int ns = kukaLWR.getNrOfSegments();
1462 CPPUNIT_ASSERT(
Equal(nj, ns));
1502 for(
unsigned int i=0 ;i<ns; i++)
1504 f_ext[ns - 1] = f_tool;
1508 Vector gravity(0.0, 0.0, -9.81);
1512 ret = FdSolver.
CartToJnt(q, qd, ff_tau, f_ext, qdd);
1514 std::cout <<
"KDL: forward dynamics ERROR: " << ret << std::endl;
1523 int numberOfConstraints = 6;
1524 Jacobian alpha(numberOfConstraints);
1528 JntArray beta(numberOfConstraints);
1533 Vector linearAcc(0.0, 0.0, 9.81);
Vector angularAcc(0.0, 0.0, 0.0);
1534 Twist root_Acc(linearAcc, angularAcc);
1542 fksolverpos.
JntToCart(q, end_effector_pose, kukaLWR.getNrOfSegments());
1543 f_ext[ns - 1] = end_effector_pose.
M * f_tool;
1546 ret = constraintSolver.
CartToJnt(q, qd, q_dd_Ver, alpha, beta, f_ext, ff_tau, constraint_tau);
1548 std::cout <<
"KDL: Vereshchagin solver ERROR: " << ret << std::endl;
1552 CPPUNIT_ASSERT(
Equal(q_dd_Ver(0), qdd(0), eps));
1553 CPPUNIT_ASSERT(
Equal(q_dd_Ver(1), qdd(1), eps));
1554 CPPUNIT_ASSERT(
Equal(q_dd_Ver(2), qdd(2), eps));
1555 CPPUNIT_ASSERT(
Equal(q_dd_Ver(3), qdd(3), eps));
1556 CPPUNIT_ASSERT(
Equal(q_dd_Ver(4), qdd(4), eps));
1557 CPPUNIT_ASSERT(
Equal(q_dd_Ver(5), qdd(5), eps));
1558 CPPUNIT_ASSERT(
Equal(q_dd_Ver(6), qdd(6), eps));
1572 std::cout <<
"KDL External Wrench Estimator Test" << std::endl;
1581 double eps_wrench = 0.5, eps_torque = 0.3;
1583 unsigned int nj = kukaLWR.getNrOfJoints();
1584 unsigned int ns = kukaLWR.getNrOfSegments();
1585 CPPUNIT_ASSERT(
Equal(nj, ns));
1601 Frame end_effector_pose;
1602 Frame desired_end_eff_pose;
1606 Eigen::Matrix<double, 6, 1> end_eff_force;
1607 Eigen::Matrix<double, 6, 1> end_eff_pos_error;
1608 Eigen::Matrix<double, 6, 1> end_eff_vel_error;
1611 Vector linearAcc(0.0, 0.0, -9.81);
Vector angularAcc(0.0, 0.0, 0.0);
1622 int numberOfConstraints = 6;
1623 Jacobian alpha(numberOfConstraints);
1624 JntArray beta(numberOfConstraints);
1627 Twist vereshchagin_root_Acc(-linearAcc, angularAcc);
1631 double sample_frequency = 1000.0;
1632 double estimation_gain = 45.0;
1633 double filter_constant = 0.5;
1637 std::vector<KDL::JntArray> jnt_pos;
1638 std::vector<KDL::Wrench> wrench_reference;
1641 std::random_device rd;
1642 std::mt19937 gen(rd());
1643 std::uniform_real_distribution<> dis_force(-15.0, 15.0);
1644 std::uniform_real_distribution<> dis_moment(-0.9, 0.9);
1645 std::uniform_real_distribution<> dis_jnt_vel(-0.5, 0.5);
1655 jnt_pos.push_back(q);
1656 wrench_reference.push_back(
Wrench(
Vector(dis_force(gen), dis_force(gen), dis_force(gen)),
Vector(0.0, 0.0, 0.0)));
1666 jnt_pos.push_back(q);
1667 wrench_reference.push_back(
Wrench(
Vector(0.0, 0.0, 0.0),
Vector(dis_moment(gen), dis_moment(gen), 0.0)));
1677 jnt_pos.push_back(q);
1678 wrench_reference.push_back(
Wrench(
Vector(dis_force(gen), dis_force(gen), dis_force(gen)),
Vector(dis_moment(gen), 0.0, dis_moment(gen))));
1685 double k_p = 1500.0;
1689 double simulationTime = 0.4;
1690 double timeDelta = 1.0 / sample_frequency;
1693 for (
unsigned int i = 0; i < jnt_pos.size(); i++)
1697 qd(0) = dis_jnt_vel(gen);
1698 qd(1) = dis_jnt_vel(gen);
1699 qd(2) = dis_jnt_vel(gen);
1700 qd(3) = dis_jnt_vel(gen);
1701 qd(4) = dis_jnt_vel(gen);
1702 qd(5) = dis_jnt_vel(gen);
1703 qd(6) = dis_jnt_vel(gen);
1705 end_eff_force.setZero();
1706 end_eff_pos_error.setZero();
1707 end_eff_vel_error.setZero();
1708 f_ext_base = f_ext_zero;
1715 fksolverpos.
JntToCart(q, end_effector_pose);
1716 desired_end_eff_pose.
p(0) = end_effector_pose.
p(0) + 0.02;
1717 desired_end_eff_pose.
p(1) = end_effector_pose.
p(1) + 0.02;
1718 desired_end_eff_pose.
p(2) = end_effector_pose.
p(2) + 0.02;
1719 desired_end_eff_twist.
p.
v(0) = 0.0;
1720 desired_end_eff_twist.
p.
v(1) = 0.0;
1721 desired_end_eff_twist.
p.
v(2) = 0.0;
1723 for (
double t = 0.0; t <= simulationTime; t = t + timeDelta)
1725 ret = jacobian_solver.
JntToJac(q, jacobian_end_eff);
1728 std::cout <<
"Jacobian solver ERROR: " << ret << std::endl;
1732 ret = fksolverpos.
JntToCart(q, end_effector_pose);
1735 std::cout <<
"FK pos solver ERROR: " << ret << std::endl;
1739 jnt_position_velocity.
q = q;
1740 jnt_position_velocity.
qdot = qd;
1741 ret = fksolvervel.
JntToCart(jnt_position_velocity, end_eff_twist);
1744 std::cout <<
"FK vel solver ERROR: " << ret << std::endl;
1748 end_eff_pos_error(0) = end_effector_pose.
p(0) - desired_end_eff_pose.
p(0);
1749 end_eff_pos_error(1) = end_effector_pose.
p(1) - desired_end_eff_pose.
p(1);
1750 end_eff_pos_error(2) = end_effector_pose.
p(2) - desired_end_eff_pose.
p(2);
1752 end_eff_vel_error(0) = end_eff_twist.
p.
v(0) - desired_end_eff_twist.
p.
v(0);
1753 end_eff_vel_error(1) = end_eff_twist.
p.
v(1) - desired_end_eff_twist.
p.
v(1);
1754 end_eff_vel_error(2) = end_eff_twist.
p.
v(2) - desired_end_eff_twist.
p.
v(2);
1756 end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d;
1759 ret = IdSolver.
CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque);
1762 std::cout <<
"KDL RNE solver ERROR: " << ret << std::endl;
1767 command_torque.
data = jacobian_end_eff.
data.transpose() * end_eff_force;
1768 command_torque.
data += gravity_torque.
data;
1771 if (t > 0.2) f_ext_base[ns - 1] = end_effector_pose.
M * wrench_reference[i];
1774 ret = constraintSolver.
CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
1777 std::cout <<
"KDL Vereshchagin solver ERROR: " << ret << std::endl;
1786 for (
unsigned int j = 0; j < nj; j++)
1788 q(j) = std::fmod(q(j), 360 *
deg2rad);
1789 if (q(j) < 0.0) q(j) += 360 *
deg2rad;
1793 extwrench_estimator.
JntToExtWrench(q, qd, command_torque, f_tool_estimated);
1797 Eigen::Matrix<double, 6, 1> wrench;
1798 wrench(0) = f_ext_base[ns - 1](0);
1799 wrench(1) = f_ext_base[ns - 1](1);
1800 wrench(2) = f_ext_base[ns - 1](2);
1801 wrench(3) = f_ext_base[ns - 1](3);
1802 wrench(4) = f_ext_base[ns - 1](4);
1803 wrench(5) = f_ext_base[ns - 1](5);
1804 ext_torque_reference.
data = jacobian_end_eff.
data.transpose() * wrench;
1812 CPPUNIT_ASSERT(
Equal(f_tool_estimated(0), wrench_reference[i](0), eps_wrench));
1813 CPPUNIT_ASSERT(
Equal(f_tool_estimated(1), wrench_reference[i](1), eps_wrench));
1814 CPPUNIT_ASSERT(
Equal(f_tool_estimated(2), wrench_reference[i](2), eps_wrench));
1815 CPPUNIT_ASSERT(
Equal(f_tool_estimated(3), wrench_reference[i](3), eps_wrench));
1816 CPPUNIT_ASSERT(
Equal(f_tool_estimated(4), wrench_reference[i](4), eps_wrench));
1817 CPPUNIT_ASSERT(
Equal(f_tool_estimated(5), wrench_reference[i](5), eps_wrench));
1819 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque));
1820 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque));
1821 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque));
1822 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque));
1823 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque));
1824 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque));
1825 CPPUNIT_ASSERT(
Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque));
This abstract class encapsulates the inverse position solver for a KDL::Chain.
virtual void updateInternalDataStructures()
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
const double PI_2
the value of pi/2
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
void getTotalTorque(JntArray &total_tau)
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)=0
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
void IkVelSolverWDLSTest()
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
const double PI
the value of pi
double getLambdaScaled() const
virtual void updateInternalDataStructures()
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Maximum number of iterations exceeded.
unsigned int getNrZeroSigmas() const
This class represents an fixed size array containing joint values of a KDL::Chain.
6D Inertia of a rigid body
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
int ldl_solver_eigen(const Eigen::MatrixXd &A, const Eigen::VectorXd &v, Eigen::MatrixXd &L, Eigen::VectorXd &D, Eigen::VectorXd &vtmp, Eigen::VectorXd &q)
Solves the system of equations Aq = v for q via LDL decomposition, where A is a square positive defin...
Input size does not match internal state.
void FdAndVereshchaginSolversConsistencyTest()
unsigned int getNrZeroSigmas() const
void FkVelAndJacLocal(Chain &chain, ChainFkSolverVel &fksolvervel, ChainJntToJacSolver &jacsolver)
void ExternalWrenchEstimatorTest()
Recursive newton euler forward dynamics solver.
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)=0
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)
Rotation M
Orientation of the Frame.
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
void setInertia(const RigidBodyInertia &Iin)
virtual void updateInternalDataStructures()
represents both translational and rotational velocities.
IMETHOD void SetToZero(Vector &v)
virtual void updateInternalDataStructures()
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
unsigned int getNrOfJoints() const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)=0
A concrete implementation of a 3 dimensional vector class.
static Rotation RPY(double roll, double pitch, double yaw)
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
static Frame DH(double a, double alpha, double d, double theta)
virtual void updateInternalDataStructures()
void getTransformedLinkAcceleration(Twists &x_dotdot)
void FkPosAndJacLocal(Chain &chain, ChainFkSolverPos &fksolverpos, ChainJntToJacSolver &jacsolver)
Requested index out of range.
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)=0
void setColumn(unsigned int i, const Twist &t)
Recursive newton euler inverse dynamics solver.
Vector p
origine of the Frame
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
virtual int getError() const
Return the latest error.
virtual void updateInternalDataStructures()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void IkSingularValueTest()
void updateInternalDataStructures()
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
First-order momentum observer for the estimation of external wrenches applied on the robot's end-effe...
void FdSolverConsistencyTest()
int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques)
void resize(unsigned int newSize)
std::vector< Wrench > Wrenches
void setLambda(const double lambda)
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
void FdSolverDevelopmentTest()
represents a frame transformation in 3D space (rotation + translation)
virtual void updateInternalDataStructures()
void getEstimatedJntTorque(JntArray &external_joint_torque)
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
static Rotation Identity()
Gives back an identity rotaton matrix.
represents both translational and rotational acceleration.
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
CPPUNIT_TEST_SUITE_REGISTRATION(SolverTest)
virtual void updateInternalDataStructures()
const double deg2rad
the value pi/180
void FkVelAndIkVelLocal(Chain &chain, ChainFkSolverVel &fksolvervel, ChainIkSolverVel &iksolvervel)
virtual void updateInternalDataStructures()
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix &H)
void FkPosAndIkPosLocal(Chain &chain, ChainFkSolverPos &fksolverpos, ChainIkSolverPos &iksolverpos)
virtual void updateInternalDataStructures()
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
virtual void updateInternalDataStructures()
This abstract class encapsulates a solver for the forward velocity kinematics for a KDL::Chain...
Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 19...
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
virtual int JntToGravity(const JntArray &q, JntArray &gravity)
virtual void updateInternalDataStructures()
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
double getSigmaMin() const
This abstract class encapsulates the inverse velocity solver for a KDL::Chain.
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Chain...