class AcadosInterface(SolverInterface):
"""
The ACADOS solver interface
Attributes
----------
acados_ocp: AcadosOcp
The current AcadosOcp reference
acados_model: AcadosModel
The current AcadosModel reference
lagrange_costs: SX
The lagrange cost function
mayer_costs: SX
The mayer cost function
y_ref = list[np.ndarray]
The lagrange targets
y_ref_end = list[np.ndarray]
The mayer targets
params = dict
All the parameters to optimize
W: np.ndarray
The Lagrange weights
W_e: np.ndarray
The Mayer weights
status: int
The status of the optimization
all_constr: SX
All the Lagrange constraints
end_constr: SX
All the Mayer constraints
all_g_bounds = Bounds
All the Lagrange bounds on the variables
end_g_bounds = Bounds
All the Mayer bounds on the variables
x_bound_max = np.ndarray
All the bounds max
x_bound_min = np.ndarray
All the bounds min
Vu: np.ndarray
The control objective functions
Vx: np.ndarray
The Lagrange state objective functions
Vxe: np.ndarray
The Mayer state objective functions
Methods
-------
__acados_export_model(self, ocp: OptimalControlProgram)
Creating a generic ACADOS model
__prepare_acados(self, ocp: OptimalControlProgram)
Set some important ACADOS variables
__set_constr_type(self, constr_type: str = "BGH")
Set the type of constraints
__set_constraints(self, ocp: OptimalControlProgram)
Set the constraints from the ocp
__set_cost_type(self, cost_type: str = "NONLINEAR_LS")
Set the type of cost functions
__set_costs(self, ocp: OptimalControlProgram)
Set the cost functions from ocp
__update_solver(self)
Update the ACADOS solver to new values
configure(self, options: dict)
Set some ACADOS options
get_optimized_value(self) -> Union[list[dict], dict]
Get the previously optimized solution
solve(self) -> "AcadosInterface"
Solve the prepared ocp
"""
def __init__(self, ocp, **solver_options):
"""
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
solver_options: dict
The options to pass to the solver
"""
if not isinstance(ocp.cx(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP"
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.nparams = 0
self.params_initial_guess = None
self.params_bounds = None
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
self.all_constr = None
self.end_constr = SX()
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
self.Vu = np.array([],
dtype=np.int64).reshape(0,
ocp.nlp[0].controls.shape)
self.Vx = np.array([], dtype=np.int64).reshape(0,
ocp.nlp[0].states.shape)
self.Vxe = np.array([],
dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
def __acados_export_model(self, ocp):
"""
Creating a generic ACADOS model
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
if ocp.n_phases > 1:
raise NotImplementedError(
"More than 1 phase is not implemented yet with ACADOS backend")
# Declare model variables
x = ocp.nlp[0].X[0]
u = ocp.nlp[0].U[0]
p = ocp.nlp[0].parameters.cx
if ocp.v.parameters_in_list:
for param in ocp.v.parameters_in_list:
if str(param.cx)[:11] == f"time_phase_":
raise RuntimeError(
"Time constraint not implemented yet with Acados.")
self.nparams = ocp.nlp[0].parameters.shape
self.params_initial_guess = ocp.v.parameters_in_list.initial_guess
self.params_initial_guess.check_and_adjust_dimensions(self.nparams, 1)
self.params_bounds = ocp.v.parameters_in_list.bounds
self.params_bounds.check_and_adjust_dimensions(self.nparams, 1)
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * self.nparams,
ocp.nlp[0].dynamics_func(x[self.nparams:, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.con_h_expr = np.zeros((0, 0))
self.acados_model.con_h_expr_e = np.zeros((0, 0))
self.acados_model.p = []
now = datetime.now() # current date and time
self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}"
def __prepare_acados(self, ocp):
"""
Set some important ACADOS variables
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].states.shape + ocp.nlp[
0].parameters.shape
self.acados_ocp.dims.nu = ocp.nlp[0].controls.shape
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type: str = "BGH"):
"""
Set the type of constraints
Parameters
----------
constr_type: str
The requested type of constraints
"""
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
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 in range(ocp.n_phases):
for g, G in enumerate(ocp.nlp[i].g):
if not G:
continue
if G[0]["constraint"].node[0] is Node.ALL:
self.all_constr = vertcat(self.all_constr,
G[0]["val"].reshape((-1, 1)))
self.all_g_bounds.concatenate(G[0]["bounds"])
if len(G) > ocp.nlp[0].ns:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
elif G[0]["constraint"].node[0] is Node.END:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
self.end_g_bounds.concatenate(G[0]["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])
def __set_cost_type(self, cost_type: str = "NONLINEAR_LS"):
"""
Set the type of cost functions
Parameters
----------
cost_type: str
The type of cost function
"""
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
"""
Set the cost functions from ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
if ocp.n_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
ctrl_objs = [
PenaltyType.MINIMIZE_TORQUE,
PenaltyType.MINIMIZE_MUSCLES_CONTROL,
PenaltyType.MINIMIZE_ALL_CONTROLS,
]
state_objs = [
PenaltyType.MINIMIZE_STATE,
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
n_states = ocp.nlp[0].states.shape
n_controls = ocp.nlp[0].controls.shape
self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls)
self.Vx = np.array([], dtype=np.int64).reshape(0, n_states)
self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states)
for i in range(ocp.n_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
if J[0]["objective"].type.value[0] in ctrl_objs:
index = J[0]["objective"].index if J[0][
"objective"].index else list(
np.arange(n_controls))
vu = np.zeros(n_controls)
vu[index] = 1.0
self.Vu = np.vstack((self.Vu, np.diag(vu)))
self.Vx = np.vstack(
(self.Vx, np.zeros((n_controls, n_states))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
n_controls))
if J[0]["target"] is not None:
y_tp = np.zeros((n_controls, 1))
y_ref_tp = []
for J_tp in J:
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append(
[np.zeros((n_controls, 1)) for _ in J])
elif J[0]["objective"].type.value[0] in state_objs:
index = J[0]["objective"].index if J[0][
"objective"].index else list(
np.arange(n_states))
vx = np.zeros(n_states)
vx[index] = 1.0
self.Vx = np.vstack((self.Vx, np.diag(vx)))
self.Vu = np.vstack(
(self.Vu, np.zeros((n_states, n_controls))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] * n_states))
if J[0]["target"] is not None:
y_ref_tp = []
for J_tp in J:
y_tp = np.zeros((n_states, 1))
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append(
[np.zeros((n_states, 1)) for _ in J])
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term with "
f"LINEAR_LS cost type")
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
index = J[0]["objective"].index if J[0][
"objective"].index else list(
np.arange(n_states))
vxe = np.zeros(n_states)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] * n_states))
if J[0]["target"] is not None:
y_tp = np.zeros((n_states, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(np.zeros((n_states, 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
if J[0]["objective"].type.value[0] in state_objs:
index = J[0]["objective"].index if J[0][
"objective"].index else list(
np.arange(n_states))
vxe = np.zeros(n_states)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] * n_states))
if J[0]["target"] is not None:
y_tp = np.zeros((n_states, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(np.zeros((n_states, 1)))
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term "
f"with LINEAR_LS cost type")
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.nparams:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.n_phases):
for j, J in enumerate(ocp.nlp[i].J):
if not J:
continue
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(
J[-1]["target"].T.reshape((-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[-1]["val"].numel(), 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.nparams:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag(([J[0]["objective"].weight] *
J[0]["val"].numel())))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel(
) else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel(
) else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def __update_solver(self):
"""
Update the ACADOS solver to new values
"""
param_init = []
for n in range(self.acados_ocp.dims.N):
if self.y_ref: # Target
self.ocp_solver.cost_set(
n, "yref",
np.vstack([data[n] for data in self.y_ref])[:, 0])
# check following line
# self.ocp_solver.cost_set(n, "W", self.W)
if self.nparams:
param_init = self.params_initial_guess.init.evaluate_at(n)
self.ocp_solver.set(
n, "x",
np.concatenate(
(param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u",
self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu",
self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu",
self.ocp.nlp[0].u_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:,
0])
self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:,
0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
1])
if self.y_ref_end:
if len(self.y_ref_end) == 1:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.array(self.y_ref_end[0])[:, 0])
else:
self.ocp_solver.cost_set(
self.acados_ocp.dims.N, "yref",
np.concatenate([data for data in self.y_ref_end])[:, 0])
# check following line
# self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx",
self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx",
self.x_bound_max[:, -1])
if len(self.end_g_bounds.max[:, 0]):
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh",
self.end_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh",
self.end_g_bounds.min[:, 0])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.nparams:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate((
self.params_initial_guess.init[:, 0],
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N],
)),
)
else:
self.ocp_solver.set(
self.acados_ocp.dims.N, "x",
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def configure(self, options: dict):
"""
Set some ACADOS options
Parameters
----------
options: dict
The dictionary of options
"""
if options is None:
options = {}
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
if self.ocp_solver is None:
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "IRK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
else:
available_options = [
"nlp_solver_tol_comp",
"nlp_solver_tol_eq",
"nlp_solver_tol_ineq",
"nlp_solver_tol_stat",
]
for key in options:
if key in available_options:
short_key = key[11:]
self.ocp_solver.options_set(short_key, options[key])
else:
raise RuntimeError(
f"[ACADOS] Only editable solver options after solver creation are :\n {available_options}"
)
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self) -> Union[list, dict]:
"""
Get the previously optimized solution
Returns
-------
A solution or a list of solution depending on the number of phases
"""
ns = self.acados_ocp.dims.N
n_params = self.ocp.nlp[0].parameters.shape
acados_x = np.array(
[self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:n_params, :]
acados_x = acados_x[n_params:, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
"iter": self.ocp_solver.get_stats("sqp_iter")[0],
"status": self.status,
}
out["x"] = vertcat(out["x"], acados_x.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_u.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_p[:, 0])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self) -> "AcadosInterface":
"""
Solve the prepared ocp
Returns
-------
A reference to the solution
"""
# Populate costs and constraints vectors
self.__set_costs(self.ocp)
self.__set_constraints(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
self.__update_solver()
self.status = self.ocp_solver.solve()
self.get_optimized_value()
return self
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Use use_SX "
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
def __acados_export_model(self, ocp):
# Declare model variables
x = ocp.nlp[0]["X"][0]
u = ocp.nlp[0]["U"][0]
p = ocp.nlp[0]["p"]
mod = ocp.nlp[0]["model"]
x_dot = SX.sym("x_dot", mod.nbQdot() * 2, 1)
f_expl = ocp.nlp[0]["dynamics_func"](x, u, p)
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.p = []
self.acados_model.name = "model_name"
def __prepare_acados(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError(
"more than 1 phase is not implemented yet with ACADOS backend")
if ocp.param_to_optimize:
raise NotImplementedError(
"Parameters optimization is not implemented yet with ACADOS")
# set model
self.acados_ocp.model = self.acados_model
# set dimensions
for i in range(ocp.nb_phases):
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[i]["tf"]
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[i]["nx"]
self.acados_ocp.dims.nu = ocp.nlp[i]["nu"]
self.acados_ocp.dims.ny = self.acados_ocp.dims.nx + self.acados_ocp.dims.nu
self.acados_ocp.dims.ny_e = ocp.nlp[i]["nx"]
self.acados_ocp.dims.N = ocp.nlp[i]["ns"]
for i in range(ocp.nb_phases):
# set constraints
for j in range(ocp.nlp[i]["nx"]):
if ocp.nlp[i]["X_bounds"].min[j, 0] == -np.inf and ocp.nlp[i][
"X_bounds"].max[j, 0] == np.inf:
pass
elif ocp.nlp[i]["X_bounds"].min[
j, 0] != ocp.nlp[i]["X_bounds"].max[j, 0]:
raise RuntimeError(
"Initial constraint on state must be hard. Hint: you can pass it as an objective"
)
else:
self.acados_ocp.constraints.x0 = np.array(
ocp.nlp[i]["X_bounds"].min[:, 0])
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# control constraints
self.acados_ocp.constraints.constr_type = "BGH"
self.acados_ocp.constraints.lbu = np.array(
ocp.nlp[i]["U_bounds"].min[:, 0])
self.acados_ocp.constraints.ubu = np.array(
ocp.nlp[i]["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
# path constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.ubx = np.array(
ocp.nlp[i]["X_bounds"].max[:, 1])
self.acados_ocp.constraints.lbx = np.array(
ocp.nlp[i]["X_bounds"].min[:, 1])
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# terminal constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.ubx_e = np.array(
ocp.nlp[i]["X_bounds"].max[:, -1])
self.acados_ocp.constraints.lbx_e = np.array(
ocp.nlp[i]["X_bounds"].min[:, -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
return self.acados_ocp
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros(
(self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye(
self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros(
(self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
# TODO: Yes the objectives in ocp are not dealt with,
# does that makes sense since we only work with 1 phase ?
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
self.y_ref_end.append(
[np.zeros((J[0]["val"].numel(), 1))])
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny,
1), ))
if len(self.y_ref_end):
self.acados_ocp.cost.yref_e = np.concatenate(
self.y_ref_end, -1).T.squeeze()
else:
self.acados_ocp.cost.yref_e = np.zeros((1, ))
if self.W.shape == (0, 0):
self.acados_ocp.cost.W = np.zeros((1, 1))
else:
self.acados_ocp.cost.W = self.W
if self.W_e.shape == (0, 0):
self.acados_ocp.cost.W_e = np.zeros((1, 1))
else:
self.acados_ocp.cost.W_e = self.W_e
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError(
"TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError(
"TODO: I may have broken this (is this the right J?)"
)
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J["val"]])
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
self.acados_ocp.model.cost_expr_ext_cost = sum1(
self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "ERK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
def get_iterations(self):
raise NotImplementedError(
"return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self, ocp):
acados_x = np.array([
self.ocp_solver.get(i, "x") for i in range(ocp.nlp[0]["ns"] + 1)
]).T
acados_q = acados_x[:ocp.nlp[0]["nu"], :]
acados_qdot = acados_x[ocp.nlp[0]["nu"]:, :]
acados_u = np.array(
[self.ocp_solver.get(i, "u") for i in range(ocp.nlp[0]["ns"])]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
}
for i in range(ocp.nlp[0]["ns"]):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ocp.nlp[0]["ns"]])
out["x"] = vertcat(out["x"], acados_qdot[:, ocp.nlp[0]["ns"]])
return out
def solve(self):
# populate costs vectors
self.__set_costs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
for n in range(self.acados_ocp.dims.N):
self.ocp_solver.cost_set(
n, "yref",
np.concatenate([data[n] for data in self.y_ref])[:, 0])
self.ocp_solver.solve()
return self
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Please set use_SX to True in OCP"
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
def __acados_export_model(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError(
"More than 1 phase is not implemented yet with ACADOS backend")
# Declare model variables
x = ocp.nlp[0].X[0]
u = ocp.nlp[0].U[0]
p = ocp.nlp[0].p
if ocp.nlp[0].parameters_to_optimize:
for n in range(ocp.nb_phases):
for i in range(len(ocp.nlp[0].parameters_to_optimize)):
if str(ocp.nlp[0].p[i]) == f"time_phase_{n}":
raise RuntimeError(
"Time constraint not implemented yet with Acados.")
self.params = ocp.nlp[0].parameters_to_optimize
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * ocp.nlp[0].np,
ocp.nlp[0].dynamics_func(x[ocp.nlp[0].np:, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.con_h_expr = np.zeros((0, 0))
self.acados_model.con_h_expr_e = np.zeros((0, 0))
self.acados_model.p = []
now = datetime.now() # current date and time
self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}"
def __prepare_acados(self, ocp):
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].nx + ocp.nlp[0].np
self.acados_ocp.dims.nu = ocp.nlp[0].nu
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type="BGH"):
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
def __set_constrs(self, ocp):
# constraints handling in self.acados_ocp
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 in range(ocp.nb_phases):
for g, G in enumerate(ocp.nlp[i].g):
if not G:
continue
if G[0]["constraint"].node[0] is Node.ALL:
self.all_constr = vertcat(self.all_constr,
G[0]["val"].reshape((-1, 1)))
self.all_g_bounds.concatenate(G[0]["bounds"])
if len(G) > ocp.nlp[0].ns:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
elif G[0]["constraint"].node[0] is Node.END:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
self.end_g_bounds.concatenate(G[0]["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
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.params:
param_bounds_max = np.concatenate(
[self.params[key].bounds.max for key in self.params.keys()])[:,
0]
param_bounds_min = np.concatenate(
[self.params[key].bounds.min for key in self.params.keys()])[:,
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])
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
ctrl_objs = [
PenaltyType.MINIMIZE_TORQUE,
PenaltyType.MINIMIZE_MUSCLES_CONTROL,
PenaltyType.MINIMIZE_ALL_CONTROLS,
]
state_objs = [
PenaltyType.MINIMIZE_STATE,
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nu)
self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
if J[0]["objective"].type.value[0] in ctrl_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nu)))
vu = np.zeros(ocp.nlp[0].nu)
vu[index] = 1.0
self.Vu = np.vstack((self.Vu, np.diag(vu)))
self.Vx = np.vstack(
(self.Vx,
np.zeros((ocp.nlp[0].nu, ocp.nlp[0].nx))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nu))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nu, 1))
y_ref_tp = []
for J_tp in J:
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append([
np.zeros((ocp.nlp[0].nu, 1)) for J_tp in J
])
elif J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vx = np.zeros(ocp.nlp[0].nx)
vx[index] = 1.0
self.Vx = np.vstack((self.Vx, np.diag(vx)))
self.Vu = np.vstack(
(self.Vu,
np.zeros((ocp.nlp[0].nx, ocp.nlp[0].nu))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_ref_tp = []
for J_tp in J:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append([
np.zeros((ocp.nlp[0].nx, 1)) for J_tp in J
])
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term with "
f"LINEAR_LS cost type")
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
if J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term "
f"with LINEAR_LS cost type")
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.params:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(
J[-1]["target"].T.reshape((-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[-1]["val"].numel(), 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag(([J[0]["objective"].weight] *
J[0]["val"].numel())))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel(
) else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel(
) else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def __update_solver(self):
param_init = []
for n in range(self.acados_ocp.dims.N):
if self.y_ref:
self.ocp_solver.cost_set(
n, "yref",
np.vstack([data[n] for data in self.y_ref])[:, 0])
# check following line
# self.ocp_solver.cost_set(n, "W", self.W)
if self.params:
param_init = np.concatenate([
self.params[key].initial_guess.init.evaluate_at(n)
for key in self.params.keys()
])
self.ocp_solver.set(
n, "x",
np.concatenate(
(param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u",
self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu",
self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu",
self.ocp.nlp[0].u_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:,
0])
self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:,
0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
1])
if self.y_ref_end:
if len(self.y_ref_end) == 1:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.array(self.y_ref_end[0])[:, 0])
else:
self.ocp_solver.cost_set(
self.acados_ocp.dims.N, "yref",
np.concatenate([data for data in self.y_ref_end])[:, 0])
# check following line
# self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx",
self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx",
self.x_bound_max[:, -1])
if len(self.end_g_bounds.max[:, 0]):
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh",
self.end_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh",
self.end_g_bounds.min[:, 0])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.params:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate((
np.concatenate([
self.params[key]["initial_guess"].init
for key in self.params.keys()
])[:, 0],
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N],
)),
)
else:
self.ocp_solver.set(
self.acados_ocp.dims.N, "x",
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
if self.ocp_solver is None:
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "IRK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
else:
available_options = [
"nlp_solver_tol_comp",
"nlp_solver_tol_eq",
"nlp_solver_tol_ineq",
"nlp_solver_tol_stat",
]
for key in options:
if key in available_options:
short_key = key[11:]
self.ocp_solver.options_set(short_key, options[key])
else:
raise RuntimeError(
f"[ACADOS] Only editable solver options after solver creation are :\n {available_options}"
)
def get_iterations(self):
raise NotImplementedError(
"return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self):
ns = self.acados_ocp.dims.N
nx = self.acados_ocp.dims.nx
nq = self.ocp.nlp[0].q.shape[0]
nparams = self.ocp.nlp[0].np
acados_x = np.array(
[self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:nparams, :]
acados_q = acados_x[nparams:nq + nparams, :]
acados_qdot = acados_x[nq + nparams:nx, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
"iter": self.ocp_solver.get_stats("sqp_iter")[0],
"status": self.status,
}
for i in range(ns):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ns])
out["x"] = vertcat(out["x"], acados_qdot[:, ns])
out["x"] = vertcat(acados_p[:, 0], out["x"])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self):
# Populate costs and constrs vectors
self.__set_costs(self.ocp)
self.__set_constrs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
self.__update_solver()
self.status = self.ocp_solver.solve()
self.get_optimized_value()
return self
class AcadosInterface(SolverInterface):
"""
The ACADOS solver interface
Attributes
----------
acados_ocp: AcadosOcp
The current AcadosOcp reference
acados_model: AcadosModel
The current AcadosModel reference
lagrange_costs: SX
The lagrange cost function
mayer_costs: SX
The mayer cost function
y_ref = list[np.ndarray]
The lagrange targets
y_ref_end = list[np.ndarray]
The mayer targets
params = dict
All the parameters to optimize
W: np.ndarray
The Lagrange weights
W_e: np.ndarray
The Mayer weights
status: int
The status of the optimization
all_constr: SX
All the Lagrange constraints
end_constr: SX
All the Mayer constraints
all_g_bounds = Bounds
All the Lagrange bounds on the variables
end_g_bounds = Bounds
All the Mayer bounds on the variables
x_bound_max = np.ndarray
All the bounds max
x_bound_min = np.ndarray
All the bounds min
Vu: np.ndarray
The control objective functions
Vx: np.ndarray
The Lagrange state objective functions
Vxe: np.ndarray
The Mayer state objective functions
opts: ACADOS
Options of Acados from ACADOS
Methods
-------
__acados_export_model(self, ocp: OptimalControlProgram)
Creating a generic ACADOS model
__prepare_acados(self, ocp: OptimalControlProgram)
Set some important ACADOS variables
__set_constr_type(self, constr_type: str = "BGH")
Set the type of constraints
__set_constraints(self, ocp: OptimalControlProgram)
Set the constraints from the ocp
__set_cost_type(self, cost_type: str = "NONLINEAR_LS")
Set the type of cost functions
__set_costs(self, ocp: OptimalControlProgram)
Set the cost functions from ocp
__update_solver(self)
Update the ACADOS solver to new values
get_optimized_value(self) -> Union[list[dict], dict]
Get the previously optimized solution
solve(self) -> "AcadosInterface"
Solve the prepared ocp
"""
def __init__(self, ocp, solver_options: Solver.ACADOS = None):
"""
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
solver_options: ACADOS
The options to pass to the solver
"""
if not isinstance(ocp.cx(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP"
)
super().__init__(ocp)
# solver_options = solver_options.__dict__
if solver_options is None:
solver_options = Solver.ACADOS()
self.acados_ocp = AcadosOcp(acados_path=solver_options.acados_dir)
self.acados_model = AcadosModel()
self.__set_cost_type(solver_options.cost_type)
self.__set_constr_type(solver_options.constr_type)
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.nparams = 0
self.params_initial_guess = None
self.params_bounds = None
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
self.real_time_to_optimize = -1
self.all_constr = None
self.end_constr = SX()
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
self.Vu = np.array([],
dtype=np.int64).reshape(0,
ocp.nlp[0].controls.shape)
self.Vx = np.array([], dtype=np.int64).reshape(0,
ocp.nlp[0].states.shape)
self.Vxe = np.array([],
dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
self.opts = Solver.ACADOS(
) if solver_options is None else solver_options
def __acados_export_model(self, ocp):
"""
Creating a generic ACADOS model
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
if ocp.n_phases > 1:
raise NotImplementedError(
"More than 1 phase is not implemented yet with ACADOS backend")
# Declare model variables
x = ocp.nlp[0].states.cx
u = ocp.nlp[0].controls.cx
p = ocp.nlp[0].parameters.cx
if ocp.v.parameters_in_list:
for param in ocp.v.parameters_in_list:
if str(param.cx)[:11] == f"time_phase_":
raise RuntimeError(
"Time constraint not implemented yet with Acados.")
self.nparams = ocp.nlp[0].parameters.shape
self.params_initial_guess = ocp.v.parameters_in_list.initial_guess
self.params_initial_guess.check_and_adjust_dimensions(self.nparams, 1)
self.params_bounds = ocp.v.parameters_in_list.bounds
self.params_bounds.check_and_adjust_dimensions(self.nparams, 1)
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * self.nparams,
ocp.nlp[0].dynamics_func(x[self.nparams:, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.con_h_expr = np.zeros((0, 0))
self.acados_model.con_h_expr_e = np.zeros((0, 0))
self.acados_model.p = []
now = datetime.now() # current date and time
self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}"
def __prepare_acados(self, ocp):
"""
Set some important ACADOS variables
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].states.shape + ocp.nlp[
0].parameters.shape
self.acados_ocp.dims.nu = ocp.nlp[0].controls.shape
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type: str = "BGH"):
"""
Set the type of constraints
Parameters
----------
constr_type: str
The requested type of constraints
"""
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
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])
def __set_cost_type(self, cost_type: str = "NONLINEAR_LS"):
"""
Set the type of cost functions
Parameters
----------
cost_type: str
The type of cost function
"""
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
"""
Set the cost functions from ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
def add_linear_ls_lagrange(acados, objectives):
def add_objective(n_variables, is_state):
v_var = np.zeros(n_variables)
var_type = acados.ocp.nlp[
0].states if is_state else acados.ocp.nlp[0].controls
rows = objectives.rows + var_type[
objectives.params["key"]].index[0]
v_var[rows] = 1.0
if is_state:
acados.Vx = np.vstack((acados.Vx, np.diag(v_var)))
acados.Vu = np.vstack(
(acados.Vu, np.zeros((n_states, n_controls))))
else:
acados.Vx = np.vstack(
(acados.Vx, np.zeros((n_controls, n_states))))
acados.Vu = np.vstack((acados.Vu, np.diag(v_var)))
acados.W = linalg.block_diag(
acados.W, np.diag([objectives.weight] * n_variables))
node_idx = objectives.node_idx[:-1] if objectives.node[
0] == Node.ALL else objectives.node_idx
y_ref = [
np.zeros((n_states if is_state else n_controls, 1))
for _ in node_idx
]
if objectives.target is not None:
for idx in node_idx:
y_ref[idx][rows] = objectives.target[...,
idx].T.reshape(
(-1, 1))
acados.y_ref.append(y_ref)
if objectives.type in allowed_control_objectives:
add_objective(n_controls, False)
elif objectives.type in allowed_state_objectives:
add_objective(n_states, True)
else:
raise RuntimeError(
f"{objectives[0]['objective'].type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_linear_ls_mayer(acados, objectives):
if objectives.type in allowed_state_objectives:
vxe = np.zeros(n_states)
rows = objectives.rows + acados.ocp.nlp[0].states[
objectives.params["key"]].index[0]
vxe[rows] = 1.0
acados.Vxe = np.vstack((acados.Vxe, np.diag(vxe)))
acados.W_e = linalg.block_diag(
acados.W_e, np.diag([objectives.weight] * n_states))
y_ref_end = np.zeros((n_states, 1))
if objectives.target is not None:
y_ref_end[rows] = objectives.target[..., -1].T.reshape(
(-1, 1))
acados.y_ref_end.append(y_ref_end)
else:
raise RuntimeError(
f"{objectives.type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_nonlinear_ls_lagrange(acados, objectives, x, u, p):
acados.lagrange_costs = vertcat(
acados.lagrange_costs,
objectives.function(x, u, p).reshape((-1, 1)))
acados.W = linalg.block_diag(
acados.W,
np.diag([objectives.weight] * objectives.function.numel_out()))
node_idx = objectives.node_idx[:-1] if objectives.node[
0] == Node.ALL else objectives.node_idx
if objectives.target is not None:
acados.y_ref.append([
objectives.target[..., idx].T.reshape((-1, 1))
for idx in node_idx
])
else:
acados.y_ref.append([
np.zeros((objectives.function.numel_out(), 1))
for _ in node_idx
])
def add_nonlinear_ls_mayer(acados, objectives, x, u, p):
acados.W_e = linalg.block_diag(
acados.W_e,
np.diag([objectives.weight] * objectives.function.numel_out()))
x = x if objectives.function.sparsity_in("i0").shape != (
0, 0) else []
u = u if objectives.function.sparsity_in("i1").shape != (
0, 0) else []
acados.mayer_costs = vertcat(
acados.mayer_costs,
objectives.function(x, u, p).reshape((-1, 1)))
if objectives.target is not None:
acados.y_ref_end.append(objectives.target[..., -1].T.reshape(
(-1, 1)))
else:
acados.y_ref_end.append(
np.zeros((objectives.function.numel_out(), 1)))
if ocp.n_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
allowed_control_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_CONTROL]
allowed_state_objectives = [
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
ObjectiveFcn.Mayer.TRACK_STATE
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
n_states = ocp.nlp[0].states.shape
n_controls = ocp.nlp[0].controls.shape
self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls)
self.Vx = np.array([], dtype=np.int64).reshape(0, n_states)
self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states)
for i in range(ocp.n_phases):
for J in ocp.nlp[i].J:
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_linear_ls_lagrange(self, J)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_linear_ls_mayer(self, J)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_linear_ls_mayer(self, J)
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.nparams:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i, nlp in enumerate(ocp.nlp):
for j, J in enumerate(nlp.J):
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_nonlinear_ls_lagrange(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.objectives, for now.
if self.nparams:
nlp = ocp.nlp[0] # Assume 1 phase
for j, J in enumerate(ocp.J):
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx, nlp.parameters.cx)
# Set costs
self.acados_ocp.model.cost_y_expr = (self.lagrange_costs.reshape(
(-1, 1)) if self.lagrange_costs.numel() else SX(1, 1))
self.acados_ocp.model.cost_y_expr_e = (self.mayer_costs.reshape(
(-1, 1)) if self.mayer_costs.numel() else SX(1, 1))
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def __update_solver(self):
"""
Update the ACADOS solver to new values
"""
param_init = []
for n in range(self.acados_ocp.dims.N):
if self.y_ref: # Target
self.ocp_solver.cost_set(
n, "yref",
np.vstack([data[n] for data in self.y_ref])[:, 0])
# check following line
# self.ocp_solver.cost_set(n, "W", self.W)
if self.nparams:
param_init = self.params_initial_guess.init.evaluate_at(n)
self.ocp_solver.set(
n, "x",
np.concatenate(
(param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u",
self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu",
self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu",
self.ocp.nlp[0].u_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:,
0])
self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:,
0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
1])
if self.y_ref_end:
if len(self.y_ref_end) == 1:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.array(self.y_ref_end[0])[:, 0])
else:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.concatenate(self.y_ref_end)[:, 0])
# check following line
# self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx",
self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx",
self.x_bound_max[:, -1])
if len(self.end_g_bounds.max[:, 0]):
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh",
self.end_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh",
self.end_g_bounds.min[:, 0])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.nparams:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate(
(self.params_initial_guess.init[:, 0],
self.ocp.nlp[0].x_init.init[:,
self.acados_ocp.dims.N])),
)
else:
self.ocp_solver.set(
self.acados_ocp.dims.N, "x",
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self) -> Union[list, dict]:
"""
Get the previously optimized solution
Returns
-------
A solution or a list of solution depending on the number of phases
"""
ns = self.acados_ocp.dims.N
n_params = self.ocp.nlp[0].parameters.shape
acados_x = np.array(
[self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:n_params, :]
acados_x = acados_x[n_params:, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"x": [],
"u": acados_u,
"solver_time_to_optimize":
self.ocp_solver.get_stats("time_tot")[0],
"real_time_to_optimize": self.real_time_to_optimize,
"iter": self.ocp_solver.get_stats("sqp_iter")[0],
"status": self.status,
"solver": SolverType.ACADOS,
}
out["x"] = vertcat(out["x"], acados_x.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_u.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_p[:, 0])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self) -> Union[list, dict]:
"""
Solve the prepared ocp
Returns
-------
A reference to the solution
"""
tic = perf_counter()
# Populate costs and constraints vectors
self.__set_costs(self.ocp)
self.__set_constraints(self.ocp)
options = self.opts.as_dict(self)
if self.ocp_solver is None:
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
self.opts.set_only_first_options_has_changed(False)
self.opts.set_has_tolerance_changed(False)
else:
if self.opts.only_first_options_has_changed:
raise RuntimeError(
"Some options has been changed the second time acados was run.",
"Only " + str(Solver.ACADOS.get_tolerance_keys()) +
" can be modified.",
)
if self.opts.has_tolerance_changed:
for key in self.opts.get_tolerance_keys():
short_key = key[12:]
self.ocp_solver.options_set(short_key, options[key[1:]])
self.opts.set_has_tolerance_changed(False)
self.__update_solver()
self.status = self.ocp_solver.solve()
self.real_time_to_optimize = perf_counter() - tic
return self.get_optimized_value()
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError("CasADi graph must be SX to be solved with ACADOS. Please set use_SX to True in OCP")
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
def __acados_export_model(self, ocp):
# Declare model variables
x = ocp.nlp[0].X[0]
u = ocp.nlp[0].U[0]
p = ocp.nlp[0].p
self.params = ocp.nlp[0].parameters_to_optimize
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * ocp.nlp[0].np, ocp.nlp[0].dynamics_func(x[ocp.nlp[0].np :, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.p = []
self.acados_model.name = "model_name"
def __prepare_acados(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError("More than 1 phase is not implemented yet with ACADOS backend")
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].nx + ocp.nlp[0].np
self.acados_ocp.dims.nu = ocp.nlp[0].nu
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type="BGH"):
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
def __set_constrs(self, ocp):
# constraints handling in self.acados_ocp
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)
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."
)
## TODO: implement constraints in g
# setup state constraints
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.params:
param_bounds_max = np.concatenate([self.params[key]["bounds"].max for key in self.params.keys()])[:, 0]
param_bounds_min = np.concatenate([self.params[key]["bounds"].min for key in self.params.keys()])[:, 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
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError("ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
raise RuntimeError("LINEAR_LS is not interfaced yet.")
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
self.y_ref.append([np.zeros((J_tp["val"].numel(), 1)) for J_tp in J])
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel()))
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel() else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel() else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
# Set weight
self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],))
self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError("External is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def __update_solver(self):
param_init = []
for n in range(self.acados_ocp.dims.N):
self.ocp_solver.cost_set(n, "yref", np.concatenate([data[n] for data in self.y_ref])[:, 0])
self.ocp_solver.cost_set(n, "W", self.W)
if self.params:
param_init = np.concatenate(
[self.params[key]["initial_guess"].init.evaluate_at(n) for key in self.params.keys()]
)
self.ocp_solver.set(n, "x", np.concatenate((param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u", self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu", self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu", self.ocp.nlp[0].u_bounds.max[:, 0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 1])
if self.y_ref_end:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.concatenate([data for data in self.y_ref_end]))
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx", self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx", self.x_bound_max[:, -1])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.params:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate(
(
np.concatenate([self.params[key]["initial_guess"].init for key in self.params.keys()])[
:, 0
],
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N],
)
),
)
else:
self.ocp_solver.set(self.acados_ocp.dims.N, "x", self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
if self.ocp_solver is None:
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "IRK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
else:
for key in options:
if key[:11] == "nlp_solver_":
short_key = key[11:]
self.ocp_solver.options_set(short_key, options[key])
else:
raise RuntimeError(
"[ACADOS] Only editable solver options after solver creation are :\n"
"nlp_solver_tol_comp\n"
"nlp_solver_tol_eq\n"
"nlp_solver_tol_ineq\n"
"nlp_solver_tol_stat\n"
)
def get_iterations(self):
raise NotImplementedError("return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError("online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self):
ns = self.acados_ocp.dims.N
nx = self.acados_ocp.dims.nx
nq = self.ocp.nlp[0].q.shape[0]
nparams = self.ocp.nlp[0].np
acados_x = np.array([self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:nparams, :]
acados_q = acados_x[nparams : nq + nparams, :]
acados_qdot = acados_x[nq + nparams : nx, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
"status": self.status,
}
for i in range(ns):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ns])
out["x"] = vertcat(out["x"], acados_qdot[:, ns])
out["x"] = vertcat(acados_p[:, 0], out["x"])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self):
# Populate costs and constrs vectors
self.__set_costs(self.ocp)
self.__set_constrs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json")
self.__update_solver()
self.status = self.ocp_solver.solve()
self.get_optimized_value()
return self
