[KdlSolver] Alternative to LMA

[jgvictores]

[KdlSolver] Alternative to LMA

[jgvictores]

Something like:

#ifdef USE_LMA

Eigen::Matrix<double,6,1> L;
  L(0)=0.5;L(1)=0.5;L(2)=0.5;
  L(3)=0.05;L(4)=0.05;L(5)=0.05;

  //-- Main invKin (pos) solver lines
KDL::ChainIkSolverPos_LMA inv_pos_solver(chain,L,1E-10,100000,1E-25);

inv_pos_solver.CartToJnt(qGuessInRad,frameXd,kdlq);

#else //USE_LMA

//Forward solvers, needed by the geometric solver
KDL::ChainFkSolverPos_recursive fksolver(chain);
KDL::ChainIkSolverVel_pinv iksolver(chain);  // _givens

//Geometric solver definition (with joint limits)
KDL::ChainIkSolverPos_NR_JL inv_pos_solver(chain,q_min,q_max,fksolver,iksolver,100000,1E-15);

inv_pos_solver.CartToJnt(qGuessInRad,frameXd,kdlq);

#endif //USE_LMA

[jgvictores]

Done at 96a62f3.