def __set_constraints(self, ocp):
"""
Set the constraints from the ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# constraints handling in self.acados_ocp
if ocp.nlp[0].x_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the x_bounds"
)
if ocp.nlp[0].u_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the u_bounds"
)
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
# TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i, nlp in enumerate(ocp.nlp):
x = nlp.states.cx
u = nlp.controls.cx
p = nlp.parameters.cx
for g, G in enumerate(nlp.g):
if not G:
continue
if G.node[0] == Node.ALL or G.node[0] == Node.ALL_SHOOTING:
self.all_constr = vertcat(self.all_constr, G.function(x, u, p))
self.all_g_bounds.concatenate(G.bounds)
if G.node[0] == Node.ALL:
self.end_constr = vertcat(self.end_constr, G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
elif G.node[0] == Node.END:
self.end_constr = vertcat(self.end_constr, G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError("u_bounds min must be the same at each shooting point with ACADOS")
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError("u_bounds max must be the same at each shooting point with ACADOS")
if (
not np.isfinite(u_min).all()
or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()
):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it "
"to a big value instead."
)
# setup state constraints
# TODO replace all these np.concatenate by proper bound and initial_guess classes
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.nparams:
param_bounds_max = self.params_bounds.max[:, 0]
param_bounds_min = self.params_bounds.min[:, 0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate((param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate((param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0])
self.acados_ocp.constraints.idxbu = np.array(range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:, 0])
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N * [f] + [f_N])
ls_cost.ls_cost_matrix = N * [block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
nlp.set_path_constraints(N * [g] + [g_N])
for i in range(N):
nlp.lg[i] = -0.5
nlp.ug[i] = +0.5
solver = ocp_nlp_solver(
'sqp', nlp, {
'integrator_steps': 2,
'qp_solver': 'condensing_qpoases',
'sensitivity_method': 'gauss-newton'
})
# Simulation
STATES = [array([0.1, 0.1])]
CONTROLS = []
# set dimensions
ocp.dims.N = N
# set cost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
W_u = 1e-3
theta = model.x[1]
ocp.model.cost_expr_ext_cost = tanh(theta)**2 + .5*(model.x[0]**2 + W_u*model.u**2)
ocp.model.cost_expr_ext_cost_e = tanh(theta)**2 + .5*model.x[0]**2
custom_hess_u = W_u
J = horzcat(SX.eye(2), SX(2,2))
print(DM(J.sparsity()))
# diagonal matrix with second order terms of outer loss function.
D = SX.sym('D', Sparsity.diag(2))
D[0, 0] = 1
[hess_tan, grad_tan] = hessian(tanh(theta)**2, theta)
D[1, 1] = if_else(theta == 0, hess_tan, grad_tan/theta)
custom_hess_x = J.T @ D @ J
zeros = SX(1, nx)
cost_expr_ext_cost_custom_hess = blockcat(custom_hess_u, zeros, zeros.T, custom_hess_x)
cost_expr_ext_cost_custom_hess_e = custom_hess_x
from casadi import SX, DM, vertcat, reshape, Function, nlpsol, inf, norm_2
import matplotlib.pyplot as plt
import random
if __name__ == '__main__':
# Parameter konfiguration
A_B = 2.8274e-3 # [m**2]
A_SP = 0.4299e-3 # [m**2]
m = 2.8e-3 # [kg]
g = 9.81 # [m/(s**2)]
T_M = 0.57 # [s]
k_M = 0.31 # [s**-1]
k_V = 6e-5 # [m**3]
k_L = 2.18e-4 # [kg/m]
eta_0 = 1900 / 60 # / 60 * 2 * pi
A_d = SX([[1, 0.1, 0], [0, -0.14957, 0.024395], [0, 0, 0.82456]])
B_d = SX([[0], [0], [0.054386]])
h_ub = 2
h_lb = 0
eta_ub = 200
eta_lb = 0
# Abtastzeit und Prediction horizon
T = 0.1
N_pred = 19
# Zielruhelage von den Zustaenden
# Referenz von h, h_p, eta
state_r = [0.8, 0, 48.760258862]
u_r = [157.291157619]
# activate_ips_on_exception()
def lamb_sym(self):
if self.has_adjoint_variables:
return self.x[self.n_x // 2:]
else:
return SX()