Hi 
This post is a follow-up to my previous post. The constraint-tightening implementation is inspired by this post about variable constraints. and this post about constraint-tightening.
I am implementing probabilistic constraint-tightening for my MPC, so I play around with the parameter p for the constraints. I implemented two variants (with bounded constraint and nonlinear constraint expressions) based on the example code of Acados. I noticed that everything works well with nonlinear constraints, but the optimization is infeasible when using bounded constraints.
What I tried to implement is tightening Fmax by F_tighten from both sides in the input constants, namely, - (Fmax - F_tigthen) <= u <= (Fmax - F_tigthen). The F_tighten is defined as a parameter using p and is set at runtime.
With bounded constraints, I added the variable to the constraint as follows
# tighten the input constraints
ocp.constraints.Jbu = np.eye(nu)
ocp.constraints.lbu = np.array([-Fmax + p_tightening])
ocp.constraints.ubu = np.array([+Fmax - p_tightening])
ocp.constraints.idxbu = np.array([0])
With nonlinear constraint expressions, I rewrite them into half-space expressions,
h_expr = cs.vertcat(-Fmax + p_tightening - ocp.model.u , \
-Fmax + p_tightening + ocp.model.u )
h0_expr = cs.vertcat(-Fmax + p_tightening - ocp.model.u , \
-Fmax + p_tightening + ocp.model.u )
h_ub = 0 * np.ones(h_expr.shape)
h_lb = -1e6 * np.ones(h_expr.shape)
h0_ub = 0 * np.ones(h0_expr.shape)
h0_lb = -1e6 * np.ones(h0_expr.shape)
The parameter is set at runtime
# set parameter values
if constraint_tightening:
for i in range(N):
# tighten the input constraints
ocp_solver.set(i, "p", np.array([F_tighten]))
The nonlinear constraints tighten the constraint by the given amount while the bounded constraints complain an error during QP solving with the following error message
ocp_nlp_sqp: maximum iterations reached
iter res_stat res_eq res_ineq res_comp qp_stat qp_iter alpha
0 4.000000e+03 0.000000e+00 0.000000e+00 0.000000e+00 0 0 0.000000e+00
1 3.769408e+03 2.220446e-16 3.205571e-13 9.167510e-12 0 7 5.764801e-02
2 3.552109e+03 2.220446e-16 6.226347e-13 3.458655e-11 0 7 5.764801e-02
3 3.347337e+03 2.220446e-16 9.072982e-13 7.344135e-11 0 7 5.764801e-02
4 3.154370e+03 4.440892e-16 1.175551e-12 1.232888e-10 0 7 5.764801e-02
5 2.972527e+03 4.440892e-16 1.428340e-12 1.820136e-10 0 7 5.764801e-02
6 2.801166e+03 2.220446e-16 1.666556e-12 2.477882e-10 0 7 5.764801e-02
7 2.639685e+03 2.220446e-16 1.891040e-12 3.190376e-10 0 7 5.764801e-02
8 2.487512e+03 4.440892e-16 2.102582e-12 3.944088e-10 0 7 5.764801e-02
9 2.344112e+03 3.330669e-16 2.301930e-12 4.727425e-10 0 7 5.764801e-02
10 2.208979e+03 6.661338e-16 2.489785e-12 5.530499e-10 0 7 5.764801e-02
11 2.081635e+03 4.440892e-16 2.666811e-12 6.344904e-10 0 7 5.764801e-02
12 1.961633e+03 4.440892e-16 2.833632e-12 7.163535e-10 0 7 5.764801e-02
13 1.848549e+03 6.661338e-16 2.990836e-12 7.980419e-10 0 7 5.764801e-02
14 1.741984e+03 4.440892e-16 3.138977e-12 8.790566e-10 0 7 5.764801e-02
15 1.641562e+03 4.440892e-16 3.278578e-12 9.589848e-10 0 7 5.764801e-02
16 1.546929e+03 6.661338e-16 3.410132e-12 1.037488e-09 0 7 5.764801e-02
17 1.457752e+03 6.661338e-16 3.534102e-12 1.114291e-09 0 7 5.764801e-02
18 1.373715e+03 4.440892e-16 3.650925e-12 1.189177e-09 0 7 5.764801e-02
19 1.294523e+03 4.440892e-16 3.761013e-12 1.261974e-09 0 7 5.764801e-02
20 1.219897e+03 6.661338e-16 3.864756e-12 1.332553e-09 0 7 5.764801e-02
21 1.149572e+03 6.661338e-16 3.962517e-12 1.400822e-09 0 7 5.764801e-02
22 1.083301e+03 4.440892e-16 4.054643e-12 1.466715e-09 0 7 5.764801e-02
23 1.020851e+03 6.661338e-16 4.141458e-12 1.530196e-09 0 7 5.764801e-02
24 9.620013e+02 6.661338e-16 4.223268e-12 1.591248e-09 0 7 5.764801e-02
25 9.065438e+02 6.661338e-16 4.300362e-12 1.649874e-09 0 7 5.764801e-02
26 8.542834e+02 4.440892e-16 4.373012e-12 1.706090e-09 0 7 5.764801e-02
27 8.050356e+02 4.440892e-16 4.441474e-12 1.759928e-09 0 7 5.764801e-02
28 7.586269e+02 4.440892e-16 4.505989e-12 1.811427e-09 0 7 5.764801e-02
29 7.148936e+02 6.661338e-16 4.566785e-12 1.860637e-09 0 7 5.764801e-02
30 6.736814e+02 6.661338e-16 4.624076e-12 1.907614e-09 0 7 5.764801e-02
31 6.348450e+02 8.881784e-16 4.678064e-12 1.952418e-09 0 7 5.764801e-02
32 5.982474e+02 6.661338e-16 4.728940e-12 1.995116e-09 0 7 5.764801e-02
33 5.637597e+02 6.661338e-16 4.776883e-12 2.035775e-09 0 7 5.764801e-02
34 5.312601e+02 6.661338e-16 4.822062e-12 2.074466e-09 0 7 5.764801e-02
35 5.006340e+02 4.440892e-16 4.864637e-12 2.111259e-09 0 7 5.764801e-02
36 4.717734e+02 4.440892e-16 4.904758e-12 2.146227e-09 0 7 5.764801e-02
37 4.445766e+02 4.440892e-16 4.942565e-12 2.179442e-09 0 7 5.764801e-02
38 4.189477e+02 6.661338e-16 4.978193e-12 2.210976e-09 0 7 5.764801e-02
39 3.947962e+02 6.661338e-16 5.011767e-12 2.240900e-09 0 7 5.764801e-02
40 3.720369e+02 6.661338e-16 5.043406e-12 2.269282e-09 0 7 5.764801e-02
41 3.505898e+02 8.881784e-16 5.073221e-12 2.296192e-09 0 7 5.764801e-02
42 3.303790e+02 8.881784e-16 5.101317e-12 2.321695e-09 0 7 5.764801e-02
43 3.113333e+02 1.110223e-15 5.127793e-12 2.345857e-09 0 7 5.764801e-02
44 2.933855e+02 1.110223e-15 5.152743e-12 2.368741e-09 0 7 5.764801e-02
45 2.764724e+02 1.110223e-15 5.176255e-12 2.390407e-09 0 7 5.764801e-02
46 2.605343e+02 1.110223e-15 5.198411e-12 2.410915e-09 0 7 5.764801e-02
47 2.455151e+02 1.110223e-15 5.219290e-12 2.430320e-09 0 7 5.764801e-02
48 2.313616e+02 1.110223e-15 5.238966e-12 2.448678e-09 0 7 5.764801e-02
49 2.180241e+02 1.110223e-15 5.257507e-12 2.466041e-09 0 7 5.764801e-02
50 2.054554e+02 1.332268e-15 5.274979e-12 2.482459e-09 0 7 5.764801e-02
51 1.936113e+02 1.332268e-15 5.291444e-12 2.497980e-09 0 7 5.764801e-02
52 1.824500e+02 1.554312e-15 5.306960e-12 2.512651e-09 0 7 5.764801e-02
53 1.719321e+02 1.554312e-15 5.321581e-12 2.526516e-09 0 7 5.764801e-02
54 1.620206e+02 1.554312e-15 5.335360e-12 2.539616e-09 0 7 5.764801e-02
55 1.526804e+02 1.554312e-15 5.348344e-12 2.551992e-09 0 7 5.764801e-02
56 1.438787e+02 1.776357e-15 5.360580e-12 2.563682e-09 0 7 5.764801e-02
57 1.355844e+02 1.998401e-15 5.372110e-12 2.574722e-09 0 7 5.764801e-02
58 1.277682e+02 1.998401e-15 5.382976e-12 2.585148e-09 0 7 5.764801e-02
59 1.204026e+02 1.998401e-15 5.393215e-12 2.594992e-09 0 7 5.764801e-02
60 1.134617e+02 1.998401e-15 5.402864e-12 2.604286e-09 0 7 5.764801e-02
61 1.069208e+02 1.998401e-15 5.411957e-12 2.613059e-09 0 7 5.764801e-02
62 1.007570e+02 1.776357e-15 5.420525e-12 2.621340e-09 0 7 5.764801e-02
63 9.494860e+01 1.776357e-15 5.428600e-12 2.629156e-09 0 7 5.764801e-02
64 8.947500e+01 1.776357e-15 5.436209e-12 2.636531e-09 0 7 5.764801e-02
65 8.431695e+01 1.776357e-15 5.443380e-12 2.643491e-09 0 7 5.764801e-02
66 7.945624e+01 1.776357e-15 5.450137e-12 2.650058e-09 0 7 5.764801e-02
67 7.487575e+01 1.776357e-15 5.456504e-12 2.656254e-09 0 7 5.764801e-02
68 7.055931e+01 1.776357e-15 5.462505e-12 2.662099e-09 0 7 5.764801e-02
69 6.649171e+01 1.776357e-15 5.468159e-12 2.667613e-09 0 7 5.764801e-02
70 6.265859e+01 1.776357e-15 5.473488e-12 2.672815e-09 0 7 5.764801e-02
71 5.904645e+01 1.776357e-15 5.478509e-12 2.677721e-09 0 7 5.764801e-02
72 5.564254e+01 1.776357e-15 5.483241e-12 2.682349e-09 0 7 5.764801e-02
73 5.106014e+01 1.554312e-15 5.489611e-12 2.688585e-09 0 7 8.235430e-02
74 4.811662e+01 1.554312e-15 5.493703e-12 2.692595e-09 0 7 5.764801e-02
75 4.534279e+01 1.554312e-15 5.497559e-12 2.696376e-09 0 7 5.764801e-02
76 4.272887e+01 1.554312e-15 5.501193e-12 2.699942e-09 0 7 5.764801e-02
77 4.026564e+01 1.776357e-15 5.504617e-12 2.703304e-09 0 7 5.764801e-02
78 3.552843e+01 1.332268e-15 5.511203e-12 2.709776e-09 0 7 1.176490e-01
79 3.348028e+01 1.332268e-15 5.514050e-12 2.712577e-09 0 7 5.764801e-02
80 3.155021e+01 1.332268e-15 5.516733e-12 2.715217e-09 0 7 5.764801e-02
81 2.783836e+01 1.110223e-15 5.521893e-12 2.720299e-09 0 7 1.176490e-01
82 2.623353e+01 1.110223e-15 5.524124e-12 2.722497e-09 0 7 5.764801e-02
83 2.472122e+01 1.110223e-15 5.526226e-12 2.724570e-09 0 7 5.764801e-02
84 2.329609e+01 1.110223e-15 5.528208e-12 2.726524e-09 0 7 5.764801e-02
85 2.195312e+01 1.110223e-15 5.530074e-12 2.728366e-09 0 7 5.764801e-02
86 1.826346e+01 8.881784e-16 5.535204e-12 2.733429e-09 0 7 1.680700e-01
87 1.721061e+01 8.881784e-16 5.536667e-12 2.734875e-09 0 7 5.764801e-02
88 1.579324e+01 8.881784e-16 5.538638e-12 2.736822e-09 0 7 8.235430e-02
89 1.488279e+01 8.881784e-16 5.539903e-12 2.738073e-09 0 7 5.764801e-02
90 1.402483e+01 8.881784e-16 5.541096e-12 2.739252e-09 0 7 5.764801e-02
91 1.321632e+01 8.881784e-16 5.542220e-12 2.740363e-09 0 7 5.764801e-02
92 1.004308e+01 6.661338e-16 5.546631e-12 2.744727e-09 0 7 2.401000e-01
93 9.464121e+00 6.661338e-16 5.547436e-12 2.745524e-09 0 7 5.764801e-02
94 6.217928e+00 2.220446e-16 5.551949e-12 2.749992e-09 0 7 3.430000e-01
95 5.859476e+00 2.220446e-16 5.552447e-12 2.750486e-09 0 7 5.764801e-02
96 5.521689e+00 2.220446e-16 5.552917e-12 2.750951e-09 0 7 5.764801e-02
97 5.203375e+00 2.220446e-16 5.553359e-12 2.751390e-09 0 7 5.764801e-02
98 4.903411e+00 2.220446e-16 5.553776e-12 2.751803e-09 0 7 5.764801e-02
99 4.620739e+00 2.220446e-16 5.554169e-12 2.752192e-09 0 7 5.764801e-02
100 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0 7 5.764801e-02
Exception: acados returned status 2.
The error is reproducible with the following code based on the getting_started/minimal_example_ocp.py.
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
F_tighten = 70 # tighten Fmax by F_tighten from both sides of the input constraints
'''
set use_nonlinear_constraints_expression = False to use the original bounded constraints and reproduce the error
'''
use_nonlinear_constraints_expression = True
# use_nonlinear_constraints_expression = False
constraint_tightening = True
# constraint_tightening = False
if constraint_tightening:
# tightening variable
p_tightening = cs.SX.sym('p_tightening')
# pass the parameter to the model
ocp.model.p = p_tightening
# set dummy parameter values (will be overwritten later)
ocp.parameter_values = np.array([0])
if not use_nonlinear_constraints_expression:
if constraint_tightening:
# tighten the input constraints
ocp.constraints.Jbu = np.eye(nu)
ocp.constraints.lbu = np.array([-Fmax + p_tightening])
ocp.constraints.ubu = np.array([+Fmax - p_tightening])
ocp.constraints.idxbu = np.array([0])
else:
# original bounded constraints
ocp.constraints.Jbu = np.eye(nu)
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
else:
# nonlinear constraints expression
if constraint_tightening:
# define the constraint expressions
h_expr = cs.vertcat(-Fmax + p_tightening - ocp.model.u , \
-Fmax + p_tightening + ocp.model.u )
h0_expr = cs.vertcat(-Fmax + p_tightening - ocp.model.u , \
-Fmax + p_tightening + ocp.model.u )
else:
h_expr = cs.vertcat(-Fmax - ocp.model.u, \
-Fmax + ocp.model.u)
h0_expr = cs.vertcat(-Fmax - ocp.model.u, \
-Fmax + ocp.model.u)
h_ub = 0 * np.ones(h_expr.shape)
h_lb = -1e6 * np.ones(h_expr.shape)
h0_ub = 0 * np.ones(h0_expr.shape)
h0_lb = -1e6 * np.ones(h0_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.model.con_h_expr_0 = h0_expr
ocp.dims.nh_0 = h0_expr.shape[0]
ocp.constraints.uh_0 = h0_ub
ocp.constraints.lh_0 = h0_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))
# set parameter values
if constraint_tightening:
for i in range(N):
# tighten the input constraints
ocp_solver.set(i, "p", np.array([F_tighten]))
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")
print('simU', simU.T)
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()
Is there anything wrong with my bounded constraint implementation?
Thank you in advance.
Best,
Mingxuan