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
Constraint tightening in the Python interface and
How can i set a Variable constraints ,
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
ocp = AcadosOcp()
# 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'
acados_ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json)
# create an integrator with the same settings as used in the OCP solver.
acados_integrator = AcadosSimSolver(ocp, json_file = solver_json)
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]])
#acados_ocp_solver.constraints_set(stage, "lbx", - x_bound_loop)
#acados_ocp_solver.constraints_set(stage, "ubx", x_bound_loop)
acados_ocp_solver.set(stage, "lbx", - x_bound_loop)
acados_ocp_solver.set(stage, "ubx", x_bound_loop)
# solve ocp and get next control input
simU[i,:] = acados_ocp_solver.solve_for_x0(x0_bar=simX[i, :])
# 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 acados_template import AcadosModel
from casadi import SX, vertcat
from matplotlib import pyplot as plt
def create_model(eps=1, omeg=1) -> AcadosModel:
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 = AcadosModel()
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