Run time discussion

[lupusorina]

Hi,

First of all, thanks a lot for writing such a useful and easy to use library.

I'm trying to solve a relatively straightforward problem (see below), in which p, v, a are the position, velocity and accelerations of a foot for a robot in 3D.

I have observed that the optimization problem solves fast (in the order of microseconds) when no safe constraints are added for both velocity and acceleration. However, when adding the constraints, the time increases considerably. My requirement is to solve this problem in under 5 ms, so I was wondering if you have any advice on how to speed up the computation when using both acc and vel constraints.

Some benchmarking below:
nb variables | nb constraints | run time
270 | 279 | 5e-2 -> only acceleration constraints
270 | 189 | 7e-4 -> no safe set acceleration and velocity constraints

I believe the linear equation from Algorithm 1 (line 3) from the OSQP paper takes long to solve due to a high number of constraints (large A). I saw your library also offers options for multithreading using the MKL Library, however, I haven't really explored that route yet. My code implementation is using the OSQP-Eigen library.

Thank you very much.

[goulart-paul]

What is the sparsity of your additional constraints? Are $\mathcal{V}$ and $\mathcal{A}$ box constraint sets, which would just add a bunch of identity blocks to $A$? If so, then I would have guessed that the factorization time for the KKT matrix would not be hugely different, or at least not so different that it would explain the computation time difference you see. If $\mathcal{V}$ and $\mathcal{A}$ are dense then it is a different story of course.

What are:

1. the number of iterations at convergence for each case?
2. the number of KKT matrix refactorizations in each case?

For the second one, I don't recall if we report that value directly anywhere. You could get it by enabling verbose output and counting the number of the times that the rho value changes though.

What happens if you keep the constraints but relax their bounds such that they are never active? Note that you might get bad solver behaviour if you make the bounds really huge, so maybe try solving once with no constraints and then setting bounds on the RHS of the constraints that are slightly looser than the unconstrained optimal solution.

[lupusorina]

Thank you so much for your answer. $\mathcal{V}$ and $\mathcal{A}$ are box constraints and thus A has sparse ones.

I did another benchmarking, and you are right, if we set the bounds huge, then the pb takes longer to compute. It is still a bit slow with constraints, however, definitely an improvement.

[goulart-paul]

I tried to reproduce the issue but haven't been able to do so. If I implement what (I think..) you have above, then I always get a solution in about 100 iterations.

Some further suggestions:

• The problem seems relatively small, or at least the description is fairly compact, so it should be straightforward to implement it in either CVXPY (in python) or JuMP (in Julia). Perhaps try that and post an example here for us to try?

• the problem seems separable in $(x,y,z)$ if $\mathcal{A}$ and $\mathcal{V}$ are boxes. Can you solve it as three separate problems, maybe in parallel?

• If you are solving this problem in a receding horizon scheme, then warm starting the solution at each step from the previous one should (?) bring the median solve time down a lot. It won't help with the first solve though.

• related to the suggestion above, and assuming that the problem is separable, could you make an educated guess about what a nearly optimal solution might be (e.g. based on some plausible constant ramp rate solution), and then use that as an initializer? It may (?) be especially helpful to the solver to provide a good initializer for the dual variables, particularly of they are large at optimality.

[lupusorina]

Amazing. Thanks a lot for taking the time to reproduce the issue. Regarding your suggestions:

1. definitely will post a CVXPY implementation here in a little bit.
2. Great idea. I was thinking about running the pb in parallel, as well, however, I think the pb is simple enough to be run fast.
3. Will do.
4. Yes, I can, for this problem, the solutions are approximately just a set of ramps and 2nd order polynomials.

[lupusorina]

Sorry for taking such a long, I had a deadline for my other project I am working on.

Here are two implementations https://github.com/lupusorina/traj_optimization:

• using CVXPY and osqp as solver
• using OSQP in Python. For the robot, I implemented everything in osqp-eigen, however, it's easier to read the pb in Python. I can share the osqp-eigen implementation as well, if needed.

Thank you.

[goulart-paul]

Thank you. I will have a look at see if I can work out what is going wrong with OSQP.

In the interim, you might also consider a different solver. Changing from cp.OSQP to cp.CLARABEL in CVXPY and leaving everything else the same gives me the output below, with a solve time of about 1.5ms (on an Apple M2, single threaded). The termination tolerances are quite tight for that solver relative to OSQP, so you could probably get away with loosening them a bit. It reaches the same accuracy as OSQP after ~6 iterations, or about 1ms.

That solver is written in Rust, but we intend to release C and C++/Eigen interfaces to it in a few days.

===============================================================================
                                     CVXPY                                     
                                     v1.3.2                                    
===============================================================================
(CVXPY) Jul 27 11:01:28 PM: Your problem has 270 variables, 549 constraints, and 0 parameters.
(CVXPY) Jul 27 11:01:28 PM: It is compliant with the following grammars: DCP, DQCP
(CVXPY) Jul 27 11:01:28 PM: (If you need to solve this problem multiple times, but with different data, consider using parameters.)
(CVXPY) Jul 27 11:01:28 PM: CVXPY will first compile your problem; then, it will invoke a numerical solver to obtain a solution.
-------------------------------------------------------------------------------
                                  Compilation                                  
-------------------------------------------------------------------------------
(CVXPY) Jul 27 11:01:28 PM: Compiling problem (target solver=CLARABEL).
(CVXPY) Jul 27 11:01:28 PM: Reduction chain: Dcp2Cone -> CvxAttr2Constr -> ConeMatrixStuffing -> CLARABEL
(CVXPY) Jul 27 11:01:28 PM: Applying reduction Dcp2Cone
(CVXPY) Jul 27 11:01:28 PM: Applying reduction CvxAttr2Constr
(CVXPY) Jul 27 11:01:28 PM: Applying reduction ConeMatrixStuffing
(CVXPY) Jul 27 11:01:28 PM: Applying reduction CLARABEL
(CVXPY) Jul 27 11:01:28 PM: Finished problem compilation (took 1.426e-01 seconds).
-------------------------------------------------------------------------------
                                Numerical solver                               
-------------------------------------------------------------------------------
(CVXPY) Jul 27 11:01:28 PM: Invoking solver CLARABEL  to obtain a solution.
-------------------------------------------------------------------------------
                                    Summary                                    
-------------------------------------------------------------------------------
(CVXPY) Jul 27 11:01:28 PM: Problem status: optimal
-------------------------------------------------------------
           Clarabel.rs v0.5.1  -  Clever Acronym              

                   (c) Paul Goulart                          
                University of Oxford, 2022                   
-------------------------------------------------------------

problem:
  variables     = 276
  constraints   = 555
  nnz(P)        = 96
  nnz(A)        = 909
  cones (total) = 2
    :        Zero = 1,  numel = 195
    : Nonnegative = 1,  numel = 360

settings:
  linear algebra: direct [/](https://file+.vscode-resource.vscode-cdn.net/) qdldl, precision: 64 bit
  max iter = 200, time limit = Inf,  max step = 0.990
  tol_feas = 1.0e-8, tol_gap_abs = 1.0e-8, tol_gap_rel = 1.0e-8,
  static reg : on, ϵ1 = 1.0e-8, ϵ2 = 4.9e-32
  dynamic reg: on, ϵ = 1.0e-13, δ = 2.0e-7
  iter refine: on, reltol = 1.0e-13, abstol = 1.0e-12,
               max iter = 10, stop ratio = 5.0
  equilibrate: on, min_scale = 1.0e-4, max_scale = 1.0e4
               max iter = 10

iter    pcost        dcost       gap       pres      dres      k/t        μ       step      
---------------------------------------------------------------------------------------------
  0  +6.0668e+02  -2.2326e+06  2.23e+06  5.12e-02  2.79e-13  1.00e+00  8.41e+03   ------   
  1  +5.3138e+02  -2.6291e+05  2.63e+05  6.65e-03  7.64e-14  8.85e+01  1.24e+03  8.67e-01  
  2  +2.5101e+02  -2.2859e+04  2.31e+04  7.99e-04  1.60e-13  6.40e+01  1.19e+02  9.90e-01  
  3  +1.1630e+02  -6.9358e+03  7.05e+03  3.59e-04  4.11e-14  1.79e+01  3.55e+01  9.40e-01  
  4  +1.0421e+02  -7.2141e+01  1.76e+02  9.43e-06  1.36e-13  4.28e-01  8.74e-01  9.79e-01  
  5  +1.0389e+02  +1.0086e+02  3.00e-02  1.32e-07  1.86e-15  7.03e-03  1.48e-02  9.87e-01  
  6  +1.0389e+02  +1.0386e+02  3.04e-04  1.32e-09  1.87e-17  7.33e-05  1.54e-04  9.90e-01  
  7  +1.0389e+02  +1.0389e+02  3.04e-06  1.32e-11  1.03e-16  7.34e-07  1.54e-06  9.90e-01  
  8  +1.0389e+02  +1.0389e+02  3.04e-08  1.32e-13  1.03e-16  7.34e-09  1.54e-08  9.90e-01  
  9  +1.0389e+02  +1.0389e+02  3.08e-10  1.43e-15  1.46e-16  7.33e-11  1.54e-10  9.90e-01  
---------------------------------------------------------------------------------------------
Terminated with status = Solved
solve time = 1.474333ms
(CVXPY) Jul 27 11:01:28 PM: Optimal value: 1.039e+02
(CVXPY) Jul 27 11:01:28 PM: Compilation took 1.426e-01 seconds
(CVXPY) Jul 27 11:01:28 PM: Solver (including time spent in interface) took 1.700e-03 seconds

[goulart-paul]

I have tried various things using the scripts you posted but without a lot of success. The issue is perhaps (?) related to the large number of banded equality conditions and the fact that P is low rank, and even the full rank part of P is badly conditioned. I suspect that the difficulty is not really specific to this solver, but rather a general issue with an ADMM based method since SCS also takes a huge number of iterations.

You could perhaps try eliminating all of the equality conditions and just optimize over the acceleration terms. Note that since v and p are just accumulators on a, you can define a lower triangular matrix $M$ of ones such that (in each axis) v = Ma \in \mathcal{V} and p = M*M*a, multiplied by the time step. Then solve the problem entirely in terms of a only.

That would make the problem a lot smaller but with much denser KKT matrix. I don't have a good sense of whether the problem in that form would require fewer iterations. It would very likely take longer to factor the KKT system, but maybe that doesn't matter to you if you intend to use this in an embedded system where the KKT matrix can be prefactored one time up front, or if you are using the OSQP embedded codegen from python.

[lupusorina]

Thank you so much! I will try your suggestion with using $a$. Currently, OSQP supports only ADMM solvers, right?

[goulart-paul]

Yes, the algorithm in OSQP is ADMM. The other package suggested above is interior point.

[goulart-paul]

FYI: Clarabel C++ wrappers are now available and support calling it using Eigen. See here: https://github.com/oxfordcontrol/Clarabel.cpp