CPP IK for Timor

Implement a Jacobian inverse based numerical IK for timor implemented with Pinocchio in CPP.

Why?

Freaking fast; for reference, the current test case runs about 50x faster than the same in Python (find random, self-collision-free IK within joint limits) with +/- same success rate. Output:

C++ Failed 14 times out of 1000 iterations
C++ Average time 0.0011139421180000474 +/- 0.002108966273794128
Python Failed 25 times out of 1000 iterations
Python Average time 0.05468771997999997 +/- 0.07759612371141704

Functionality

• Jacobian-based IK
• Restart on self-collision
• Restart on out of joint limits
• Integration into outer IK loop of timor.PinRobot

Side-effects

• Longer pip install time as C++ code needs time to compile (< 1 min)
• No need for extra apt-get dependencies (eigen3 installed via PyPI; C++ compiler s.a. gcc/++ should be installed by default together with python3 and python3-pip)
• Vectorized self-collision check (50% faster than Python side)
• Test effect on pypi via https://test.pypi.org/ to ensure that install from pypi is not hurt by C++ code