Implementing a Parallel QP Solver with Cusadi

[HariP19]

Hello! First off, thank you for developing and sharing Cusadi. I've been looking for something like this my work. I'm currently trying to implement a parallel QP solver for a MPC application. I read through your paper tried recreating the Pentaly QP solver that you've discussed in the paper I had a few questions after reading your implementation.

Here's the Pseudo code of the Penalty QP solver that I implemented.

Algorithm PenaltyQP

// Given:
//    Minimize f(x) = (1/2) xᵀ Q x + qᵀ x + μ ||max(0, Gx - h)||²
//    Subject to A x = b

Fixed:
    n (number of variables)
    m_eq (number of equality constraints)
    m_ineq (number of inequality constraints)
    max_newton_steps (maximum number of Newton iterations)

Input:
    Q ∈ Rⁿˣⁿ (Quadratic cost matrix)
    q ∈ Rⁿ
    G ∈ Rᵐⁱⁿˣⁿ (inequality constraint matrix)
    h ∈ Rᵐⁱⁿᵉ (inequality constraint vector)
    A ∈ Rᵐᵉˣⁿ (equality constraint matrix)
    b ∈ Rᵐᵉ (equality constraint vector)
    x⁰ ∈ Rⁿ (initial guess for x)
    λ⁰ ∈ Rᵐᵉ (initial Lagrange multipliers)
    μ > 0 (penalty multiplier)

Initialize:
    x_curr ← x⁰
    λ_curr ← λ⁰

Solve Newton-Steps for max_newton_steps:
    1. Compute gradient and Hessian:
        grad_f ← Gradient of penalized cost with respect to x at x_curr
        hess_f ← Hessian of penalized cost with respect to x at x_curr

    2. Construct KKT matrix and right-hand side:
        // KKT matrix:
        KKT ← [ hess_f  Aᵀ ]
               [ A       0  ]
        // Right-hand side:
        rhs ← [ -(grad_f + Aᵀ λ_curr) ]
               [ -(A x_curr - b)     ]

    3. Solve KKT system for (dx, dλ):
        delta ← Solve(KKT, rhs)
        dx ← delta[0:n]
        dλ ← delta[n:n+meq]

    4. Update variables:
        x_curr ← x_curr + dx
        λ_curr ← λ_curr + dλ

Output:
    x_solution ← x_curr
    λ_solution ← λ_curr

// Outside of the loop: 
// Increase μ (e.g., multiply by 10), 
// x⁰ = x_solution,
// λ⁰ = λ_solution
// repeat until convergence.

The above code works as expected and I'm seeing very good results when I benchmarked the parallel QP against other QP solvers sequentially solving multiple QP problems (decision variables 16, inequality constraints 32). However I have a few questions before I can proceed further and I was hoping you can answer them.

Questions:

1. Newton Iterations and Solving the Equality-Constrained QP:
   1. My understanding is that Cusadi functions must have a fixed structure, so I have to pre-fix the number of newton iterations and size of the vectors that are passed as input to the functions. Is this correct?
   2. In my implementation, I'm using a fixed number of newton steps (around 5-8) & casadi's inbuilt linear solver to solve each equality-constrained QP. In your paper, you mentioned using an LDLT factorization to solve the linear KKT system, but there's no mention of any newton steps. Can I first check if my approach is similar to yours. If so, can you tell me what number of Newton iterations did you find effective? If not, could you provide some more details about your implementation, as I'm trying to make my QP solver as efficient as possible.
2. Large Problem size and Compilation Time:
   1. The MPC problem that I want to solve parallely is direct multiple shooting. As a result it is quite large as I want to optimize the entire state and input trajectory over the horizon. This results in a very large Casadi function in terms of file size and consequently, the run_codegen.py is taking very long time. Can I check if you encountered this problem for the humanoid SQP solver? Are there any strategies or optimizations within Cusadi to solve this long compilation time for large problems?

I greatly appreciate any insights or guidance you can provide. Thanks again for your work!

[jnskkmhr]

Hi @HariP19
Have you solved this issue?
I am also facing the same problem as 2nd issue you brought up.
Even though my problem is single shooting and number of decision variable is 12*horizon_length (usually around 10), it is taking long time for compilation. (10sec for horizon=1)

@se-hwan
Thank you so much for this great work.
It would be great if you could give us some insight about this issue.

My qp solver looks like this (penalty-based like yours)

def create_qp_solver_function(num_z:int, num_eq:int, num_ineq:int):
    """
    casadi funcion arguments:
    H: torch.Tensor of shape (nz, nz)
    g: torch.Tensor of shape (nz, 1)
    A: torch.Tensor of shape (neq, nz)
    b: torch.Tensor of shape (neq, 1)
    G: torch.Tensor of shape (nineq, nz)
    h: torch.Tensor of shape (nineq, 1)
    
    solve
    min_z 1/2 z^T H z + g^T
    s.t. 
    Az = b
    Gz <= h
    
    Move inequality constraint to objective function using penalty method
    min _z, lambda L = 1/2 z^T H z + g^T z + rho/2 (max(0, Gz - h))^2
    s.t. 
    Az = b
    
    Use augmented lagrangian and construct KKT matrix, then solve linear problem. 
    L = 1/2 z^T H z + g^T z + rho/2 (max(0, Gz - h))^2 + lambda^T (Az - b)
    
    KKT condition 
    H z + g + A^T lambda + G^T mu + rho * G^T (Gz-h) = 0 (stationarity)
    Az -b = 0 (primal feasibility)
    
    Above KKT condition reduces problem to linear problem
    Px= q
    P = 
    [H+rho*G^TG, A^T;
     A, 0]
    q = [-g+rho*G^Th; b]
    """
    
    # define input qp matrices
    H = casadi.SX.sym('H', num_z, num_z)
    g = casadi.SX.sym('g', num_z)
    A = casadi.SX.sym('A', num_eq, num_z)
    b = casadi.SX.sym('b', num_eq)
    G = casadi.SX.sym('G', num_ineq, num_z)
    h = casadi.SX.sym('h', num_ineq)
    rho = casadi.SX.sym('rho', 1)
    
    # construct KKT matrix
    P1 = casadi.horzcat(H + rho*G.T @ G, A.T)
    P2 = casadi.horzcat(A, casadi.SX.zeros(num_eq, num_eq))
    P = casadi.vertcat(P1, P2)
    
    q = casadi.vertcat(-g + rho*G.T @ h, b)
    
    # solve KKT system Px = q
    res = casadi.solve(P, q)
    
    z_prime = res[:num_z]
    # multiplier = res[num_z:]
    
    # create function
    qp_solver = casadi.Function('qp_solver', [H, g, A, b, G, h, rho], [z_prime])
    
    return qp_solver