Take orientation into account in LMA

- Inspired on e2bfa1c old iros2018 branch
roboticslab-uc3m · Jan 19, 2018 · 093594c · 093594c
1 parent a2e9576
commit 093594c
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp b/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp
@@ -116,7 +116,7 @@ bool roboticslab::KdlSolver::invKin(const std::vector<double> &xd, const std::ve
 
     Eigen::Matrix<double, 6, 1> L;
     L(0) = 1; L(1) = 1; L(2) = 1;
-    L(3) = 0; L(4) = 0; L(5) = 0;
+    L(3) = 0.1; L(4) = 0.1; L(5) = 0.1;
 
     //-- Main invKin (pos) solver lines
     KDL::ChainIkSolverPos_LMA iksolver_pos(chain, L);