Inverse kinematics

[LemonPi]

Add inverse kinematics (IK) via basic pseudo inverse method. Comes with many options for early termination and so on.

[PeterMitrano]

Are we trying to support multi-ee IK yet? I assume not since we don't have that jacobian method implemented but just want to confirm.

Also I modified the test so check IK problems generated by FK and the results suggest it's not very reliable in finding a solution:

Convergence: 517 / 10000
Iterations 30
Success 257 / 1000 goals

[PeterMitrano]

Are we sure this is a good rotation distance? Just substracting euler angles isn't generally a good idea -- you have potential wrap-around/discontinuity issues

[LemonPi]

Thanks for finding this! I replaced it with unit quaternion error (the quaternion required to get from current to desired orientation), converted to an axis angle representation.

Note that this is for computing error in end effector position and orientation to be multiplied by the jacobian pseudoinverse, so it needs to be 6D. [\dot{x} \omega] = J \dot{q} where we are approximating it to [\Delta{x} \Delta{r}] = J \Delta{q}

It's getting much better convergence now.

[LemonPi]

mujoco_menagerie seems to need jax (workflow file)

      - name: Clone mujoco_menagerie repository into the tests/ folder
        run: |
          git clone https://github.com/google-deepmind/mujoco_menagerie
        working-directory: ${{ runner.workspace }}/pytorch_kinematics/tests

how do you think we should include it? Something like:

      - name: Clone mujoco_menagerie repository into the tests/ folder
        run: |
          python -m pip install "jax[cpu]"
          git clone https://github.com/google-deepmind/mujoco_menagerie
        working-directory: ${{ runner.workspace }}/pytorch_kinematics/tests

[PeterMitrano]

just one small comment, otherwise LGTM

[PeterMitrano]

do we actually need this? I don't see it being used

[LemonPi]

This has been removed a couple commits back (only needed for mujoco menagerie)