# Constraint Tightening for OCP example

Hey there, I need to use constraint tightening for my thesis, so I first wanted to implement it in a simple example based on the examples in `<acados_root>/examples/acados_python/getting_started`. As a model, I used the Van der Pol oscillator. I applied the given commands from the posts

but by setting the costs `Q_mat` and `R_mat` to all zeros, it shows that the constraint-tightening does not work, since it returns an error. If the constraints really would work, the state would have to approach the origin even without weights in `Q_mat`, just to not violate the constraints.

Can you help me and tell me, what I did implement wrong? The constraint tightening is implemented in the closed loop

File: `main.py`:

``````from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver
from pendulum_model import export_pendulum_ode_model
import numpy as np
import scipy.linalg

from utils import create_model, make_test_tube, plot_result

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

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

# control parametres
Tf = 1.0
nx = model.x.size()[0] # = 2
nu = model.u.size()[0] # = 1
ny = nx + nu # = 3
ny_e = nx # = 2
N_horizon = 20

# set dimensions
ocp.dims.N = N_horizon # = 20

# set cost module

ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'

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

ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) # was sind W, Vx und Vu?

ocp.cost.W_e = Q_mat

ocp.cost.Vx = np.zeros((ny, nx)) # 3 x 2 Matrix
ocp.cost.Vx[:nx,:nx] = np.eye(nx)

Vu = np.zeros((ny, nu)) # 3 x 1 Matrix
Vu[2,0] = 1.0
ocp.cost.Vu = Vu

ocp.cost.Vx_e = np.eye(nx) # 2 x 2 Matrix

ocp.cost.yref  = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))

# set constraints
Nsim = 100
total_tube = make_test_tube(Nsim + N_horizon, 5, 0.2, mode = 'asymp')

x_bound_base = np.array([total_tube[0], total_tube[0]]) # absolute distance of constraints (lbx = - x_bound_base, ubx = x_bound_base), NOT considering the tube
# x_bound_base = np.array([])
# Fmax = float('inf')
x0 = np.array([2, 3])
ocp.constraints.constr_type = 'BGH'
# ocp.constraints.lbu = np.array([-Fmax])
# ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.x0 = x0
# ocp.constraints.idxbu = np.array([0])
ocp.constraints.idxbx = np.array([0, 1])
ocp.constraints.ubx = x_bound_base
ocp.constraints.lbx = - x_bound_base

ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI

ocp.solver_options.qp_solver_cond_N = N_horizon

# set prediction horizon
ocp.solver_options.tf = Tf

solver_json = 'acados_ocp_' + model.name + '.json'

# create an integrator with the same settings as used in the OCP solver.

simX = np.ndarray((Nsim+1, nx))
simU = np.ndarray((Nsim, nu))

simX[0,:] = x0

# closed loop
for i in range(Nsim):
x_bound_base = np.array([total_tube[i], total_tube[i]])
ocp.constraints.lbx = - x_bound_base
ocp.constraints.ubx = x_bound_base
for stage in range(1, N_horizon): # my desperate try to implement constraint tightening
x_bound_loop = np.array([total_tube[i + stage], total_tube[i + stage]])

# solve ocp and get next control input

# simulate system
simX[i+1, :] = acados_integrator.simulate(x=simX[i, :], u=simU[i,:])

# plot results

plot_result(simX)

if __name__ == '__main__':
main()
``````

File: `utils.py`:

``````import numpy as np
from matplotlib import pyplot as plt

name = 'VanDerPol'

# states

x1 = SX.sym('x1')
x2 = SX.sym('x2')
x = vertcat(x1, x2)

# state derrivatives

dx1 = SX.sym('dx1')
dx2 = SX.sym('dx2')
dx = vertcat(dx1, dx2)

# inputs

F = SX.sym('F')
u = vertcat(F)

# dynamics

f_expl = vertcat(x2,
eps * omeg * (1 - x1**2) * x2 - omeg**2 * x1) + F
f_impl = dx - f_expl

model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = dx
model.u = u
model.name = name

return model

def make_test_tube(N, start, end, mode = 'linear') -> np.ndarray:
tube = np.empty((N,))
if mode == 'linear' or mode == 'lin':
for i in range(N):
tube[i] = ((N - 1 - i) / (N - 1)) * start + (i / (N - 1)) * end
elif mode == 'exponential' or mode == 'exp':
if start == 0:
raise ValueError('When an exponential tube is used, start cannot be equal to 0.')
elif end == 0:
raise ValueError('When an exponential tube is used, end cannot be equal to 0.')
factor = np.log(end / start) / (N - 1)
for i in range(N):
tube[i] = start * np.exp(factor * i)
elif mode == 'asymptotic' or mode == 'asymp':
if start == 0:
raise ValueError('When an asymptotic tube is used, start cannot be equal to 0.')
elif end == 0:
raise ValueError('When an asymptotic tube is used, end cannot be equal to 0.')
for i in range(N):
tube[i] = 1 / (((N - 1 - i) / (N - 1)) / start + (i / (N - 1)) / end)
elif mode == 'logarithmic' or mode == 'log':
for i in range(N):
tube[i] = np.log(((N - 1 - i) / (N - 1)) * np.exp(start) + (i / (N - 1)) * np.exp(end))
else:
raise ValueError('The mode of make_tube must be \'linear\' / \'lin\', \'exponential\' / \'exp\', \'asymptotic\' / \'asymp\' or \'logarithmic\' / \'log\'.')
return tube

def plot_result(x_array: np.ndarray) -> None:
assert len(x_array.shape) == 2
assert x_array.shape[1] == 2
fig, ax = plt.subplots(1, 1, figsize = (10, 10))
ax.plot(x_array[:, 0], x_array[:, 1], label='Trajectory')
ax.legend()
ax.set_aspect(1) # both lines work equally
# plt.gca().set_aspect('equal') # both lines work equally
fig.tight_layout()
plt.show()
return None

if __name__ == '__main__':
raise Exception('This script is NOT supposed to be run on its own!')
``````

Thank you very much for your help!
Marco