[regression] inverse kinematic from urdf chain broken in 3.3

[LorenzoFerriniCodes]

I have installed today ikpy v3.3, and it seems that on code that was previously working it is always estimating 0 (or very close to zero) values for the joint angles. It seems that the problem was introduced with commit d879456.

Brief explanation of what we are doing in the code linked: using human 3D pose estimation from RGB images to compute human joint angles, according to a human urdf model.

Before the problem was introduced: rviz visualized human model was moving its arms according to humans moving in front of the camera. Inverse kinematic was perfectly running and estimating joint angles.

Once the problem appeared: rviz visualized human model is no more moving its arms, because of ik estimating null joint angles, while 3D pose estimation is correctly going on.

[Phylliade]

Ouch that's weird!

Thanks for the clear issue @LorenzoFerriniCodes !

The difference between the code before and after the commit you pointed is that a new solver is used: instead of using a standard scalar solver, a least square solver is used;
It generally improved the speed and accuracy of the computations;

That's very weird it's breaking your use-case, i will have a look!

[Phylliade]

In the meantime, is it a problem for you to use v3.2?

[LorenzoFerriniCodes]

No problem, for the moment I will specify 3.2 version as a requirement.

[Phylliade]

Cool! Thanks again for the issue!

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[Luca-Pozzi]

I have run into this issue while working with hri_fullbody which relies on the human urdf model mentioned by @LorenzoFerriniCodes.
I would just like to report what I have understood about the problem.

Setup and scenario

Perform keypoint detection of me casually moving arms in front of my PC webcam (driver usb_cam).

Insights

I have tried to inspect the course of the optimization by setting the scipy.optimize.least_squares(..., verbose=2) for the arms.
A pretty typical behavior of the cost function (as displayed by scipy) is the following:

Iteration     Total nfev        Cost      Cost reduction    Step norm     Optimality   
       0              1         3.7055e-02                                    1.56e-01    
       1              2         3.7055e-02      6.33e-12       1.01e-10       1.56e-01    
`ftol` termination condition is satisfied.
Function evaluations 2, initial cost 3.7055e-02, final cost 3.7055e-02, first-order optimality 1.56e-01.
[0.00000000e+00 1.74221231e-12 2.57989079e-11 2.37574406e-12
 1.97362542e-10 0.00000000e+00]

As far as I could see, this behavior happens no matter how much you move the arms.

Take your time to imagine me fidgeting in front of the PC while colleagues watch me.

From scipy documentation I have found that the default value for ftol is 1e-8. I then tried to neglect the ftol termination criterion, by setting scipy.optimize.least_squares(..., ftol=None, verbose=2) and I have got the following:

   Iteration     Total nfev        Cost      Cost reduction    Step norm     Optimality   
       0              1         1.1482e-01                                    2.23e-01    
       1              2         1.1482e-01      8.96e-12       1.01e-10       2.23e-01    
       2              3         1.1482e-01      1.79e-11       2.01e-10       2.23e-01    
       3              4         1.1482e-01      3.59e-11       4.03e-10       2.23e-01    
       4              5         1.1482e-01      7.18e-11       8.05e-10       2.23e-01    
       5              6         1.1482e-01      1.44e-10       1.61e-09       2.23e-01    
       6              7         1.1482e-01      2.87e-10       3.22e-09       2.23e-01    
       7              8         1.1482e-01      5.75e-10       6.44e-09       2.23e-01    
       8              9         1.1482e-01      1.15e-09       1.29e-08       2.23e-01    
       9             10         1.1482e-01      2.30e-09       2.57e-08       2.23e-01    
      10             11         1.1482e-01      4.61e-09       5.14e-08       2.23e-01    
      11             12         1.1482e-01      9.22e-09       1.03e-07       2.23e-01    
      12             13         1.1482e-01      1.84e-08       2.06e-07       2.23e-01    
      13             14         1.1481e-01      3.69e-08       4.11e-07       2.23e-01    
      14             15         1.1481e-01      7.38e-08       8.22e-07       2.23e-01    
      15             16         1.1481e-01      1.48e-07       1.64e-06       2.23e-01    
      16             17         1.1481e-01      2.95e-07       3.29e-06       2.23e-01    
      17             18         1.1481e-01      5.90e-07       6.58e-06       2.23e-01    
      18             19         1.1481e-01      1.18e-06       1.32e-05       2.23e-01    
      19             20         1.1481e-01      2.36e-06       2.63e-05       2.23e-01    
      20             21         1.1481e-01      4.72e-06       5.26e-05       2.23e-01    
      21             22         1.1480e-01      9.44e-06       1.05e-04       2.23e-01    
      22             23         1.1478e-01      1.89e-05       2.10e-04       2.23e-01    
      23             24         1.1474e-01      3.78e-05       4.21e-04       2.23e-01    
      24             25         1.1466e-01      7.56e-05       8.42e-04       2.22e-01    
      25             26         1.1451e-01      1.51e-04       1.68e-03       2.22e-01    
      26             27         1.1421e-01      3.02e-04       3.37e-03       2.22e-01    
      27             28         1.1361e-01      6.04e-04       6.73e-03       2.22e-01    
      28             29         1.1240e-01      1.21e-03       1.34e-02       2.21e-01    
      29             30         1.0998e-01      2.41e-03       2.68e-02       2.19e-01    
      30             31         1.0517e-01      4.81e-03       5.32e-02       2.15e-01    
      31             32         9.5650e-02      9.52e-03       1.05e-01       2.06e-01    
      32             33         7.7334e-02      1.83e-02       2.06e-01       1.81e-01    
      33             34         4.5798e-02      3.15e-02       3.92e-01       1.20e-01    
      34             35         1.2072e-02      3.37e-02       6.92e-01       7.51e-02    
      35             36         5.2347e-03      6.84e-03       9.73e-01       9.32e-02    
      36             38         1.8493e-04      5.05e-03       3.50e-01       7.19e-03    
      37             40         2.3132e-06      1.83e-04       1.30e-01       4.71e-04    
      38             43         5.2931e-09      2.31e-06       1.76e-02       7.87e-06    
      39             46         4.3016e-09      9.91e-10       2.76e-03       6.98e-06    
      40             48         2.4154e-14      4.30e-09       1.46e-03       2.91e-08    
      41             53         1.6381e-14      7.77e-15       8.21e-06       1.54e-07    
      42             56         1.5794e-14      5.87e-16       1.52e-06       2.35e-08    
      43             57         7.8503e-18      1.58e-14       2.65e-06       5.24e-10    
`gtol` termination condition is satisfied.
Function evaluations 57, initial cost 1.1482e-01, final cost 7.8503e-18, first-order optimality 5.24e-10.

As you can see, the cost reduction starts from tiny values (which causes the optimization to stop if ftol is left to the default 1e-8), then grows and drops again (I guess because the "actual" optimum has been found).

With this solution (i.e. ftol=None) the arms of the stickman in hri_fullbody indeed move according to the ones of the user, but the output is extremely jerky (I guess because changing the termination criteria requires the minimization algorithm to run more iterations).

Conclusion

Unfortunately, simply setting ftol=None does not look like a solution to the problem. A qualitative comparison of the output still suggests that using ikpy==3.2.2 and its linear solver should be considered the best workaround to solve this issue.

I hope that these insights can help someone who has a deeper understanding of how scipy.optimize.least_squares works in proposing a solution!

[Phylliade]

Thanks for these very clear insights @Luca-Pozzi!
@LorenzoFerriniCodes i've added an option to select the optimizer in the release 3.3.4!