[CPP] Question on TOPP-RA Forward Pass

[leonardoedgar]

Hi, may I know what are the possible issues that may lead to this failure?

toppra/cpp/src/toppra/algorithm/toppra.cpp

Line 30 in f8c55e3

if (!solver_ret) {

From what I understand from the paper, once the backward pass is finished, and the controllable sets is not empty, the algorithm theoretically should always be success (able to return a trajectory), since in the forward pass, it only greedily selects the fastest control from the previously computed controllable sets.

Is it possible that the failure is due to numerical issue of the solver? I faced the issue at idx=0

Let me know if you have any suggestions, thanks.

[leonardoedgar]

Is it possibly related to this? I am using the qpoases solver with the TOPP-RA C++ API

toppra/toppra/algorithm/reachabilitybased/reachability_algorithm.py

Line 315 in f8c55e3

if np.isnan(optim_res[0]):

[jmirabel]

In my experience, seidel solver works better. Have you tried with it ?

[leonardoedgar]

I see, yes, but I still have the issue. This is the error message

-inf	<= x[0] <= inf
1	<= x[1] <= 1
-> infeasible
[DEBUG]: Seidel: solver fails (upper ? 0)
[DEBUG]: Fail: forward pass, idx: 0

[jmirabel]

Looks like there is a numerical issue. Can you give a bit more context ? The output should contain v and A I think. Also, in the logs, you should be able to see some Seidel LP [12]D:. The output is important to understand the reason for the failure.

Are you compiling from source ?

[jmirabel]

Actually, the issue just occurred to me. The output in the terminal is

[WARN]: Seidel LP 1D:
v: -0.563731         0
A:
    0.563731      1.78807
   -0.563731      -15.333
 -8.6309e-13      -8.2434
 1.30866e-12      -1.0062
 9.85443e-13     -19.7585
 6.10442e-13     -14.5819
-2.72028e-13      -12.285
 2.00278e-14  4.28102e-13
  8.6309e-13     -11.7566
-1.30866e-12     -18.9938
-9.85443e-13    -0.241533
-6.10442e-13     -5.41811
 2.72028e-13     -7.71502
-2.00278e-14          -20
-9.29181e-13      -8.2434
 1.57102e-12      -1.0062
 1.30088e-12     -19.7585
 6.49997e-13     -14.5819
-2.16952e-13      -12.285
 1.52767e-13         -inf
-1.52767e-13         -inf
    0.563731 -4.80711e-12
   -0.563731     -32.6131
-> constraint 7 infeasible.
[WARN]: Seidel LP 2D:
v: -1e-09      1
A:
   -0.100799           -1            0
    0.100799            1      -13.545
  -0.0990251  1.55786e-12          -10
   -0.507009 -2.18403e-12          -10
    0.550115 -1.89715e-12          -10
    0.258296 -1.15286e-12          -10
    0.128811  4.47642e-13          -10
   -0.563731   1.1724e-13          -10
   0.0990251 -1.55786e-12          -10
    0.507009  2.18403e-12          -10
   -0.550115  1.89715e-12          -10
   -0.258296  1.15286e-12          -10
   -0.128811 -4.47642e-13          -10
    0.563731  -1.1724e-13          -10
  -0.0990251   1.6751e-12          -10
   -0.507009 -2.64944e-12          -10
    0.550115  -2.4567e-12          -10
    0.258296 -1.22302e-12          -10
    0.128811  3.49942e-13          -10
   -0.563731  1.52767e-13          -10
   0.0990251  -1.6751e-12          -10
    0.507009  2.64944e-12          -10
   -0.550115   2.4567e-12          -10
   -0.258296  1.22302e-12          -10
   -0.128811 -3.49942e-13          -10
    0.563731 -1.52767e-13          -10
-inf	<= x[0] <= inf
0	<= x[1] <= 32.6131
-> infeasible

I haven't had the time to inspect the issue yet.

[leonardoedgar]

Interesting, I will try to provide a minimal reproducible example when I have the time, I am using a forked version, but the algorithm packages/files are all up to date with the latest develop

[hungpham2511]

Can you try with the "Interpolation" discretization scheme? This should be the default behavior on latest develop.