mxche
May 14, 2024, 5:53pm
1
Hi

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!

mxche
May 14, 2024, 6:01pm
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?

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

mxche
May 15, 2024, 9:27am
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

FreyJo
May 15, 2024, 12:26pm
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

mxche
May 15, 2024, 12:37pm
6
Hi Jonathan,

changing `ub`

and `lb`

to 1-d solves this issue.

Thank you!

Best,
Mingxuan

1 Like

FreyJo
May 15, 2024, 12:42pm
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