Implements functionality of ik as described in docstring
The IK implementations had some bugs:
- IK scipy did not actually perform a proper random restart. Instead, the same q_init was used over and over again
- The
allow_random_restart
kwarg was ignored in ik_scipy - The logic for
allow_random_restart
in ik_jacobian was implemented the wrong way round: If it was set toFalse
, the random restart was done anyways.