[C++] numerical issue

Hi @hungpham2511

I encounter a numerical issue at this line: https://github.com/hungpham2511/toppra/blob/5ffa8e1b92c8151e837a2fd4253948860d9bbc38/cpp/src/toppra/solver/glpk-wrapper.cpp#L113

Function intersection(a, b, c) computes the intersection between interval a and interval b and stores it into c. At some point, I get a = [0.11138810763690521, 0.11138810763690521 ] and b = [ 0, 0.11138810763690514 ]. If you look carefully at the values, you will see that:

• the intersection is empty and
• the upper bound of b is very close to a.

I get this during the forward pass. In my scenario, I have only one linear velocity constraint (which gives b). When the fault happens, the a value is the x value passed to the solver (so the feasible set at the index i).

I can make a work around in this solver but I am wondering whether this should be addressed at a higher stage, like in the TOPPRA algorithm itself.

Any thoughts on this ?

[hungpham2511]

Yes for qpOASES. I measured it. Also, the worst-case time-complexity of 2D LP is O(m^2), while 1D LP is O(m). So the speedup is quite minimal for fewer constraints. Nothing very major.

That said if we really want to get the best possible performance out of the algorithm, then each LP should also use information from the last, i.e., what constraints are active in the last step. Since we know for sure that there can be a fixed number of constraint-switching in the final trajectory, the time-complexity is actually ~ O(1) for a fine gridpoint. This is the main motivation for using qpOASES with its warmstart functionality.

[jmirabel]

Thanks for your insight. It is helpful.

Actually this is even better because the line search is much simpler to solve.

I agree the problem is simpler to solver but I expect the solver to actually do that simplification:

• GPLK knows when a variable is fixed,
• I am not sure for qpOASES but as equality constraints are built from inequalities, I would expect it to detect fixed variables.

Did you check that it is actually faster ?