Ask Your Question

Revision history [back]

Your URDF describes a 3 DOF manipulator. However, your solver sets 6 constraints on the end effector (3 position + 3 orientation). In general, it is not possible to satisfy the 6 constraints only with 3DOF. The jacobbian you are using is 6x3. That means that it tries to satisfy the 6 constraints. That is because you get those incorrect and strange solutions.

And that is why I developed the solution described in

It worked well for me reaching a 3D position with a 3DOF manipulator.

Another choice is adding 3 "fake revolute artificial joints" in the end effector.

However, the best solution should be change the jacobbian matrix to a 3x3 matrix that only considers the final position of the end effector (ignoring its final orientation). If you (or someother) develop this solution, please, share it with us :-).