[PSEUDO] Debug slow speed

[HiroIshida]

Found a strange speed down when wrapping a function by pybind11.

Because tinyfk takes advantage of the caching strategy, there should be almost no additional computational load when adding the same link points a lot.

As I expected, in the bench-marking via pure c++, no speed down is observed. However, doing the same thing inside a method _solve_forward_kinematics in pybind side, incurs a significant (almost 2x) slow down.

To exclude the possibility that the huge overhead when passing objects between python-side and c++-side, I measured the time inside _solve_forward_kinematics but the result doesn't change much.

comparison of pure forward kinematics (without jac) at ff74fa3

pure c++ bench_tinyfk : 0.32 sec
_solve_forward_kinematics : 0.55 sec
inside _solve_forward_kinematics : 0.53 sec

[HiroIshida]

output of bench_tinyfk

N: 100000
joint_ids: 9 8 11 0 1 12 13 
link_ids: 13 16 23 24 18 22 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 
tinyfk.FK_with_cache : 0.307943

output of compare_skrobot:

N: 100000
joint_ids: 9 8 11 0 1 12 13 
link_ids: 13 16 23 24 18 22 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 13 
0.51067
tinyfk jac computation : 0.5271542072296143 sec

To see the limit case I changed link number M=100 -> 1000, 10000, 100000
M=100=> cpp 0.31, python 0.52, ratio=0.59
M=1000 => cpp 2.40, python 4.09, ratio=0.58
M=10000=> cpp 23.7, python 42.3, ratio = 0.56