Infeasible QPs when using nonlinear casadi constraint expressions

User Questions
, ,
1

Hi :wave:

I am using acados python to solve my MPC problems. Since I would like to cope with complex constraints in the MPC formulation, I tried to play around with the nonlinear constraint expression. I found the following problem when I write a linear constraint as a casadi expression h.

My MPC was running nicely with the following bounded input constraints -1 <= u <= 1

nu = 1
ocp.constraints.Jbu = np.eye(nu)
ocp.constraints.lbu = np.array([-1])
ocp.constraints.ubu = np.array([1])
ocp.constraints.idxbu = np.arange(nu)

However, when I switch it to a casadi expression as follows, which basically convert the bounded constraint to -inf < -1 - u <= 0 and -inf < u - 1 <= 0. I learned this from this example code.

h_expr = cs.vertcat(-1 - ocp.model.u, -1 + ocp.model.u)
h_ub = 0 * np.ones(h_expr.shape)
h_lb = -1e15 * np.ones(h_expr.shape)
# pass the constraints to the ocp object
ocp.model.con_h_expr = h_expr
ocp.dims.nh = h_expr.shape[0]
ocp.constraints.uh = h_ub
ocp.constraints.lh = h_lb

The optimization is no longer feasible.


QP solver returned error status 3 in SQP iteration 1, QP iteration 7.

iter    res_stat        res_eq          res_ineq        res_comp        qp_stat qp_iter alpha
0       9.958037e-02    2.262079e-15    1.102833e+01    0.000000e+00    0       0       0.000000e+00
1       0.000000e+00    0.000000e+00    0.000000e+00    0.000000e+00    3       7       0.000000e+00

Could you hint a bit about what could lead to this infeasibility? Any suggestion on how to improve this?

Thanks in advance!

2

I also tried adding slack variables. This does help with the infeasibility issue.

# slack variables for nonlinear constraints
ocp.constraints.Jsh = np.eye(h_expr.shape[0])
L2_pen = 0.01
L1_pen = 0.01
ocp.cost.Zu = L2_pen * np.ones(h_expr.shape[0])
ocp.cost.Zl = L2_pen * np.ones(h_expr.shape[0])
ocp.cost.zl = L1_pen * np.ones(h_expr.shape[0]) 
ocp.cost.zu = L1_pen * np.ones(h_expr.shape[0])

But this adds some hand-tuning of the slack penalty. Is there a more elegant way to solve this problem?

3

Hi,

probably the issue is that the upper bound value 1e15 leads to numerical problems in the QP solver. I suggest using something smaller, like 1e6.
Basically, the best way to do this is to choose the smallest value such that you still no the constraint you do not want will never be active.
Let me know if that works.

Also adding support for one-sided constraints is moving more up in the acados development todo list Handling One-Sided Constraints · Issue #650 · acados/acados · GitHub

Best,
Jonathan

4

Hi Jonathan,

Thank you for your response.

I tried some values from 1e1 to 1e8, but this hasn’t resolved the issue.

To reproduce the error, I changed the minimal_example_ocp.py in the getting_started example folder.

from acados_template import AcadosOcp, AcadosOcpSolver
from pendulum_model import export_pendulum_ode_model
import numpy as np
from utils import plot_pendulum
import casadi as cs

def main():
    # create ocp object to formulate the OCP
    ocp = AcadosOcp()

    # set model
    model = export_pendulum_ode_model()
    ocp.model = model

    Tf = 1.0
    nx = model.x.rows()
    nu = model.u.rows()
    N = 20

    # set number of shooting intervals
    ocp.dims.N = N

    # set prediction horizon
    ocp.solver_options.tf = Tf

    # set cost
    Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2])
    R_mat = 2*np.diag([1e-2])

    # 'EXTERNAL' cost type 
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL'
    ocp.model.cost_expr_ext_cost = model.x.T @ Q_mat @ model.x + model.u.T @ R_mat @ model.u
    ocp.model.cost_expr_ext_cost_e = model.x.T @ Q_mat @ model.x

    # set constraints
    Fmax = 80

    # original bounded constraints which work well
    # ocp.constraints.lbu = np.array([-Fmax])
    # ocp.constraints.ubu = np.array([+Fmax])
    # ocp.constraints.idxbu = np.array([0])
    
    # nonlinear constraints expression that will result in qp solver error
    h_expr = cs.vertcat(-Fmax - ocp.model.u, -Fmax + ocp.model.u)
    h_ub = 0 * np.ones(h_expr.shape) 
    h_lb = -1e7 * np.ones(h_expr.shape)
    ocp.model.con_h_expr = h_expr
    ocp.dims.nh = h_expr.shape[0]
    ocp.constraints.uh = h_ub
    ocp.constraints.lh = h_lb

    ocp.constraints.x0 = np.array([1.0, 0, 0.0, 0.0])

    # set options
    ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 
    ocp.solver_options.integrator_type = 'IRK'
    # ocp.solver_options.print_level = 1
    ocp.solver_options.nlp_solver_type = 'SQP' 
    ocp.solver_options.globalization = 'MERIT_BACKTRACKING' 

    ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')

    simX = np.zeros((N+1, nx))
    simU = np.zeros((N, nu))

    status = ocp_solver.solve()
    ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")

    if status != 0:
        raise Exception(f'acados returned status {status}.')

    # get solution
    for i in range(N):
        simX[i,:] = ocp_solver.get(i, "x")
        simU[i,:] = ocp_solver.get(i, "u")
    simX[N,:] = ocp_solver.get(N, "x")

    plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=True, time_label=model.t_label, x_labels=model.x_labels, u_labels=model.u_labels)
    

if __name__ == '__main__':
    main()

Best,
Mingxuan

5

Hi Mingxuan,

I just checked and the issue was that the bounds you provided for h where two dimensional numpy arrays instead of one dimensional ones.
This lead to the solver using zeros instead of the large value.
I just implemented a sanity check in acados, such that this cannot occur again, see Python: check if bounds are 1-dimensional numpy arrays by FreyJo · Pull Request #1104 · acados/acados · GitHub

Best,
Jonathan

1 Like
6

Hi Jonathan,

changing ub and lb to 1-d solves this issue.

Thank you!

Best,
Mingxuan

1 Like
7

Great!
Just to briefly add, h is only enforced at the intermediate stages. To also enforce it at the initial stage, set h_0 too.

1 Like