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
tau = -Kp_theta * (target_position[2] - x_noise[2]) - Kd_theta * (
target_position[5] - x_noise[5]) - Kp_phi * (
target_position[0] - x_noise[0]) - Kd_phi * (target_position[3] -
x_noise[3])
f = mb * g * np.cos(
x_noise[2]) + Kp_l * (target_position[1] - x_noise[1]) + Kd_l * (
target_position[4] - x_noise[4])
# update reference
for j in range(N):
acados_solver.set(j, "p", np.array([tau, f]))
# solve ocp
t = time.time()
status = acados_solver.solve()
if status != 0:
raise Exception(
"acados returned status {} in closed loop iteration {}.".format(
status, i))
elapsed = time.time() - t
time_iterations[i] = elapsed
# manage timings
tcomp_sum += elapsed
if elapsed > tcomp_max:
tcomp_max = elapsed
# get solution
x0 = acados_solver.get(0, "x")
def main(interface_type='ctypes'):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
# define the different options for the use-case demonstration
N0 = 20 # original number of shooting nodes
N12 = 15 # change the number of shooting nodes for use-cases 1 and 2
condN12 = max(1, round(N12/1)) # change the number of cond_N for use-cases 1 and 2 (for PARTIAL_* solvers only)
Tf_01 = 1.0 # original final time and for use-case 1
Tf_2 = Tf_01 * 0.7 # change final time for use-case 2 (but keep N identical)
# set dimensions
ocp.dims.N = N0
# set cost
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W_e = Q
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4, 0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.zeros((ny,))
ocp.cost.yref_e = np.zeros((ny_e,))
# set constraints
Fmax = 80
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf_01
print(80*'-')
print('generate code and compile...')
if interface_type == 'cython':
AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json')
AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json')
elif interface_type == 'ctypes':
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
elif interface_type == 'cython_prebuilt':
from c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
ocp_solver = AcadosOcpSolverCython(ocp.model.name, ocp.solver_options.nlp_solver_type, ocp.dims.N)
# test setting HPIPM options
ocp_solver.options_set('qp_tol_ineq', 1e-8)
ocp_solver.options_set('qp_tau_min', 1e-10)
ocp_solver.options_set('qp_mu0', 1e0)
# --------------------------------------------------------------------------------
# 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py'
nvariant = 0
simX0 = np.ndarray((N0 + 1, nx))
simU0 = np.ndarray((N0, nu))
print(80*'-')
print(f'solve original code with N = {N0} and Tf = {Tf_01} s:')
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
raise Exception(f'acados returned status {status}.')
# get solution
for i in range(N0):
simX0[i, :] = ocp_solver.get(i, "x")
simU0[i, :] = ocp_solver.get(i, "u")
simX0[N0, :] = ocp_solver.get(N0, "x")
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True)
if PLOT:# plot but don't halt
plot_pendulum(np.linspace(0, Tf_01, N0 + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N0}, Tf={Tf_01}')
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()
j, "y_ref",
nmp.array([
-1, -1, 0, 0.770, -0.421, -0.421, -0.230, 0, 0, 0, uSS,
uSS, uSS, uSS
]))
elif tCurr <= 6:
# roll = -1 pitch = 1 yaw = 0
# q = 0.770 -0.421 0.421 0.230
acados_ocp_solver.set(
j, "y_ref",
nmp.array([
-1, 1, 0, 0.770, -0.421, 0.421, 0.230, 0, 0, 0, uSS, uSS,
uSS, uSS
]))
status = acados_ocp_solver.solve()
if status != 0:
raise Exception(
'acados acados_ocp_solver returned status {}. Exiting.'.format(
status))
simU[i, :] = acados_ocp_solver.get(0, "u")
acados_integrator.set("x", x0)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados integrator returned status {}. Exiting.'.format(status))
# update state
x0 = acados_integrator.get("x")
class QuadOptimizer:
def __init__(self,
quad_model,
quad_constraints,
t_horizon,
n_nodes,
sim_required=False,
max_hight=1.0):
self.model = quad_model
self.constraints = quad_constraints
self.g = 9.8066
self.T = t_horizon
self.N = n_nodes
self.simulation_required = sim_required
robot_name_ = rospy.get_param("~robot_name", "bebop1_r")
self.current_pose = None
self.current_state = np.zeros((13, 1))
self.dt = 0.02
self.rate = rospy.Rate(1 / self.dt)
self.time_stamp = None
self.trajectory_path = None
self.current_twist = np.zeros(3)
self.att_command = AttitudeTarget()
self.att_command.type_mask = 3
self.pendulum_state = np.zeros(4)
# subscribers
# the robot state
robot_state_sub_ = rospy.Subscriber('/robot_pose', Odometry,
self.robot_state_callback)
# pendulum state
pendulum_state_sub_ = rospy.Subscriber('/pendulum_pose', Odometry,
self.pendulum_state_callback)
# trajectory
robot_trajectory_sub_ = rospy.Subscriber(
'/robot_trajectory', itm_trajectory_msg,
self.trajectory_command_callback)
# publisher
self.att_setpoint_pub = rospy.Publisher(
'/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
# create a server
server_ = rospy.Service('uav_mpc_server', SetBool, self.state_server)
# setup optimizer
self.quadrotor_optimizer_setup()
# # It seems that thread cannot ensure the performance of the time
self.att_thread = Thread(target=self.send_command, args=())
self.att_thread.daemon = True
self.att_thread.start()
def robot_state_callback(self, data):
# robot state as [x, y, z, vx, vy, vz, roll, pitch, yaw]
roll, pitch, yaw = self.quaternion_to_rpy(data.pose.pose.orientation)
# self.current_state[:6] = np.array([data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z,
# data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z]).reshape(6,1)
# self.current_state[-3:] = np.array([roll, pitch, yaw]).reshape(3,1)
self.current_state = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z, data.twist.twist.linear.x,
data.twist.twist.linear.y, data.twist.twist.linear.z,
data.pose.pose.position.x, data.pose.pose.position.y, 0., 0., roll,
pitch, yaw
],
dtype=np.float64)
def pendulum_state_callback(self, data):
# pendulum state as [x, y, z, vx, vy, vz, s, r, ds, dr, roll, pitch, yaw]
s, r = data.pose.pose.position.x, data.pose.pose.position.y
ds, dr = data.twist.twist.linear.x, data.twist.twist.linear.y
# self.current_state[7:10] = np.array([s, r, ds, dr]).reshape(4, 1)
self.pendulum_state = np.array([s, r, ds, dr]) # .reshape(4, 1)
# self.current_state_pendulum = np.array([self.current_state[0], self.current_state[1], self.current_state[2],
# self.current_state[3], self.current_state[4], self.current_state[5],
# s, r, ds, dr,
# self.current_state[10], self.current_state[11], self.current_state[12]], dtype=np.float64)
def trajectory_command_callback(self, data):
temp_traj = data.traj
if data.size != len(temp_traj):
rospy.logerr('Some data are lost')
else:
self.trajectory_path = np.zeros((data.size, 13))
for i in range(data.size):
self.trajectory_path[i] = np.array([
temp_traj[i].x,
temp_traj[i].y,
temp_traj[i].z,
temp_traj[i].vx,
temp_traj[i].vy,
temp_traj[i].vz,
temp_traj[i].load_x,
temp_traj[i].load_y,
temp_traj[i].load_vx,
temp_traj[i].load_vy,
temp_traj[i].roll,
temp_traj[i].pitch,
temp_traj[i].yaw,
])
def quadrotor_optimizer_setup(self, ):
# Q_m_ = np.diag([80.0, 80.0, 120.0, 20.0, 20.0,
# 30.0, 10.0, 10.0, 0.0]) # position, velocity, roll, pitch, (not yaw)
# P_m_ = np.diag([86.21, 86.21, 120.95,
# 6.94, 6.94, 11.04]) # only p and v
# P_m_[0, 3] = 6.45
# P_m_[3, 0] = 6.45
# P_m_[1, 4] = 6.45
# P_m_[4, 1] = 6.45
# P_m_[2, 5] = 10.95
# P_m_[5, 2] = 10.95
# R_m_ = np.diag([50.0, 60.0, 1.0])
Q_m_ = np.diag(
[
10.0,
10.0,
10.0,
3e-1,
3e-1,
3e-1,
#3e-1, 3e-1, 3e-2, 3e-2,
100.0,
100.0,
1e-3,
1e-3,
10.5,
10.5,
10.5
]
) # position, velocity, load_position, load_velocity, [roll, pitch, yaw]
P_m_ = np.diag([
10.0, 10.0, 10.0, 0.05, 0.05, 0.05
# 10.0, 10.0, 10.0,
# 0.05, 0.05, 0.05
]) # only p and v
# P_m_[0, 8] = 6.45
# P_m_[8, 0] = 6.45
# P_m_[1, 9] = 6.45
# P_m_[9, 1] = 6.45
# P_m_[2, 10] = 10.95
# P_m_[10, 2] = 10.95
R_m_ = np.diag([3.0, 3.0, 3.0, 1.0])
nx = self.model.x.size()[0]
self.nx = nx
nu = self.model.u.size()[0]
self.nu = nu
ny = nx + nu
n_params = self.model.p.size()[0] if isinstance(self.model.p,
ca.SX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, acados_source_path)
# create OCP
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = self.model
ocp.dims.N = self.N
ocp.solver_options.tf = self.T
# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)
# cost type
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_)
ocp.cost.W_e = P_m_ # np.zeros((nx-3, nx-3))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
ocp.cost.Vx_e = np.zeros((nx - 7, nx)) # only consider p and v
ocp.cost.Vx_e[:nx - 7, :nx - 7] = np.eye(nx - 7)
# initial reference trajectory_ref
x_ref = np.zeros(nx)
x_ref_e = np.zeros(nx - 7)
u_ref = np.zeros(nu)
u_ref[-1] = self.g
ocp.cost.yref = np.concatenate((x_ref, u_ref))
ocp.cost.yref_e = x_ref_e
# Set constraints
ocp.constraints.lbu = np.array([
self.constraints.roll_min, self.constraints.pitch_min,
self.constraints.yaw_min, self.constraints.thrust_min
])
ocp.constraints.ubu = np.array([
self.constraints.roll_max, self.constraints.pitch_max,
self.constraints.yaw_max, self.constraints.thrust_max
])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# initial state
ocp.constraints.x0 = x_ref
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# explicit Runge-Kutta integrator
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI'
ocp.solver_options.levenberg_marquardt = 0.12 # 0.0
# compile acados ocp
json_file = os.path.join('./' + self.model.name + '_acados_ocp.json')
self.solver = AcadosOcpSolver(ocp, json_file=json_file)
if self.simulation_required:
self.integrator = AcadosSimSolver(ocp, json_file=json_file)
def mpc_estimation_loop(self, mpc_iter):
if self.trajectory_path is not None and self.current_state is not None:
t1 = time.time()
# dt = 0.1
current_trajectory = self.trajectory_path
u_des = np.array([0.0, 0.0, 0.0, self.g])
new_state = self.current_state
# new_state[6:10] = self.pendulum_state
new_state[6] = self.pendulum_state[0] - self.current_state[0]
new_state[7] = self.pendulum_state[1] - self.current_state[1]
new_state[8] = self.pendulum_state[2] - self.current_state[3]
new_state[9] = self.pendulum_state[3] - self.current_state[4]
simX[mpc_iter] = new_state # mpc_iter+1 ?
simD[mpc_iter] = current_trajectory[0]
l = 0.1
#print(np.rad2deg(np.arcsin(new_state[6]/l)))
# print()
# print("current state: ")
# np.set_printoptions(suppress=True)
# print(new_state)
# print()
# print("current trajectory: ")
# print(current_trajectory[-1])
tra = current_trajectory[0]
# error_xyz = np.linalg.norm(np.array([new_state[0] - tra[0], new_state[1] - tra[1], new_state[2] - tra[2]]))
# print(error_xyz)
self.solver.set(self.N, 'yref', current_trajectory[-1, :6])
for i in range(self.N):
self.solver.set(
i, 'yref',
np.concatenate([current_trajectory[i].flatten(), u_des]))
# self.solver.set(0, 'lbx', self.current_state)
# self.solver.set(0, 'ubx', self.current_state)
self.solver.set(0, 'lbx', new_state)
self.solver.set(0, 'ubx', new_state)
status = self.solver.solve()
tarr[mpc_iter] = time.time() - t1
if status != 0:
rospy.logerr("MPC cannot find a proper solution.")
# self.solver.print_statistics()
print()
print("current state: ")
np.set_printoptions(suppress=True)
print(new_state)
print()
print("current trajectory: ")
print(current_trajectory[0])
self.att_command.orientation = Quaternion(
*self.rpy_to_quaternion(0.0, 0.0, 0.0, w_first=False))
#self.att_command.thrust = 0.5
self.att_command.thrust = 0.62
self.att_command.body_rate.z = 0.0
else:
# print()
# print("predicted trajectory:")
# for i in range(self.N):
# print(self.solver.get(i, 'x'))
mpc_u_ = self.solver.get(0, 'u')
simU[mpc_iter] = mpc_u_
quat_local_ = self.rpy_to_quaternion(mpc_u_[0],
mpc_u_[1],
mpc_u_[2],
w_first=False)
self.att_command.orientation.x = quat_local_[0]
self.att_command.orientation.y = quat_local_[1]
self.att_command.orientation.z = quat_local_[2]
self.att_command.orientation.w = quat_local_[3]
# print(self.att_command.orientation)
#self.att_command.thrust = mpc_u_[3]/9.8066 - 0.5
# self.att_command.thrust = mpc_u_[3]/9.8066 - 0.38
self.att_command.thrust = mpc_u_[3] / 9.8066 * 0.6175
# print()
# print("mpc_u: ")
# print(mpc_u_)
# print()
# print("---------------------------------------")
# print(self.att_command.thrust)
# yaw_command_ = self.yaw_command(current_yaw_, trajectory_path_[1, -1], 0.0)
# yaw_command_ = self.yaw_controller(trajectory_path_[1, -1]-current_yaw_)
# self.att_command.angular.z = yaw_command_
#self.att_command.body_rate.z = 0.0
# self.att_setpoint_pub.publish(self.att_command)
# print("time: ")
# print(time.time()-time_1)
else:
if self.trajectory_path is None:
rospy.loginfo("waiting trajectory")
elif self.current_state is None:
rospy.loginfo("waiting current state")
else:
rospy.loginfo("Unknown error")
self.rate.sleep()
return True
@staticmethod
def quaternion_to_rpy(quaternion):
q0, q1, q2, q3 = quaternion.w, quaternion.x, quaternion.y, quaternion.z
roll_ = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
pitch_ = np.arcsin(2 * (q0 * q2 - q3 * q1))
yaw_ = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
return roll_, pitch_, yaw_
@staticmethod
def state_server(req):
return SetBoolResponse(True, 'MPC is ready')
@staticmethod
def rpy_to_quaternion(r, p, y, w_first=True):
cy = np.cos(y * 0.5)
sy = np.sin(y * 0.5)
cp = np.cos(p * 0.5)
sp = np.sin(p * 0.5)
cr = np.cos(r * 0.5)
sr = np.sin(r * 0.5)
qw = cr * cp * cy + sr * sp * sy
qx = sr * cp * cy - cr * sp * sy
qy = cr * sp * cy + sr * cp * sy
qz = cr * cp * sy - sr * sp * cy
if w_first:
return np.array([qw, qx, qy, qz])
else:
return np.array([qx, qy, qz, qw])
def send_command(self, ):
rate = rospy.Rate(100) # Hz
self.att_command.header = Header()
while not rospy.is_shutdown():
# t2 = time.time()
command_ = self.att_command
# self.att_command.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(command_)
try: # prevent garbage in console output when thread is killed
rate.sleep()
except rospy.ROSInterruptException:
pass
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
def main(discretization='shooting_nodes'):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
integrator_type = 'LIFTED_IRK' # ERK, IRK, GNSF, LIFTED_IRK
if integrator_type == 'GNSF':
acados_dae_model_json_dump(model)
# structure detection in Matlab/Octave -> produces 'pendulum_ode_gnsf_functions.json'
status = os.system('octave detect_gnsf_from_json.m')
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
Tf = 1.0
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 15
# discretization
ocp.dims.N = N
# shooting_nodes = np.linspace(0, Tf, N+1)
time_steps = np.linspace(0, 1, N)
time_steps = Tf * time_steps / sum(time_steps)
shooting_nodes = np.zeros((N + 1, ))
for i in range(len(time_steps)):
shooting_nodes[i + 1] = shooting_nodes[i] + time_steps[i]
# nonuniform discretizations can be defined either by shooting_nodes or time_steps:
if discretization == 'shooting_nodes':
ocp.solver_options.shooting_nodes = shooting_nodes
elif discretization == 'time_steps':
ocp.solver_options.time_steps = time_steps
else:
raise NotImplementedError(
f"discretization type {discretization} not supported.")
# set num_steps
ocp.solver_options.sim_method_num_steps = 2 * np.ones((N, ))
ocp.solver_options.sim_method_num_steps[0] = 3
# set num_stages
ocp.solver_options.sim_method_num_stages = 2 * np.ones((N, ))
ocp.solver_options.sim_method_num_stages[0] = 4
# set cost
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W_e = Q
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4, 0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
# set constraints
Fmax = 80
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = integrator_type
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.initialize_t_slacks = 1
# Set additional options for Simulink interface:
acados_path = get_acados_path()
json_path = os.path.join(acados_path,
'interfaces/acados_template/acados_template')
with open(json_path + '/simulink_default_opts.json', 'r') as f:
simulink_opts = json.load(f)
ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp.json',
simulink_opts=simulink_opts)
# ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
simX = np.ndarray((N + 1, nx))
simU = np.ndarray((N, nu))
# change options after creating ocp_solver
ocp_solver.options_set("step_length", 0.99999)
ocp_solver.options_set("globalization",
"fixed_step") # fixed_step, merit_backtracking
ocp_solver.options_set("tol_eq", TOL)
ocp_solver.options_set("tol_stat", TOL)
ocp_solver.options_set("tol_ineq", TOL)
ocp_solver.options_set("tol_comp", TOL)
# initialize solver
for i in range(N):
ocp_solver.set(i, "x", x0)
status = ocp_solver.solve()
if status not in [0, 2]:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get primal solution
for i in range(N):
simX[i, :] = ocp_solver.get(i, "x")
simU[i, :] = ocp_solver.get(i, "u")
simX[N, :] = ocp_solver.get(N, "x")
print("inequality multipliers at stage 1")
print(ocp_solver.get(1, "lam")) # inequality multipliers at stage 1
print("slack values at stage 1")
print(ocp_solver.get(1, "t")) # slack values at stage 1
print("multipliers of dynamic conditions between stage 1 and 2")
print(ocp_solver.get(
1, "pi")) # multipliers of dynamic conditions between stage 1 and 2
# initialize ineq multipliers and slacks at stage 1
ocp_solver.set(1, "lam", np.zeros(2, ))
ocp_solver.set(1, "t", np.zeros(2, ))
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
# timings
time_tot = ocp_solver.get_stats("time_tot")
time_lin = ocp_solver.get_stats("time_lin")
time_sim = ocp_solver.get_stats("time_sim")
time_qp = ocp_solver.get_stats("time_qp")
print(
f"timings OCP solver: total: {1e3*time_tot}ms, lin: {1e3*time_lin}ms, sim: {1e3*time_sim}ms, qp: {1e3*time_qp}ms"
)
# print("simU", simU)
# print("simX", simX)
iterate_filename = f'final_iterate_{discretization}.json'
ocp_solver.store_iterate(filename=iterate_filename, overwrite=True)
plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False)
del ocp_solver
def main(use_cython=True):
# (very) simple crane model
beta = 0.001
k = 0.9
a_max = 10
dt_max = 2.0
# states
p1 = SX.sym('p1')
v1 = SX.sym('v1')
p2 = SX.sym('p2')
v2 = SX.sym('v2')
x = vertcat(p1, v1, p2, v2)
# controls
a = SX.sym('a')
dt = SX.sym('dt')
u = vertcat(a, dt)
f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1))
model = AcadosModel()
model.f_expl_expr = f_expl
model.x = x
model.u = u
model.name = 'crane_time_opt'
# create ocp object to formulate the OCP
x0 = np.array([2.0, 0.0, 2.0, 0.0])
xf = np.array([0.0, 0.0, 0.0, 0.0])
ocp = AcadosOcp()
ocp.model = model
# N - maximum number of bangs
N = 7
Tf = N
nx = model.x.size()[0]
nu = model.u.size()[0]
# set dimensions
ocp.dims.N = N
# set cost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = dt
ocp.model.cost_expr_ext_cost_e = 0
ocp.constraints.lbu = np.array([-a_max, 0.0])
ocp.constraints.ubu = np.array([+a_max, dt_max])
ocp.constraints.idxbu = np.array([0, 1])
ocp.constraints.x0 = x0
ocp.constraints.lbx_e = xf
ocp.constraints.ubx_e = xf
ocp.constraints.idxbx_e = np.array([0, 1, 2, 3])
# set prediction horizon
ocp.solver_options.tf = Tf
# set options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 3
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
ocp.solver_options.nlp_solver_max_iter = 5000
ocp.solver_options.nlp_solver_tol_stat = 1e-6
ocp.solver_options.levenberg_marquardt = 0.1
ocp.solver_options.sim_method_num_steps = 15
ocp.solver_options.qp_solver_iter_max = 100
ocp.code_export_directory = 'c_generated_code'
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.exact_hess_constr = 0
ocp.solver_options.exact_hess_dyn = 0
if use_cython:
AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json')
AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json')
else: # ctypes
## Note: skip generate and build assuming this is done before (in cython run)
ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp.json',
build=False,
generate=False)
ocp_solver.reset()
for i, tau in enumerate(np.linspace(0, 1, N)):
ocp_solver.set(i, 'x', (1 - tau) * x0 + tau * xf)
ocp_solver.set(i, 'u', np.array([0.1, 0.5]))
simX = np.zeros((N + 1, nx))
simU = np.zeros((N, nu))
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics()
raise Exception(f'acados returned status {status}.')
# get solution
for i in range(N):
simX[i, :] = ocp_solver.get(i, "x")
simU[i, :] = ocp_solver.get(i, "u")
simX[N, :] = ocp_solver.get(N, "x")
dts = simU[:, 1]
print(
"acados solved OCP successfully, creating integrator to simulate the solution"
)
# simulate on finer grid
sim = AcadosSim()
# set model
sim.model = model
# set options
sim.solver_options.integrator_type = 'ERK'
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.T = 1.0 # dummy value
dt_approx = 0.0005
dts_fine = np.zeros((N, ))
Ns_fine = np.zeros((N, ), dtype='int16')
# compute number of simulation steps for bang interval + dt_fine
for i in range(N):
N_approx = max(int(dts[i] / dt_approx), 1)
dts_fine[i] = dts[i] / N_approx
Ns_fine[i] = int(round(dts[i] / dts_fine[i]))
N_fine = int(np.sum(Ns_fine))
simU_fine = np.zeros((N_fine, nu))
ts_fine = np.zeros((N_fine + 1, ))
simX_fine = np.zeros((N_fine + 1, nx))
simX_fine[0, :] = x0
acados_integrator = AcadosSimSolver(sim)
k = 0
for i in range(N):
u = simU[i, 0]
acados_integrator.set("u", np.hstack((u, np.ones(1, ))))
# set simulation time
acados_integrator.set("T", dts_fine[i])
for j in range(Ns_fine[i]):
acados_integrator.set("x", simX_fine[k, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(f'acados returned status {status}.')
simX_fine[k + 1, :] = acados_integrator.get("x")
simU_fine[k, :] = u
ts_fine[k + 1] = ts_fine[k] + dts_fine[i]
k += 1
# visualize
if os.environ.get('ACADOS_ON_TRAVIS'):
plt.figure()
state_labels = ['p1', 'v1', 'p2', 'v2']
for i, l in enumerate(state_labels):
plt.subplot(5, 1, i + 1)
plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution')
plt.grid(True)
plt.ylabel(l)
if i == 0:
plt.legend(loc=1)
plt.subplot(5, 1, 5)
plt.step(ts_fine,
np.hstack((simU_fine[:, 0], simU_fine[-1, 0])),
'-',
where='post')
plt.grid(True)
plt.ylabel('a')
plt.xlabel('t')
plt.show()
def solve_armijo_problem_with_setting(setting):
globalization = setting['globalization']
line_search_use_sufficient_descent = setting[
'line_search_use_sufficient_descent']
globalization_use_SOC = setting['globalization_use_SOC']
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = AcadosModel()
x = SX.sym('x')
# dynamics: identity
model.disc_dyn_expr = x
model.x = x
model.u = SX.sym('u', 0, 0) # [] / None doesnt work
model.p = []
model.name = f'armijo_problem'
ocp.model = model
# discretization
Tf = 1
N = 1
ocp.dims.N = N
ocp.solver_options.tf = Tf
# cost
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost_e = x @ x
ocp.model.cost_expr_ext_cost_custom_hess_e = 1.0 # 2.0 is the actual hessian
# constarints
ocp.constraints.idxbx = np.array([0])
ocp.constraints.lbx = np.array([-10.0])
ocp.constraints.ubx = np.array([10.0])
# options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' # 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.integrator_type = 'DISCRETE'
ocp.solver_options.print_level = 0
ocp.solver_options.tol = TOL
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = globalization
ocp.solver_options.alpha_reduction = 0.9
ocp.solver_options.line_search_use_sufficient_descent = line_search_use_sufficient_descent
ocp.solver_options.globalization_use_SOC = globalization_use_SOC
ocp.solver_options.eps_sufficient_descent = 5e-1
SQP_max_iter = 200
ocp.solver_options.qp_solver_iter_max = 400
ocp.solver_options.nlp_solver_max_iter = SQP_max_iter
# create solver
ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json')
# initialize solver
xinit = np.array([1.0])
[ocp_solver.set(i, "x", xinit) for i in range(N + 1)]
# get stats
status = ocp_solver.solve()
ocp_solver.print_statistics()
iter = ocp_solver.get_stats('sqp_iter')[0]
alphas = ocp_solver.get_stats('alpha')[1:]
qp_iters = ocp_solver.get_stats('qp_iter')
print(f"acados ocp solver returned status {status}")
# get solution
solution = ocp_solver.get(0, "x")
print(f"found solution {solution}")
# print summary
print(f"solved Armijo test problem with settings {setting}")
print(
f"cost function value = {ocp_solver.get_cost()} after {iter} SQP iterations"
)
print(f"alphas: {alphas[:iter]}")
print(f"total number of QP iterations: {sum(qp_iters[:iter])}")
# compare to analytical solution
exact_solution = np.array([0.0])
sol_err = max(np.abs(solution - exact_solution))
print(f"error wrt analytical solution {sol_err}")
# checks
if ocp.model.cost_expr_ext_cost_custom_hess_e == 1.0:
if globalization == 'MERIT_BACKTRACKING':
if sol_err > TOL * 1e1:
raise Exception(
f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}"
)
else:
print(f"matched analytical solution with tolerance {TOL}")
if status != 0:
raise Exception(
f"acados solver returned status {status} != 0.")
if line_search_use_sufficient_descent == 1:
if iter > 22:
raise Exception(f"acados ocp solver took {iter} iterations." + \
"Expected <= 22 iterations for globalized SQP method with aggressive eps_sufficient_descent condition on Armijo test problem.")
else:
if iter < 64:
raise Exception(f"acados ocp solver took {iter} iterations." + \
"Expected > 64 iterations for globalized SQP method without sufficient descent condition on Armijo test problem.")
elif globalization == 'FIXED_STEP':
if status != 2:
raise Exception(
f"acados solver returned status {status} != 2. Expected maximum iterations for full-step SQP on Armijo test problem."
)
else:
print(
f"Sucess: Expected maximum iterations for full-step SQP on Armijo test problem."
)
print(f"\n\n----------------------\n")
class MPC_controller(object):
def __init__(self,
quad_model,
quad_constraints,
t_horizon,
n_nodes,
sim_required=False):
self.model = quad_model
self.constraints = quad_constraints
self.g_ = 9.8066
self.T = t_horizon
self.N = n_nodes
self.simulation_required = sim_required
self.current_pose = None
self.current_state = None
self.dt = 0.05
self.rate = rospy.Rate(1 / self.dt)
self.time_stamp = None
self.trajectory_path = None
self.current_twist = np.zeros(3)
self.att_command = AttitudeTarget()
self.att_command.type_mask = 128
# subscribers
## the robot state
robot_state_sub_ = rospy.Subscriber('/robot_pose', Odometry,
self.robot_state_callback)
## trajectory
robot_trajectory_sub_ = rospy.Subscriber(
'/robot_trajectory', itm_trajectory_msg,
self.trajectory_command_callback)
# publisher
self.att_setpoint_pub = rospy.Publisher(
'/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
# create a server
server_ = rospy.Service('uav_mpc_server', SetBool, self.state_server)
# setup optimizer
self.quadrotor_optimizer_setup()
# # It seems that thread cannot ensure the performance of the time
self.att_thread = Thread(target=self.send_command, args=())
self.att_thread.daemon = True
self.att_thread.start()
def robot_state_callback(self, data):
# robot state as [x, y, z, vx, vy, vz, [w, x, y, z]]
self.current_state = np.array([
data.pose.pose.position.x,
data.pose.pose.position.y,
data.pose.pose.position.z,
data.pose.pose.orientation.w,
data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.twist.twist.linear.x,
data.twist.twist.linear.y,
data.twist.twist.linear.z,
]).reshape(1, -1)
def trajectory_command_callback(self, data):
temp_traj = data.traj
if data.size != len(temp_traj):
rospy.logerr('Some data are lost')
else:
self.trajectory_path = np.zeros((data.size, 10))
for i in range(data.size):
quaternion_ = self.rpy_to_quaternion(
[temp_traj[i].roll, temp_traj[i].pitch, temp_traj[i].yaw])
self.trajectory_path[i] = np.array([
temp_traj[i].x,
temp_traj[i].y,
temp_traj[i].z,
quaternion_[0],
quaternion_[1],
quaternion_[2],
quaternion_[3],
temp_traj[i].vx,
temp_traj[i].vy,
temp_traj[i].vz,
])
def quadrotor_optimizer_setup(self, ):
Q_m_ = np.diag([
10,
10,
10,
0.3,
0.3,
0.3,
0.3,
0.05,
0.05,
0.05,
]) # position, q, v
P_m_ = np.diag([10, 10, 10, 0.05, 0.05, 0.05]) # only p and v
R_m_ = np.diag([5.0, 5.0, 5.0, 0.6])
nx = self.model.x.size()[0]
self.nx = nx
nu = self.model.u.size()[0]
self.nu = nu
ny = nx + nu
n_params = self.model.p.size()[0] if isinstance(self.model.p,
ca.SX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, acados_source_path)
# create OCP
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = self.model
ocp.dims.N = self.N
ocp.solver_options.tf = self.T
# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)
# cost type
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_)
ocp.cost.W_e = P_m_
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
ocp.cost.Vx_e = np.zeros((nx - 4, nx))
# ocp.cost.Vx_e[:6, :6] = np.eye(6)
ocp.cost.Vx_e[:3, :3] = np.eye(3)
ocp.cost.Vx_e[-3:, -3:] = np.eye(3)
# initial reference trajectory_ref
x_ref = np.zeros(nx)
x_ref[3] = 1.0
x_ref_e = np.zeros(nx - 4)
u_ref = np.zeros(nu)
u_ref[-1] = self.g_
ocp.cost.yref = np.concatenate((x_ref, u_ref))
ocp.cost.yref_e = x_ref_e
# Set constraints
ocp.constraints.lbu = np.array([
self.constraints.roll_rate_min, self.constraints.pitch_rate_min,
self.constraints.yaw_rate_min, self.constraints.thrust_min
])
ocp.constraints.ubu = np.array([
self.constraints.roll_rate_max, self.constraints.pitch_rate_max,
self.constraints.yaw_rate_max, self.constraints.thrust_max
])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# initial state
ocp.constraints.x0 = x_ref
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# explicit Runge-Kutta integrator
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI'
ocp.solver_options.nlp_solver_max_iter = 400
# compile acados ocp
## files are stored in .ros/
json_file = os.path.join('./' + self.model.name + '_acados_ocp.json')
self.solver = AcadosOcpSolver(ocp, json_file=json_file)
if self.simulation_required:
self.integrator = AcadosSimSolver(ocp, json_file=json_file)
def mpc_estimation_loop(self, ):
t1 = time.time()
if self.trajectory_path is not None and self.current_state is not None:
current_trajectory = self.trajectory_path
u_des = np.array([0.0, 0.0, 0.0, self.g_])
self.solver.set(
self.N, 'yref',
np.concatenate(
(current_trajectory[-1, :3], current_trajectory[-1, -3:])))
# self.solver.set(self.N, 'yref', current_trajectory[-1, :6])
for i in range(self.N):
self.solver.set(i, 'yref',
np.concatenate((current_trajectory[i], u_des)))
self.solver.set(0, 'lbx', self.current_state.flatten())
self.solver.set(0, 'ubx', self.current_state.flatten())
# print(self.current_state)
status = self.solver.solve()
if status != 0:
rospy.logerr("MPC cannot find a proper solution.")
self.att_command.thrust = 0.5
self.att_command.body_rate.z = 0.0
self.att_command.body_rate.x = 0.0
self.att_command.body_rate.y = 0.0
# only for debug
# print(self.trajectory_path)
# print("----")
# print(self.current_state)
else:
mpc_u_ = self.solver.get(0, 'u')
# print(mpc_u_)
# for i in range(self.N):
# print(self.solver.get(i, 'x'))
self.att_command.body_rate.x = mpc_u_[0]
self.att_command.body_rate.y = mpc_u_[1]
self.att_command.body_rate.z = mpc_u_[2]
self.att_command.thrust = mpc_u_[3] / self.g_ - 0.5
# self.att_setpoint_pub.publish(self.att_command)
else:
if self.trajectory_path is None:
rospy.loginfo("waiting trajectory")
elif self.current_state is None:
rospy.loginfo("waiting current state")
else:
rospy.loginfo("Unknown error")
self.rate.sleep()
# print(time.time()-t1)
return True
@staticmethod
def quaternion_to_rpy(quaternion):
q0, q1, q2, q3 = quaternion.w, quaternion.x, quaternion.y, quaternion.z
roll_ = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
pitch_ = np.arcsin(2 * (q0 * q2 - q3 * q1))
yaw_ = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
return roll_, pitch_, yaw_
@staticmethod
def rpy_to_quaternion(rqy):
roll_, pitch_, yaw_ = rqy
cy = np.cos(yaw_ * 0.5)
sy = np.sin(yaw_ * 0.5)
cp = np.cos(pitch_ * 0.5)
sp = np.sin(pitch_ * 0.5)
cr = np.cos(roll_ * 0.5)
sr = np.sin(roll_ * 0.5)
w_ = cr * cp * cy + sr * sp * sy
x_ = sr * cp * cy - cr * sp * sy
y_ = cr * sp * cy + sr * cp * sy
z_ = cr * cp * sy - sr * sp * cy
return np.array([w_, x_, y_, z_])
def state_server(self, req):
return SetBoolResponse(True, 'MPC is ready')
def send_command(self, ):
rate = rospy.Rate(100) # Hz
self.att_command.header = Header()
while not rospy.is_shutdown():
# t2 = time.time()
command_ = self.att_command
# self.att_command.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(command_)
try: # prevent garbage in console output when thread is killed
rate.sleep()
except rospy.ROSInterruptException:
pass
def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_num_hess=0,
integrator_type='ERK'):
print(f"using: cost_type {cost_type}, integrator_type {integrator_type}")
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
Tf = 1.0
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 20
ocp.dims.N = N
# set cost
Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2*np.diag([1e-2])
x = ocp.model.x
u = ocp.model.u
cost_W = scipy.linalg.block_diag(Q, R)
if cost_type == 'LS':
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4,0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
elif cost_type == 'NONLINEAR_LS':
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
ocp.model.cost_y_expr = vertcat(x, u)
ocp.model.cost_y_expr_e = x
elif cost_type == 'EXTERNAL':
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = vertcat(x, u).T @ cost_W @ vertcat(x, u)
ocp.model.cost_expr_ext_cost_e = x.T @ Q @ x
else:
raise Exception('Unknown cost_type. Possible values are \'LS\' and \'NONLINEAR_LS\'.')
if cost_type in ['LS', 'NONLINEAR_LS']:
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
ocp.cost.W_e = Q
ocp.cost.W = cost_W
# set constraints
Fmax = 80
ocp.constraints.constr_type = 'BGH'
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = hessian_approximation
ocp.solver_options.regularize_method = 'CONVEXIFY'
ocp.solver_options.integrator_type = integrator_type
if ocp.solver_options.integrator_type == 'GNSF':
import json
with open('../getting_started/common/' + model.name + '_gnsf_functions.json', 'r') as f:
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
ocp.solver_options.qp_solver_cond_N = 5
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.ext_cost_num_hess = ext_cost_use_num_hess
ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
# set NaNs as input to test reset() -> NOT RECOMMENDED!!!
# ocp_solver.options_set('print_level', 2)
for i in range(N):
ocp_solver.set(i, 'x', np.NaN * np.ones((nx,)))
ocp_solver.set(i, 'u', np.NaN * np.ones((nu,)))
status = ocp_solver.solve()
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
if status == 0:
raise Exception(f'acados returned status {status}, although NaNs were given.')
else:
print(f'acados returned status {status}, which is expected, since NaNs were given.')
# RESET
ocp_solver.reset()
for i in range(N):
ocp_solver.set(i, 'x', x0)
if cost_type == 'EXTERNAL':
# NOTE: hessian is wrt [u,x]
if ext_cost_use_num_hess:
for i in range(N):
ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ]))
ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ]))
simX = np.ndarray((N+1, nx))
simU = np.ndarray((N, nu))
status = ocp_solver.solve()
ocp_solver.print_statistics()
if status != 0:
raise Exception(f'acados returned status {status} for cost_type {cost_type}\n'
f'integrator_type = {integrator_type}.')
# get solution
for i in range(N):
simX[i,:] = ocp_solver.get(i, "x")
simU[i,:] = ocp_solver.get(i, "u")
simX[N,:] = ocp_solver.get(N, "x")
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
def run_closed_loop_experiment(FORMULATION):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
Tf = 1.0
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 20
# set dimensions
# NOTE: all dimensions but N ar detected
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4, 0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.W_e = Q
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
ocp.cost.zl = 2000 * np.ones((1, ))
ocp.cost.Zl = 1 * np.ones((1, ))
ocp.cost.zu = 2000 * np.ones((1, ))
ocp.cost.Zu = 1 * np.ones((1, ))
# set constraints
Fmax = 80
vmax = 5
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
# bound on u
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
if FORMULATION == 0:
# soft bound on x
ocp.constraints.lbx = np.array([-vmax])
ocp.constraints.ubx = np.array([+vmax])
ocp.constraints.idxbx = np.array([2]) # v is x[2]
# indices of slacked constraints within bx
ocp.constraints.idxsbx = np.array([0])
elif FORMULATION == 1:
# soft bound on x, using constraint h
v1 = ocp.model.x[2]
ocp.model.con_h_expr = v1
ocp.constraints.lh = np.array([-vmax])
ocp.constraints.uh = np.array([+vmax])
# indices of slacked constraints within h
ocp.constraints.idxsh = np.array([0])
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.tol = 1e-1 * tol
json_filename = 'pendulum_soft_constraints.json'
acados_ocp_solver = AcadosOcpSolver(ocp, json_file=json_filename)
acados_integrator = AcadosSimSolver(ocp, json_file=json_filename)
# closed loop
Nsim = 20
simX = np.ndarray((Nsim + 1, nx))
simU = np.ndarray((Nsim, nu))
xcurrent = x0
for i in range(Nsim):
simX[i, :] = xcurrent
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
if status != 0:
raise Exception(
'acados acados_ocp_solver returned status {}. Exiting.'.format(
status))
simU[i, :] = acados_ocp_solver.get(0, "u")
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados integrator returned status {}. Exiting.'.format(
status))
# update state
xcurrent = acados_integrator.get("x")
simX[Nsim, :] = xcurrent
# get slack values at stage 1
sl = acados_ocp_solver.get(1, "sl")
su = acados_ocp_solver.get(1, "su")
print("sl", sl, "su", su)
# plot results
# plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)
# store results
np.savetxt('test_results/simX_soft_formulation_' + str(FORMULATION), simX)
np.savetxt('test_results/simU_soft_formulation_' + str(FORMULATION), simU)
print("soft constraint example: ran formulation", FORMULATION,
"successfully.")
def solve_marathos_ocp(setting):
globalization = setting['globalization']
line_search_use_sufficient_descent = setting[
'line_search_use_sufficient_descent']
globalization_use_SOC = setting['globalization_use_SOC']
qp_solver = setting['qp_solver']
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_linear_mass_model()
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nu
# discretization
Tf = 2
N = 20
shooting_nodes = np.linspace(0, Tf, N + 1)
ocp.dims.N = N
# set cost
Q = 2 * np.diag([])
R = 2 * np.diag([1e1, 1e1])
ocp.cost.W_e = Q
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
Vu = np.eye((nu))
ocp.cost.Vu = Vu
ocp.cost.yref = np.zeros((ny, ))
# set constraints
Fmax = 5
ocp.constraints.lbu = -Fmax * np.ones((nu, ))
ocp.constraints.ubu = +Fmax * np.ones((nu, ))
ocp.constraints.idxbu = np.array(range(nu))
x0 = np.array([1e-1, 1.1, 0, 0])
ocp.constraints.x0 = x0
# terminal constraint
x_goal = np.array([0, -1.1, 0, 0])
ocp.constraints.idxbx_e = np.array(range(nx))
ocp.constraints.lbx_e = x_goal
ocp.constraints.ubx_e = x_goal
if SOFTEN_TERMINAL:
ocp.constraints.idxsbx_e = np.array(range(nx))
ocp.cost.zl_e = 1e4 * np.ones(nx)
ocp.cost.zu_e = 1e4 * np.ones(nx)
ocp.cost.Zl_e = 1e6 * np.ones(nx)
ocp.cost.Zu_e = 1e6 * np.ones(nx)
# add obstacle
if OBSTACLE:
obs_rad = 1.0
obs_x = 0.0
obs_y = 0.0
circle = (obs_x, obs_y, obs_rad)
ocp.constraints.uh = np.array([100.0]) # doenst matter
ocp.constraints.lh = np.array([obs_rad**2])
x_square = model.x[0]**OBSTACLE_POWER + model.x[1]**OBSTACLE_POWER
ocp.model.con_h_expr = x_square
# copy for terminal
ocp.constraints.uh_e = ocp.constraints.uh
ocp.constraints.lh_e = ocp.constraints.lh
ocp.model.con_h_expr_e = ocp.model.con_h_expr
else:
circle = None
# soften
if OBSTACLE and SOFTEN_OBSTACLE:
ocp.constraints.idxsh = np.array([0])
ocp.constraints.idxsh_e = np.array([0])
Zh = 1e6 * np.ones(1)
zh = 1e4 * np.ones(1)
ocp.cost.zl = zh
ocp.cost.zu = zh
ocp.cost.Zl = Zh
ocp.cost.Zu = Zh
ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh))
ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh))
ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh))
ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, Zh))
# set options
ocp.solver_options.qp_solver = qp_solver # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = globalization
ocp.solver_options.alpha_min = 0.01
# ocp.solver_options.__initialize_t_slacks = 0
# ocp.solver_options.levenberg_marquardt = 1e-2
ocp.solver_options.qp_solver_cond_N = 0
ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_max_iter = 200
ocp.solver_options.qp_solver_iter_max = 400
# NOTE: this is needed for PARTIAL_CONDENSING_HPIPM to get expected behavior
qp_tol = 5e-7
ocp.solver_options.qp_solver_tol_stat = qp_tol
ocp.solver_options.qp_solver_tol_eq = qp_tol
ocp.solver_options.qp_solver_tol_ineq = qp_tol
ocp.solver_options.qp_solver_tol_comp = qp_tol
ocp.solver_options.qp_solver_ric_alg = 1
# ocp.solver_options.qp_solver_cond_ric_alg = 1
# set prediction horizon
ocp.solver_options.tf = Tf
ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_ocp.json')
ocp_solver.options_set('line_search_use_sufficient_descent',
line_search_use_sufficient_descent)
ocp_solver.options_set('globalization_use_SOC', globalization_use_SOC)
ocp_solver.options_set('full_step_dual', 1)
if INITIALIZE: # initialize solver
# [ocp_solver.set(i, "x", x0 + (i/N) * (x_goal-x0)) for i in range(N+1)]
[ocp_solver.set(i, "x", x0) for i in range(N + 1)]
# [ocp_solver.set(i, "u", 2*(np.random.rand(2) - 0.5)) for i in range(N)]
# solve
status = ocp_solver.solve()
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
sqp_iter = ocp_solver.get_stats('sqp_iter')[0]
print(f'acados returned status {status}.')
# ocp_solver.store_iterate(f'it{ocp.solver_options.nlp_solver_max_iter}_{model.name}.json')
# get solution
simX = np.array([ocp_solver.get(i, "x") for i in range(N + 1)])
simU = np.array([ocp_solver.get(i, "u") for i in range(N)])
pi_multiplier = [ocp_solver.get(i, "pi") for i in range(N)]
print(f"cost function value = {ocp_solver.get_cost()}")
# print summary
print(f"solved Marathos test problem with settings {setting}")
print(
f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations"
)
# print(f"alphas: {alphas[:iter]}")
# print(f"total number of QP iterations: {sum(qp_iters[:iter])}")
# max_infeasibility = np.max(residuals[1:3])
# print(f"max infeasibility: {max_infeasibility}")
# checks
if status != 0:
raise Exception(f"acados solver returned status {status} != 0.")
if globalization == "FIXED_STEP":
if sqp_iter != 18:
raise Exception(
f"acados solver took {sqp_iter} iterations, expected 18.")
elif globalization == "MERIT_BACKTRACKING":
if globalization_use_SOC == 1 and line_search_use_sufficient_descent == 0 and sqp_iter not in range(
21, 23):
raise Exception(
f"acados solver took {sqp_iter} iterations, expected range(21, 23)."
)
elif globalization_use_SOC == 1 and line_search_use_sufficient_descent == 1 and sqp_iter not in range(
21, 24):
raise Exception(
f"acados solver took {sqp_iter} iterations, expected range(21, 24)."
)
elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 0 and sqp_iter not in range(
155, 165):
raise Exception(
f"acados solver took {sqp_iter} iterations, expected range(155, 165)."
)
elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 1 and sqp_iter not in range(
160, 175):
raise Exception(
f"acados solver took {sqp_iter} iterations, expected range(160, 175)."
)
if PLOT:
plot_linear_mass_system_X_state_space(simX,
circle=circle,
x_goal=x_goal)
plot_linear_mass_system_U(shooting_nodes, simU)
# plot_linear_mass_system_X(shooting_nodes, simX)
# import pdb; pdb.set_trace()
print(f"\n\n----------------------\n")
# simulate
for i in tqdm(range(Nsim)):
#print(x0)
x_noise = x0 + np.diag([1, 0.1, 0.2, 1, 0.1, 0.2
]) @ np.random.normal(
0, noise_std, x0.shape)
#print(x_noise)
acados_solver.set(0, "lbx", x_noise)
acados_solver.set(0, "ubx", x_noise)
#acados_solver.set(0, "x", x_noise)
for j in range(N):
acados_solver.set(j, "yref", yref)
acados_solver.set(N, 'yref', yref_e)
status = acados_solver.solve()
if status != 0:
raise Exception(
"acados returned status {} in closed loop iteration {}."
.format(status, i))
# get solution
u0 = acados_solver.get(0, "u")
#print(u0)
for j in range(2):
simU[i, j] = u0[j]
for j in range(nx):
simX[i, j] = x0[j]
# update initial condition
def solve_marathos_problem_with_setting(setting):
globalization = setting['globalization']
line_search_use_sufficient_descent = setting[
'line_search_use_sufficient_descent']
globalization_use_SOC = setting['globalization_use_SOC']
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = AcadosModel()
x1 = SX.sym('x1')
x2 = SX.sym('x2')
x = vertcat(x1, x2)
# dynamics: identity
model.disc_dyn_expr = x
model.x = x
model.u = SX.sym('u', 0, 0) # [] / None doesnt work
model.p = []
model.name = f'marathos_problem'
ocp.model = model
# discretization
Tf = 1
N = 1
ocp.dims.N = N
ocp.solver_options.tf = Tf
# cost
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost_e = x1
# constarints
ocp.model.con_h_expr = x1**2 + x2**2
ocp.constraints.lh = np.array([1.0])
ocp.constraints.uh = np.array([1.0])
# # soften
# ocp.constraints.idxsh = np.array([0])
# ocp.cost.zl = 1e5 * np.array([1])
# ocp.cost.zu = 1e5 * np.array([1])
# ocp.cost.Zl = 1e5 * np.array([1])
# ocp.cost.Zu = 1e5 * np.array([1])
# add bounds on x
# nx = 2
# ocp.constraints.idxbx_0 = np.array(range(nx))
# ocp.constraints.lbx_0 = -2 * np.ones((nx))
# ocp.constraints.ubx_0 = 2 * np.ones((nx))
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.integrator_type = 'DISCRETE'
# ocp.solver_options.print_level = 1
ocp.solver_options.tol = TOL
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = globalization
ocp.solver_options.alpha_min = 1e-2
# ocp.solver_options.__initialize_t_slacks = 0
# ocp.solver_options.regularize_method = 'CONVEXIFY'
ocp.solver_options.levenberg_marquardt = 1e-1
# ocp.solver_options.print_level = 2
SQP_max_iter = 300
ocp.solver_options.qp_solver_iter_max = 400
ocp.solver_options.regularize_method = 'MIRROR'
# ocp.solver_options.exact_hess_constr = 0
ocp.solver_options.line_search_use_sufficient_descent = line_search_use_sufficient_descent
ocp.solver_options.globalization_use_SOC = globalization_use_SOC
ocp.solver_options.eps_sufficient_descent = 1e-1
ocp.solver_options.qp_tol = 5e-7
if FOR_LOOPING: # call solver in for loop to get all iterates
ocp.solver_options.nlp_solver_max_iter = 1
ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json')
else:
ocp.solver_options.nlp_solver_max_iter = SQP_max_iter
ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json')
# initialize solver
rad_init = 0.1 #0.1 #np.pi / 4
xinit = np.array([np.cos(rad_init), np.sin(rad_init)])
# xinit = np.array([0.82120912, 0.58406911])
[ocp_solver.set(i, "x", xinit) for i in range(N + 1)]
# solve
if FOR_LOOPING: # call solver in for loop to get all iterates
iterates = np.zeros((SQP_max_iter + 1, 2))
iterates[0, :] = xinit
alphas = np.zeros((SQP_max_iter, ))
qp_iters = np.zeros((SQP_max_iter, ))
iter = SQP_max_iter
residuals = np.zeros((4, SQP_max_iter))
# solve
for i in range(SQP_max_iter):
status = ocp_solver.solve()
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
# print(f'acados returned status {status}.')
iterates[i + 1, :] = ocp_solver.get(0, "x")
if status in [0, 4]:
iter = i
break
alphas[i] = ocp_solver.get_stats('alpha')[1]
qp_iters[i] = ocp_solver.get_stats('qp_iter')[1]
residuals[:, i] = ocp_solver.get_stats('residuals')
else:
ocp_solver.solve()
ocp_solver.print_statistics()
iter = ocp_solver.get_stats('sqp_iter')[0]
alphas = ocp_solver.get_stats('alpha')[1:]
qp_iters = ocp_solver.get_stats('qp_iter')
residuals = ocp_solver.get_stats('statistics')[1:5, 1:iter]
# get solution
solution = ocp_solver.get(0, "x")
# print summary
print(f"solved Marathos test problem with settings {setting}")
print(
f"cost function value = {ocp_solver.get_cost()} after {iter} SQP iterations"
)
print(f"alphas: {alphas[:iter]}")
print(f"total number of QP iterations: {sum(qp_iters[:iter])}")
max_infeasibility = np.max(residuals[1:3])
print(f"max infeasibility: {max_infeasibility}")
# compare to analytical solution
exact_solution = np.array([-1, 0])
sol_err = max(np.abs(solution - exact_solution))
# checks
if sol_err > TOL * 1e1:
raise Exception(
f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}"
)
else:
print(f"matched analytical solution with tolerance {TOL}")
try:
if globalization == 'FIXED_STEP':
# import pdb; pdb.set_trace()
if max_infeasibility < 5.0:
raise Exception(
f"Expected max_infeasibility > 5.0 when using full step SQP on Marathos problem"
)
if iter != 10:
raise Exception(
f"Expected 10 SQP iterations when using full step SQP on Marathos problem, got {iter}"
)
if any(alphas[:iter] != 1.0):
raise Exception(
f"Expected all alphas = 1.0 when using full step SQP on Marathos problem"
)
elif globalization == 'MERIT_BACKTRACKING':
if max_infeasibility > 0.5:
raise Exception(
f"Expected max_infeasibility < 0.5 when using globalized SQP on Marathos problem"
)
if globalization_use_SOC == 0:
if FOR_LOOPING and iter != 57:
raise Exception(
f"Expected 57 SQP iterations when using globalized SQP without SOC on Marathos problem, got {iter}"
)
elif line_search_use_sufficient_descent == 1:
if iter not in range(29, 37):
# NOTE: got 29 locally and 36 on Github actions.
# On Github actions the inequality constraint was numerically violated in the beginning.
# This leads to very different behavior, since the merit gradient is so different.
# Github actions: merit_grad = -1.669330e+00, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = -1.495535e+00
# Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00
raise Exception(
f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Marathos problem, got {iter}"
)
else:
if iter != 12:
raise Exception(
f"Expected 12 SQP iterations when using globalized SQP with SOC on Marathos problem, got {iter}"
)
except Exception as inst:
if FOR_LOOPING and globalization == "MERIT_BACKTRACKING":
print(
"\nAcados globalized OCP solver behaves different when for looping due to different merit function weights.",
"Following exception is not raised\n")
print(inst, "\n")
else:
raise (inst)
if PLOT:
plt.figure()
axs = plt.plot(solution[0], solution[1], 'x', label='solution')
if FOR_LOOPING: # call solver in for loop to get all iterates
cm = plt.cm.get_cmap('RdYlBu')
axs = plt.scatter(iterates[:iter + 1, 0],
iterates[:iter + 1, 1],
c=range(iter + 1),
s=35,
cmap=cm,
label='iterates')
plt.colorbar(axs)
ts = np.linspace(0, 2 * np.pi, 100)
plt.plot(1 * np.cos(ts) + 0, 1 * np.sin(ts) - 0, 'r')
plt.axis('square')
plt.legend()
plt.title(
f"Marathos problem with N = {N}, x formulation, SOC {globalization_use_SOC}"
)
plt.show()
print(f"\n\n----------------------\n")
# [00, @2, 00, 00, 00],
# [00, 00, @2, 00, 00],
# [00, 00, 00, @1, 00],
# [00, 00, 00, 00, @1]])
# NOTE: hessian is wrt [u,x]
if EXTERNAL_COST_USE_NUM_HESS:
for i in range(N):
ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ]))
ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ]))
simX = np.ndarray((N+1, nx))
simU = np.ndarray((N, nu))
status = ocp_solver.solve()
ocp_solver.print_statistics()
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N):
simX[i,:] = ocp_solver.get(i, "x")
simU[i,:] = ocp_solver.get(i, "u")
simX[N,:] = ocp_solver.get(N, "x")
plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)
def run_nominal_control(chain_params):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# chain parameters
n_mass = chain_params["n_mass"]
M = chain_params["n_mass"] - 2 # number of intermediate masses
Ts = chain_params["Ts"]
Tsim = chain_params["Tsim"]
N = chain_params["N"]
u_init = chain_params["u_init"]
with_wall = chain_params["with_wall"]
yPosWall = chain_params["yPosWall"]
m = chain_params["m"]
D = chain_params["D"]
L = chain_params["L"]
perturb_scale = chain_params["perturb_scale"]
nlp_iter = chain_params["nlp_iter"]
nlp_tol = chain_params["nlp_tol"]
save_results = chain_params["save_results"]
show_plots = chain_params["show_plots"]
seed = chain_params["seed"]
np.random.seed(seed)
nparam = 3*M
W = perturb_scale * np.eye(nparam)
# export model
model = export_disturbed_chain_mass_model(n_mass, m, D, L)
# set model
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
Tf = N * Ts
# initial state
xPosFirstMass = np.zeros((3,1))
xEndRef = np.zeros((3,1))
xEndRef[0] = L * (M+1) * 6
pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass)
xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef)
x0 = xrest
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
Q = 2*np.diagflat( np.ones((nx, 1)) )
q_diag = np.ones((nx,1))
strong_penalty = M+1
q_diag[3*M] = strong_penalty
q_diag[3*M+1] = strong_penalty
q_diag[3*M+2] = strong_penalty
Q = 2*np.diagflat( q_diag )
R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) )
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Q
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:nx+nu, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
# import pdb; pdb.set_trace()
yref = np.vstack((xrest, np.zeros((nu,1)))).flatten()
ocp.cost.yref = yref
ocp.cost.yref_e = xrest.flatten()
# set constraints
umax = 1*np.ones((nu,))
ocp.constraints.constr_type = 'BGH'
ocp.constraints.lbu = -umax
ocp.constraints.ubu = umax
ocp.constraints.x0 = x0.reshape((nx,))
ocp.constraints.idxbu = np.array(range(nu))
# disturbances
nparam = 3*M
ocp.parameter_values = np.zeros((nparam,))
# wall constraint
if with_wall:
nbx = M + 1
Jbx = np.zeros((nbx,nx))
for i in range(nbx):
Jbx[i, 3*i+1] = 1.0
ocp.constraints.Jbx = Jbx
ocp.constraints.lbx = yPosWall * np.ones((nbx,))
ocp.constraints.ubx = 1e9 * np.ones((nbx,))
# slacks
ocp.constraints.Jsbx = np.eye(nbx)
L2_pen = 1e3
L1_pen = 1
ocp.cost.Zl = L2_pen * np.ones((nbx,))
ocp.cost.Zu = L2_pen * np.ones((nbx,))
ocp.cost.zl = L1_pen * np.ones((nbx,))
ocp.cost.zu = L1_pen * np.ones((nbx,))
# solver options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.nlp_solver_max_iter = nlp_iter
ocp.solver_options.sim_method_num_stages = 2
ocp.solver_options.sim_method_num_steps = 2
ocp.solver_options.qp_solver_cond_N = N
ocp.solver_options.qp_tol = nlp_tol
ocp.solver_options.tol = nlp_tol
# ocp.solver_options.nlp_solver_tol_eq = 1e-9
# set prediction horizon
ocp.solver_options.tf = Tf
acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
# acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
acados_integrator = export_chain_mass_integrator(n_mass, m, D, L)
#%% get initial state from xrest
xcurrent = x0.reshape((nx,))
for i in range(5):
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", u_init)
status = acados_integrator.solve()
if status != 0:
raise Exception('acados integrator returned status {}. Exiting.'.format(status))
# update state
xcurrent = acados_integrator.get("x")
#%% actual simulation
N_sim = int(np.floor(Tsim/Ts))
simX = np.ndarray((N_sim+1, nx))
simU = np.ndarray((N_sim, nu))
wall_dist = np.zeros((N_sim,))
timings = np.zeros((N_sim,))
simX[0,:] = xcurrent
# closed loop
for i in range(N_sim):
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
timings[i] = acados_ocp_solver.get_stats("time_tot")[0]
if status != 0:
raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i))
simU[i,:] = acados_ocp_solver.get(0, "u")
print("control at time", i, ":", simU[i,:])
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i,:])
pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W)
acados_integrator.set("p", pertubation)
status = acados_integrator.solve()
if status != 0:
raise Exception('acados integrator returned status {}. Exiting.'.format(status))
# update state
xcurrent = acados_integrator.get("x")
simX[i+1,:] = xcurrent
# xOcpPredict = acados_ocp_solver.get(1, "x")
# print("model mismatch = ", str(np.max(xOcpPredict - xcurrent)))
yPos = xcurrent[range(1,3*M+1,3)]
wall_dist[i] = np.min(yPos - yPosWall)
print("time i = ", str(i), " dist2wall ", str(wall_dist[i]))
print("dist2wall (minimum over simulation) ", str(np.min(wall_dist)))
#%% plot results
if os.environ.get('ACADOS_ON_TRAVIS') is None and show_plots:
plot_chain_control_traj(simU)
plot_chain_position_traj(simX, yPosWall=yPosWall)
plot_chain_velocity_traj(simX)
animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall)
# animate_chain_position_3D(simX, xPosFirstMass)
plt.show()
