ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
# from casadi import jacobian
# ux = vertcat(ocp.model.u, ocp.model.x)
# jacobian(jacobian(ocp.model.cost_expr_ext_cost, ux), ux)
# SX(@1=0.04, @2=4000,
# [[@1, 00, 00, 00, 00],
# [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):
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Use use_SX "
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
def __acados_export_model(self, ocp):
# Declare model variables
x = ocp.nlp[0]["X"][0]
u = ocp.nlp[0]["U"][0]
p = ocp.nlp[0]["p"]
mod = ocp.nlp[0]["model"]
x_dot = SX.sym("x_dot", mod.nbQdot() * 2, 1)
f_expl = ocp.nlp[0]["dynamics_func"](x, u, p)
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.p = []
self.acados_model.name = "model_name"
def __prepare_acados(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError(
"more than 1 phase is not implemented yet with ACADOS backend")
if ocp.param_to_optimize:
raise NotImplementedError(
"Parameters optimization is not implemented yet with ACADOS")
# set model
self.acados_ocp.model = self.acados_model
# set dimensions
for i in range(ocp.nb_phases):
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[i]["tf"]
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[i]["nx"]
self.acados_ocp.dims.nu = ocp.nlp[i]["nu"]
self.acados_ocp.dims.ny = self.acados_ocp.dims.nx + self.acados_ocp.dims.nu
self.acados_ocp.dims.ny_e = ocp.nlp[i]["nx"]
self.acados_ocp.dims.N = ocp.nlp[i]["ns"]
for i in range(ocp.nb_phases):
# set constraints
for j in range(ocp.nlp[i]["nx"]):
if ocp.nlp[i]["X_bounds"].min[j, 0] == -np.inf and ocp.nlp[i][
"X_bounds"].max[j, 0] == np.inf:
pass
elif ocp.nlp[i]["X_bounds"].min[
j, 0] != ocp.nlp[i]["X_bounds"].max[j, 0]:
raise RuntimeError(
"Initial constraint on state must be hard. Hint: you can pass it as an objective"
)
else:
self.acados_ocp.constraints.x0 = np.array(
ocp.nlp[i]["X_bounds"].min[:, 0])
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# control constraints
self.acados_ocp.constraints.constr_type = "BGH"
self.acados_ocp.constraints.lbu = np.array(
ocp.nlp[i]["U_bounds"].min[:, 0])
self.acados_ocp.constraints.ubu = np.array(
ocp.nlp[i]["U_bounds"].max[:, 0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# path constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.ubx = np.array(
ocp.nlp[i]["X_bounds"].max[:, 1])
self.acados_ocp.constraints.lbx = np.array(
ocp.nlp[i]["X_bounds"].min[:, 1])
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# terminal constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.ubx_e = np.array(
ocp.nlp[i]["X_bounds"].max[:, -1])
self.acados_ocp.constraints.lbx_e = np.array(
ocp.nlp[i]["X_bounds"].min[:, -1])
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
return self.acados_ocp
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros(
(self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye(
self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros(
(self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
# TODO: Yes the objectives in ocp are not dealt with,
# does that makes sense since we only work with 1 phase ?
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
self.y_ref_end.append(
[np.zeros((J[0]["val"].numel(), 1))])
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny,
1), ))
if len(self.y_ref_end):
self.acados_ocp.cost.yref_e = np.concatenate(
self.y_ref_end, -1).T.squeeze()
else:
self.acados_ocp.cost.yref_e = np.zeros((1, ))
if self.W.shape == (0, 0):
self.acados_ocp.cost.W = np.zeros((1, 1))
else:
self.acados_ocp.cost.W = self.W
if self.W_e.shape == (0, 0):
self.acados_ocp.cost.W_e = np.zeros((1, 1))
else:
self.acados_ocp.cost.W_e = self.W_e
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError(
"TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError(
"TODO: I may have broken this (is this the right J?)"
)
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J["val"]])
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
self.acados_ocp.model.cost_expr_ext_cost = sum1(
self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "ERK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
def get_iterations(self):
raise NotImplementedError(
"return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self, ocp):
acados_x = np.array([
self.ocp_solver.get(i, "x") for i in range(ocp.nlp[0]["ns"] + 1)
]).T
acados_q = acados_x[:ocp.nlp[0]["nu"], :]
acados_qdot = acados_x[ocp.nlp[0]["nu"]:, :]
acados_u = np.array(
[self.ocp_solver.get(i, "u") for i in range(ocp.nlp[0]["ns"])]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
}
for i in range(ocp.nlp[0]["ns"]):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ocp.nlp[0]["ns"]])
out["x"] = vertcat(out["x"], acados_qdot[:, ocp.nlp[0]["ns"]])
return out
def solve(self):
# populate costs vectors
self.__set_costs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
for n in range(self.acados_ocp.dims.N):
self.ocp_solver.cost_set(
n, "yref",
np.concatenate([data[n] for data in self.y_ref])[:, 0])
self.ocp_solver.solve()
return self
class Pmpc(object):
def __init__(self,
N,
sys,
cost,
wref=None,
tuning=None,
lam_g_ref=None,
sensitivities=None,
options={}):
""" Constructor
"""
# store construction data
self.__N = N
self.__vars = sys['vars']
self.__nx = sys['vars']['x'].shape[0]
self.__nu = sys['vars']['u'].shape[0]
# nonlinear inequalities slacks
if 'us' in sys['vars']:
self.__ns = sys['vars']['us'].shape[0]
else:
self.__ns = 0
# mpc slacks
if 'usc' in sys['vars']:
self.__nsc = sys['vars']['usc'].shape[0]
self.__scost = sys['scost']
else:
self.__nsc = 0
# store system dynamics
self.__F = sys['f']
# store path constraints
if 'h' in sys:
self.__h = sys['h']
h_lin = self.__h(*self.__vars.values())
self.__h_x_idx = [
idx for idx in range(h_lin.shape[0])
if not True in ca.which_depends(
h_lin[idx], ct.vertcat(*list(self.__vars.values())[1:]))
]
else:
self.__h = None
# store slacked nonlinear inequality constraints
if 'g' in sys:
self.__gnl = sys['g']
self.__detect_state_dependent_constraints()
else:
self.__gnl = None
self.__h_us_idx = [] # no nonlinear state-dependent constraints
# store system sensitivities around steady state
self.__S = sys['S']
self.__cost = cost
# set options
self.__options = self.__default_options()
for option in options:
if option in self.__options:
self.__options[option] = options[option]
else:
raise ValueError(
'Unknown option for Pmpc class instance: "{}"'.format(
option))
# detect cost-type
if self.__cost.n_in() == 2:
# cost function of the form: l(x,u)
self.__type = 'economic'
# no tuning required
tuning = None
if self.__options['hessian_approximation'] == 'gauss_newton':
self.__options['hessian_approximation'] = 'exact'
Logger.logger.warning(
'Gauss-Newton Hessian approximation cannot be applied for economic MPC problem. Switched to exact Hessian.'
)
else:
# cost function of the form: (w-wref)'*H*(w-wref) + q'w
self.__type = 'tracking'
# check if tuning matrices are provided
assert tuning != None, 'Provide tuning matrices for tracking MPC!'
# periodicity operator
self.__p_operator = self.__options['p_operator']
self.__jac_p_operator = ca.Function('jac_p', [sys['vars']['x']], [
ca.jacobian(self.__p_operator(sys['vars']['x']), sys['vars']['x'])
])
self.__S = sensitivities
# construct MPC solver
self.__construct_solver()
# periodic indexing
self.__index = 0
self.__index_acados = 0
# create periodic reference
assert wref != None, 'Provide reference trajectory!'
self.__create_reference(wref, tuning, lam_g_ref)
# initialize log
self.__initialize_log()
# initialize acados solvers
self.__acados_ocp_solver = None
self.__acados_integrator = None
# solver initial guess
self.__set_initial_guess()
return None
def __default_options(self):
# default options
opts = {
'hessian_approximation':
'exact',
'ipopt_presolve':
False,
'max_iter':
2000,
'p_operator':
ca.Function('p_operator', [self.__vars['x']], [self.__vars['x']]),
'slack_flag':
'none'
}
return opts
def __construct_solver(self):
""" Construct periodic MPC solver
"""
# system variables and dimensions
x = self.__vars['x']
u = self.__vars['u']
# NLP parameters
if self.__type == 'economic':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, 1)),
ct.entry('xN', shape=(self.__nx, 1))
])
# reassign for brevity
x0 = self.__p['x0']
xN = self.__p['xN']
if self.__type == 'tracking':
ref_vars = (ct.entry('x', shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
ref_vars += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
# reference trajectory
wref = ct.struct_symMX([ref_vars])
nw = self.__nx + self.__nu + self.__ns
tuning = ct.struct_symMX([ # tracking tuning
ct.entry('H', shape=(nw, nw), repeat=self.__N),
ct.entry('q', shape=(nw, 1), repeat=self.__N)
])
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, )),
ct.entry('wref', struct=wref),
ct.entry('tuning', struct=tuning)
])
# reassign for brevity
x0 = self.__p['x0']
wref = self.__p.prefix['wref']
tuning = self.__p.prefix['tuning']
xN = wref['x', -1]
# NLP variables
variables_entry = (ct.entry('x',
shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
variables_entry += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
self.__wref = ct.struct_symMX([variables_entry
]) # structure of reference
if 'usc' in self.__vars:
variables_entry += (ct.entry('usc',
shape=(self.__nsc, ),
repeat=self.__N), )
# nlp variables + bounds
w = ct.struct_symMX([variables_entry])
# variable bounds are implemented as inequalities
self.__lbw = w(-np.inf)
self.__ubw = w(np.inf)
# prepare dynamics and path constraints entry
constraints_entry = (ct.entry('dyn',
shape=(self.__nx, ),
repeat=self.__N), )
if self.__gnl is not None:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
if self.__h is not None:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
# terminal constraint
self.__nx_term = self.__p_operator.size1_out(0)
# create general constraints structure
g_struct = ct.struct_symMX([
ct.entry('init', shape=(self.__nx, 1)), constraints_entry,
ct.entry('term', shape=(self.__nx_term, 1))
])
# create symbolic constraint expressions
map_args = collections.OrderedDict()
map_args['x0'] = ct.horzcat(*w['x', :-1])
map_args['p'] = ct.horzcat(*w['u'])
F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf'])
# generate constraints
constr = collections.OrderedDict()
constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])]
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*w['us'])
if self.__gnl is not None:
constr['g'] = ct.horzsplit(
self.__gnl.map(self.__N)(*map_args.values()))
if 'usc' in self.__vars:
map_args['usc'] = ct.horzcat(*w['usc'])
if self.__h is not None:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N)(*map_args.values()))
repeated_constr = list(
itertools.chain.from_iterable(zip(*constr.values())))
term_constraint = self.__p_operator(w['x', -1] - xN)
self.__g = g_struct(
ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint))
self.__lbg = g_struct(np.zeros(self.__g.shape))
self.__ubg = g_struct(np.zeros(self.__g.shape))
if self.__h is not None:
self.__ubg['h', :] = np.inf
for i in self.__h_us_idx + self.__h_x_idx: # rm constraints the only depend on x at k = 0
self.__lbg['h', 0, i] = -np.inf
# nlp cost
cost_map = self.__cost.map(self.__N)
if self.__type == 'economic':
cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])]
elif self.__type == 'tracking':
if self.__ns != 0:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k], w['us', k])
for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k], wref['us', k])
for k in range(self.__N)
])
else:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k])
for k in range(self.__N)
])
cost_args = [
cost_args_w, cost_args_w_ref,
ct.horzcat(*tuning['H']),
ct.horzcat(*tuning['q'])
]
if self.__options['hessian_approximation'] == 'gauss_newton':
if 'usc' not in self.__vars:
hess_gn = ct.diagcat(*tuning['H'],
ca.DM.zeros(self.__nx, self.__nx))
else:
hess_block = list(
itertools.chain.from_iterable(
zip(tuning['H'],
[ca.DM.zeros(self.__nsc, self.__nsc)] *
self.__N)))
hess_gn = ct.diagcat(*hess_block,
ca.DM.zeros(self.__nx, self.__nx))
J = ca.sum2(cost_map(*cost_args))
# add cost on slacks
if 'usc' in self.__vars:
J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc'])))
# create solver
prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p}
self.__w = w
self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g])
# create IPOPT-solver instance if needed
if self.__options['ipopt_presolve']:
opts = {
'ipopt': {
'linear_solver': 'ma57',
'print_level': 0
},
'expand': False
}
if Logger.logger.getEffectiveLevel() > 10:
opts['ipopt']['print_level'] = 0
opts['print_time'] = 0
opts['ipopt']['sb'] = 'yes'
self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts)
# create hessian approximation function
if self.__options['hessian_approximation'] == 'gauss_newton':
lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used
hess_approx = ca.Function('hess_approx',
[self.__w, self.__p, lam_g], [hess_gn])
elif self.__options['hessian_approximation'] == 'exact':
hess_approx = 'exact'
# create sqp solver
prob['lbg'] = self.__lbg
prob['ubg'] = self.__ubg
sqp_opts = {
'hessian_approximation': hess_approx,
'max_iter': self.__options['max_iter']
}
self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts)
def step(self, x0):
""" Compute periodic MPC feedback control for given initial condition.
"""
# reset periodic indexing if necessary
self.__index = self.__index % len(self.__ref)
# update nlp parameters
p0 = self.__p(0.0)
p0['x0'] = x0
if self.__type == 'economic':
p0['xN'] = self.__ref[self.__index][-x0.shape[0]:]
elif self.__type == 'tracking':
p0['wref'] = self.__ref[self.__index]
p0['tuning', 'H'] = self.__Href[self.__index]
p0['tuning', 'q'] = self.__qref[self.__index]
# pre-solve NLP with IPOPT for globalization
if self.__options['ipopt_presolve']:
ipopt_sol = self.__solver(x0=self.__w0,
lbg=self.__lbg,
ubg=self.__ubg,
p=p0)
self.__w0 = self.__w(ipopt_sol['x'])
self.__lam_g0 = self.__g(ipopt_sol['lam_g'])
# solve NLP
sol = self.__sqp_solver.solve(self.__w0.cat, p0.cat, self.__lam_g0.cat)
# store solution
self.__g_sol = self.__g(self.__g_fun(sol['x'], p0))
self.__w_sol = self.__w(sol['x'])
self.__extract_solver_stats()
# shift reference
self.__index += 1
# update initial guess
self.__w0, self.__lam_g0 = self.__shift_initial_guess(
self.__w_sol, self.__g(sol['lam_g']))
return self.__w_sol['u', 0]
def step_acados(self, x0):
# reset periodic indexing if necessary
self.__index_acados = self.__index_acados % self.__Nref
# format x0
x0 = np.squeeze(x0.full())
# update NLP parameters
self.__acados_ocp_solver.set(0, "lbx", x0)
self.__acados_ocp_solver.set(0, "ubx", x0)
# update reference and tuning matrices
self.__set_acados_reference()
# solve
status = self.__acados_ocp_solver.solve()
# timings
# np.append(self.__acados_times, self.__acados_ocp_solver.get_stats("time_tot"))
print("acados timings: total: ", self.__acados_ocp_solver.get_stats("time_tot"), \
" lin: ", self.__acados_ocp_solver.get_stats("time_lin"), \
" sim: ", self.__acados_ocp_solver.get_stats("time_sim"), " qp: ", \
self.__acados_ocp_solver.get_stats("time_qp"))
# if status != 0:
# raise Exception('acados solver returned status {}. Exiting.'.format(status))
# save solution
self.__w_sol_acados = self.__w(0.0)
for i in range(self.__N):
self.__w_sol_acados['x', i] = self.__acados_ocp_solver.get(i, "x")
self.__w_sol_acados['u', i] = self.__acados_ocp_solver.get(
i, "u")[:self.__nu]
if 'us' in self.__vars:
self.__w_sol_acados['us', i] = self.__acados_ocp_solver.get(
i, "u")[self.__nu:]
self.__w_sol_acados['x', self.__N] = self.__acados_ocp_solver.get(
self.__N, "x")
self.__extract_acados_solver_stats()
# feedback policy
u0 = self.__acados_ocp_solver.get(0, "u")[:self.__nu]
# update initial guess
self.__shift_initial_guess_acados()
# shift index
self.__index_acados += 1
return u0
def generate(self, dae=None, quad=None, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# sampling time
self.__ts = opts['tf'] / self.__N
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
if dae is None:
model.f_expl_expr = self.__F(x0=model.x,
p=model.u)['xf'] / self.__ts
opts['integrator_type'] = 'ERK'
opts['sim_method_num_stages'] = 1
opts['sim_method_num_steps'] = 1
else:
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] in ['IRK', 'GNSF']:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu],
z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.'
)
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
if quad is None:
model.cost_expr_ext_cost = self.__cost(
model.x, model.u[:self.__nu]) / self.__ts
else:
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
if 'integrator_type' in opts and opts['integrator_type'] == 'GNSF':
from acados_template import acados_dae_model_json_dump
import os
acados_dae_model_json_dump(model)
# Set up Octave to be able to run the following:
## if using a virtual python env, the following lines can be added to the env/bin/activate script:
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/
# echo
# echo "OCTAVE_PATH=$OCTAVE_PATH"
status = os.system(
"octave --eval \"convert({})\"".format("\'" + model.name +
"_acados_dae.json\'"))
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
import json
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
if quad is not None:
ocp.solver_options.cost_discretization = 'INTEGRATOR'
elif self.__type == 'tracking':
# set weighting matrices
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# if 'custom_hessian' in opts:
# self.__custom_hessian = opts['custom_hessian']
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
ocp.cost.zu = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
if hasattr(ocp.solver_options, option):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator
def __create_reference(self, wref, tuning, lam_g_ref):
""" Create periodic reference and tuning data.
"""
# period of reference
self.__Nref = len(wref['u'])
# create reference and tuning sequence
# for each starting point in period
ref_pr = []
ref_du = []
ref_du_struct = []
H = []
q = []
for k in range(self.__Nref):
# reference primal solution
refk = []
for j in range(self.__N):
refk += [
wref['x', (k + j) % self.__Nref],
wref['u', (k + j) % self.__Nref]
]
if 'us' in self.__vars:
refk += [wref['us', (k + j) % self.__Nref]]
refk.append(wref['x', (k + self.__N) % self.__Nref])
# reference dual solution
lamgk = self.__g(0.0)
lamgk['init'] = -lam_g_ref['dyn', (k - 1) % self.__Nref]
for j in range(self.__N):
lamgk['dyn', j] = lam_g_ref['dyn', (k + j) % self.__Nref]
if 'g' in list(lamgk.keys()):
lamgk['g', j] = lam_g_ref['g', (k + j) % self.__Nref]
if 'h' in list(lamgk.keys()):
lam_h = [lam_g_ref['h', (k + j) % self.__Nref]]
if 'usc' in self.__vars:
lam_h += [-self.__scost] # TODO not entirely correct
lamgk['h', j] = ct.vertcat(*lam_h)
lamgk['term'] = self.__p_operator(
lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref])
# adjust dual solution of terminal constraint is projected
if self.__nx_term != self.__nx:
# find new terminal multiplier
A_m = []
b_m = []
A_factor = ca.DM.eye(self.__nx)
for j in range(self.__N):
A_m.append(
ct.mtimes(
ct.mtimes(
self.__S['B'][(self.__N - j - 1) %
self.__Nref].T, A_factor),
self.__jac_p_operator(ca.DM.ones(self.__nx, 1)).T))
b_m.append(
ct.mtimes(
ct.mtimes(
self.__S['B'][(self.__N - j - 1) %
self.__Nref].T, A_factor),
lam_g_ref['dyn',
(k + self.__N - 1) % self.__Nref]))
A_factor = ct.mtimes(
self.__S['A'][(self.__N - j - 1) % self.__Nref].T,
A_factor)
A_m = ct.vertcat(*A_m)
b_m = ct.vertcat(*b_m)
LI_indeces = [
] # indeces of first full rank number linearly independent rows
R0 = 0
for i in range(A_m.shape[0]):
R = np.linalg.matrix_rank(A_m[LI_indeces + [i], :])
if R > R0:
LI_indeces.append(i)
R0 = R
lamgk['term'] = ca.solve(A_m[LI_indeces, :],
b_m[LI_indeces, :])
# recursively update dynamics multipliers
delta_lam = -lam_g_ref['dyn', (k + self.__N - 1) %
self.__Nref] + ct.mtimes(
self.__jac_p_operator(
ca.DM.ones(self.__nx, 1)).T,
lamgk['term'])
lamgk['dyn', self.__N - 1] += delta_lam
for j in range(1, self.__N + 1):
delta_lam = ct.mtimes(
self.__S['A'][(self.__N - j) % self.__Nref].T,
delta_lam)
if j < self.__N:
lamgk['dyn', self.__N - 1 - j] += delta_lam
else:
lamgk['init'] += -delta_lam
ref_pr.append(ct.vertcat(*refk))
ref_du.append(lamgk.cat)
ref_du_struct.append(lamgk)
if tuning is not None:
H.append([
tuning['H'][(k + j) % self.__Nref] for j in range(self.__N)
])
q.append([
tuning['q'][(k + j) % self.__Nref] for j in range(self.__N)
])
self.__ref = ref_pr
self.__ref_du = ref_du
self.__ref_du_struct = ref_du_struct
self.__Href = H
self.__qref = q
return None
def __initialize_log(self):
self.__log = {
'cpu': [],
'iter': [],
'f': [],
'status': [],
'sol_x': [],
'lam_x': [],
'lam_g': [],
'u0': [],
'nACtot': [],
'nAC': [],
'idx_AC': [],
'nAS': []
}
self.__log_acados = {
'time_tot': [],
'time_lin': [],
'time_sim': [],
'time_qp': [],
'sqp_iter': [],
'time_reg': [],
'time_qp_xcond': [],
'time_qp_solver_call': [],
}
return None
def __extract_solver_stats(self):
info = self.__sqp_solver.stats
self.__log['cpu'].append(info['t_wall_total'])
self.__log['iter'].append(info['iter_count'])
self.__log['status'].append(info['return_status'])
self.__log['sol_x'].append(info['x'])
self.__log['lam_g'].append(info['lam_g'])
self.__log['f'].append(info['f'])
self.__log['u0'].append(self.__w(info['x'])['u', 0])
self.__log['nACtot'].append(info['nAC'])
nAC, idx_AC = self.__detect_AC(self.__g(info['lam_g']))
self.__log['nAC'].append(nAC)
self.__log['idx_AC'].append(nAC)
self.__log['nAS'].append(info['nAS'])
return None
def __extract_acados_solver_stats(self):
for key in list(self.__log_acados.keys()):
self.__log_acados[key].append(
self.__acados_ocp_solver.get_stats(key))
return None
def __detect_AC(self, lam_g_opt):
# optimal active set
if 'h' in lam_g_opt.keys():
idx_opt = [
k for k in range(self.__h.size1_out(0) - self.__nsc)
if lam_g_opt['h', 0][k] != 0
]
lam_g_ref = self.__g(self.__ref_du[self.__index])
idx_ref = [
k for k in range(self.__h.size1_out(0) - self.__nsc)
if lam_g_ref['h', 0][k] != 0
]
else:
idx_opt = []
idx_ref = []
# get number of active set changes
nAC = len([k for k in idx_opt if k not in idx_ref])
nAC += len([k for k in idx_ref if k not in idx_opt])
return nAC, idx_opt
def reset(self):
self.__index = 0
self.__index_acados = 0
self.__initialize_log()
self.__set_initial_guess()
return None
def __shift_initial_guess(self, w0, lam_g0):
w_shifted = self.__w(0.0)
lam_g_shifted = self.__g(0.0)
lam_g_shifted['init'] = lam_g0['dyn', 0]
# shift states and controls
for i in range(self.__N):
# shift primal solution
w_shifted['x', i] = w0['x', i + 1]
if i < self.__N - 1:
w_shifted['u', i] = w0['u', i + 1]
if 'us' in self.__vars:
w_shifted['us', i] = w0['us', i + 1]
if 'usc' in self.__vars:
w_shifted['usc', i] = w0['usc', i + 1]
# shift dual solution
lam_g_shifted['dyn', i] = lam_g0['dyn', i + 1]
for constr in ['g', 'h']:
if constr in lam_g0.keys():
lam_g_shifted[constr, i] = lam_g0[constr, i + 1]
# copy final interval
w_shifted['x', self.__N] = w_shifted['x', self.__N - 1]
w_shifted['u', self.__N - 1] = w_shifted['u', self.__N - 2]
if 'us' in self.__vars:
w_shifted['us', self.__N - 1] = w_shifted['us', self.__N - 2]
if 'usc' in self.__vars:
w_shifted['usc', self.__N - 1] = w_shifted['usc', self.__N - 2]
lam_g_shifted['dyn', self.__N - 1] = lam_g_shifted['dyn', self.__N - 2]
for constr in ['g', 'h']:
if constr in lam_g0.keys():
lam_g_shifted[constr,
self.__N - 1] = lam_g_shifted[constr,
self.__N - 2]
lam_g_shifted['term'] = lam_g0['term']
return w_shifted, lam_g_shifted
def __shift_initial_guess_acados(self):
for i in range(self.__N):
x_prev = np.squeeze(self.__w_sol_acados['x', i + 1].full(), axis=1)
self.__acados_ocp_solver.set(i, "x", x_prev)
if i < self.__N - 1:
u_prev = np.squeeze(self.__w_sol_acados['u', i + 1].full(),
axis=1)
if 'us' in self.__vars:
u_prev = np.squeeze(ct.vertcat(
u_prev, self.__w_sol_acados['us', i + 1]).full(),
axis=1)
self.__acados_ocp_solver.set(i, "u", u_prev)
# initial guess in terminal stage on periodic trajectory
idx = (self.__index_acados + self.__N) % self.__Nref
# reference
xref = np.squeeze(self.__ref[(idx + 1) % self.__Nref][:self.__nx],
axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
self.__acados_ocp_solver.set(self.__N, "x", xref)
self.__acados_ocp_solver.set(self.__N - 1, "u", uref)
return None
def __set_initial_guess(self):
# create initial guess at steady state
wref = self.__wref(self.__ref[self.__index])
w0 = self.__w(0.0)
w0['x'] = wref['x']
w0['u'] = wref['u']
if 'us' in self.__vars:
w0['us'] = wref['us']
self.__w0 = w0
# initial guess for multipliers
self.__lam_g0 = self.__g(self.__ref_du[self.__index])
# acados solver initialization at reference
if self.__acados_ocp_solver is not None:
self.__set_acados_initial_guess()
return None
def __set_acados_reference(self):
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
if self.__type == 'tracking':
# construct output reference with gradient term
yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(
np.linalg.inv(self.__Href[idx][0]/self.__ts), # inverse of weighting matrix
self.__qref[idx][0].T).full()/self.__ts, # gradient term
axis = 1
)
self.__acados_ocp_solver.set(i, 'yref', yref)
# update tuning matrix
self.__acados_ocp_solver.cost_set(
i, 'W', self.__Href[idx][0] / self.__ts)
# set custom hessians if applicable
# if self.__acados_ocp_solver.acados_ocp.solver_options.ext_cost_custom_hessian:
# self.__acados_ocp_solver.cost_set(i, "cost_custom_hess", self.__custom_hessian[idx])
# update constraint bounds
if self.__h is not None:
C = self.__S['C'][idx][:, :self.__nx]
D = self.__S['C'][idx][:, self.__nx:]
lg = -self.__S['e'][idx] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ug = 1e8 - self.__S['e'][idx] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
# remove constraints that depend on states only from first shooting node
if i == 0:
for k in range(D.shape[0]):
if k in self.__h_us_idx + self.__h_x_idx:
lg[k] += -1e8
self.__acados_ocp_solver.constraints_set(
i, 'lg', np.squeeze(lg, axis=1))
self.__acados_ocp_solver.constraints_set(
i, 'ug', np.squeeze(ug, axis=1))
# update terminal constraint
idx = (self.__index_acados + self.__N) % self.__Nref
x_term = np.squeeze(self.__p_operator(self.__ref[idx][:self.__nx]),
axis=1)
self.__acados_ocp_solver.set(self.__N, 'lbx', x_term)
self.__acados_ocp_solver.set(self.__N, 'ubx', x_term)
return None
def __set_acados_initial_guess(self):
# dual reference solution
ref_dual = self.__ref_du_struct[self.__index_acados % self.__Nref]
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# initialize at reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
# set initial guess
self.__acados_ocp_solver.set(i, "x", xref)
self.__acados_ocp_solver.set(i, "u", uref)
# set dual initial guess
self.__acados_ocp_solver.set(i, "pi",
np.squeeze(ref_dual['dyn', i].full()))
# the inequalities are internally organized in the following order:
# [ lbu lbx lg lh ubu ubx ug uh ]
lam_h = []
t = []
if i == 0:
lam_x0 = copy.deepcopy(ref_dual['init'])
if 'h' in list(ref_dual.keys()):
lam_lh0 = -ref_dual['h', i][:ref_dual['h', i].shape[0] -
self.__nsc]
t_lh0 = copy.deepcopy(self.__S['e'][idx % self.__Nref])
if i == 0:
# set unused constraints at i=0 to be inactive
C = self.__S['C'][idx][:, :self.__nx]
D = self.__S['C'][idx][:, self.__nx:]
for k in range(D.shape[0]):
if k in self.__h_us_idx + self.__h_x_idx:
lam_x0 += -ct.mtimes(lam_lh0[k], C[k, :])
lam_lh0[k] = 0.0
t_lh0[k] += 1e8
lam_lx0 = -copy.deepcopy(lam_x0)
for k in range(self.__nx):
if lam_lx0[k] < 0.0:
lam_lx0[k] = 0.0 # assign multiplier to upper bound
lam_h.append(lam_lx0) # lbx_0
t.append(np.zeros((self.__nx, )))
if 'h' in list(ref_dual.keys()):
if i == 0:
lam_lh = lam_lh0
t_lh = t_lh0
else:
lam_lh = -ref_dual['h', i][:ref_dual['h', i].shape[0] -
self.__nsc]
t_lh = copy.deepcopy(self.__S['e'][idx % self.__Nref])
lam_h.append(lam_lh) # lg
t.append(t_lh)
if 'g' in list(ref_dual.keys()):
lam_lg0 = -ref_dual['g', i]
lam_ug0 = np.zeros(lam_lg0.shape)
for k in range(lam_lg0.shape[0]):
if lam_lg0[k] < 0.0:
lam_ug0[k] = -lam_lg0[k]
lam_lg0[k] = 0.0
lam_h.append(lam_lg0) # lh
t.append(np.zeros((ref_dual['g', i].shape[0], )))
if i == 0:
lam_ux0 = copy.deepcopy(lam_x0)
for k in range(self.__nx):
if lam_ux0[k] < 0.0:
lam_ux0[k] = 0.0 # assign multiplier to lower bound
lam_h.append(lam_ux0) # ubx_0
t.append(np.zeros((self.__nx, )))
if 'h' in list(ref_dual.keys()):
lam_h.append(
np.zeros((ref_dual['h', i].shape[0] - self.__nsc, ))) # ug
t.append(1e8 *
np.ones((ref_dual['h', i].shape[0] - self.__nsc, 1)) -
self.__S['e'][idx])
if 'g' in list(ref_dual.keys()):
lam_h.append(lam_ug0) # uh
t.append(np.zeros((ref_dual['g', i].shape[0], )))
if self.__nsc > 0:
lam_sl = self.__scost - ct.mtimes(lam_lh.T, self.__Jsg).T
lam_h.append(lam_sl) # ls
lam_h.append(self.__scost) # us
t.append(np.zeros((self.__nsc, ))) # slg > 0
t.append(np.zeros((self.__nsc, ))) # sug > 0
if len(lam_h) != 0:
self.__acados_ocp_solver.set(
i, "lam", np.squeeze(ct.vertcat(*lam_h).full()))
self.__acados_ocp_solver.set(i, "t",
np.squeeze(ct.vertcat(*t).full()))
# terminal state
idx = (self.__index_acados + self.__N) % self.__Nref
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
self.__acados_ocp_solver.set(self.__N, "x", xref)
# terminal multipliers
lam_lterm = -ref_dual['term']
lam_uterm = np.zeros((ref_dual['term'].shape[0], ))
for k in range(lam_lterm.shape[0]):
if lam_lterm[k] < 0.0:
lam_uterm[k] = -lam_lterm[k]
lam_lterm[k] = 0.0
lam_term = np.squeeze(ct.vertcat(lam_lterm, lam_uterm).full())
self.__acados_ocp_solver.set(self.__N, "lam", lam_term)
return None
def __detect_state_dependent_constraints(self):
""" Detect which nonlinear equalities depend on states but not on controls.
"""
g_nl = self.__gnl(self.__vars['x'], self.__vars['u'],
self.__vars['us'])
self.__gnl_x_idx = []
for i in range(g_nl.shape[0]):
if not True in ca.which_depends(g_nl[i], self.__vars['u'], 1):
self.__gnl_x_idx.append(i)
self.__h_us_idx = [
idx + self.__h.size1_out(0) - self.__ns for idx in self.__gnl_x_idx
]
return None
@property
def w(self):
return self.__w
@property
def g_sol(self):
return self.__g_sol
@property
def w_sol(self):
return self.__w_sol
@property
def log(self):
return self.__log
@property
def log_acados(self):
return self.__log_acados
@property
def index(self):
return self.__index
@property
def acados_ocp_solver(self):
return self.__acados_ocp_solver
@property
def acados_integrator(self):
return self.__acados_integrator
@property
def w_sol_acados(self):
return self.__w_sol_acados
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 Pmpc(object):
def __init__(self,
N,
sys,
cost,
wref=None,
tuning=None,
lam_g_ref=None,
options={}):
""" Constructor
"""
# store construction data
self.__N = N
self.__vars = sys['vars']
self.__nx = sys['vars']['x'].shape[0]
self.__nu = sys['vars']['u'].shape[0]
# nonlinear inequalities slacks
if 'us' in sys['vars']:
self.__ns = sys['vars']['us'].shape[0]
else:
self.__ns = 0
# mpc slacks
if 'usc' in sys['vars']:
self.__nsc = sys['vars']['usc'].shape[0]
self.__scost = sys['scost']
else:
self.__nsc = 0
# store system dynamics
self.__F = sys['f']
# store path constraints
if 'h' in sys:
self.__h = sys['h']
else:
self.__h = None
# store slacked nonlinear inequality constraints
if 'g' in sys:
self.__gnl = sys['g']
else:
self.__gnl = None
# store system sensitivities around steady state
self.__S = sys['S']
self.__cost = cost
# set options
self.__options = self.__default_options()
for option in options:
if option in self.__options:
self.__options[option] = options[option]
else:
raise ValueError(
'Unknown option for Pmpc class instance: "{}"'.format(
option))
# detect cost-type
if self.__cost.n_in() == 2:
# cost function of the form: l(x,u)
self.__type = 'economic'
# no tuning required
tuning = None
if self.__options['hessian_approximation'] == 'gauss_newton':
self.__options['hessian_approximation'] = 'exact'
Logger.logger.warning(
'Gauss-Newton Hessian approximation cannot be applied for economic MPC problem. Switched to exact Hessian.'
)
else:
# cost function of the form: (w-wref)'*H*(w-wref) + q'w
self.__type = 'tracking'
# check if tuning matrices are provided
assert tuning != None, 'Provide tuning matrices for tracking MPC!'
# periodicity operator
self.__p_operator = self.__options['p_operator']
# construct MPC solver
self.__construct_solver()
# periodic indexing
self.__index = 0
self.__index_acados = 0
# create periodic reference
assert wref != None, 'Provide reference trajectory!'
self.__create_reference(wref, tuning, lam_g_ref)
# initialize log
self.__initialize_log()
# initialize acados solvers
self.__acados_ocp_solver = None
self.__acados_integrator = None
# solver initial guess
self.__set_initial_guess()
return None
def __default_options(self):
# default options
opts = {
'hessian_approximation':
'exact',
'ipopt_presolve':
False,
'max_iter':
2000,
'p_operator':
ca.Function('p_operator', [self.__vars['x']], [self.__vars['x']]),
'slack_flag':
'none'
}
return opts
def __construct_solver(self):
""" Construct periodic MPC solver
"""
# system variables and dimensions
x = self.__vars['x']
u = self.__vars['u']
# NLP parameters
if self.__type == 'economic':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, 1)),
ct.entry('xN', shape=(self.__nx, 1))
])
# reassign for brevity
x0 = self.__p['x0']
xN = self.__p['xN']
if self.__type == 'tracking':
ref_vars = (ct.entry('x', shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
ref_vars += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
# reference trajectory
wref = ct.struct_symMX([ref_vars])
nw = self.__nx + self.__nu + self.__ns
tuning = ct.struct_symMX([ # tracking tuning
ct.entry('H', shape=(nw, nw), repeat=self.__N),
ct.entry('q', shape=(nw, 1), repeat=self.__N)
])
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, )),
ct.entry('wref', struct=wref),
ct.entry('tuning', struct=tuning)
])
# reassign for brevity
x0 = self.__p['x0']
wref = self.__p.prefix['wref']
tuning = self.__p.prefix['tuning']
xN = wref['x', -1]
# NLP variables
variables_entry = (ct.entry('x',
shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
variables_entry += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
self.__wref = ct.struct_symMX([variables_entry
]) # structure of reference
if 'usc' in self.__vars:
variables_entry += (ct.entry('usc',
shape=(self.__nsc, ),
repeat=self.__N), )
# nlp variables + bounds
w = ct.struct_symMX([variables_entry])
# variable bounds are implemented as inequalities
self.__lbw = w(-np.inf)
self.__ubw = w(np.inf)
# prepare dynamics and path constraints entry
constraints_entry = (ct.entry('dyn',
shape=(self.__nx, ),
repeat=self.__N), )
if self.__gnl is not None:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
if self.__h is not None:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
# terminal constraint
nx_term = self.__p_operator.size1_out(0)
# create general constraints structure
g_struct = ct.struct_symMX([
ct.entry('init', shape=(self.__nx, 1)), constraints_entry,
ct.entry('term', shape=(nx_term, 1))
])
# create symbolic constraint expressions
map_args = collections.OrderedDict()
map_args['x0'] = ct.horzcat(*w['x', :-1])
map_args['p'] = ct.horzcat(*w['u'])
F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf'])
# generate constraints
constr = collections.OrderedDict()
constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])]
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*w['us'])
if self.__gnl is not None:
constr['g'] = ct.horzsplit(
self.__gnl.map(self.__N)(*map_args.values()))
if 'usc' in self.__vars:
map_args['usc'] = ct.horzcat(*w['usc'])
if self.__h is not None:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N)(*map_args.values()))
repeated_constr = list(
itertools.chain.from_iterable(zip(*constr.values())))
term_constraint = self.__p_operator(w['x', -1] - xN)
self.__g = g_struct(
ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint))
self.__lbg = g_struct(np.zeros(self.__g.shape))
self.__ubg = g_struct(np.zeros(self.__g.shape))
if self.__h is not None:
self.__ubg['h', :] = np.inf
# nlp cost
cost_map = self.__cost.map(self.__N)
if self.__type == 'economic':
cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])]
elif self.__type == 'tracking':
if self.__ns != 0:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k], w['us', k])
for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k], wref['us', k])
for k in range(self.__N)
])
else:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k])
for k in range(self.__N)
])
cost_args = [
cost_args_w, cost_args_w_ref,
ct.horzcat(*tuning['H']),
ct.horzcat(*tuning['q'])
]
if self.__options['hessian_approximation'] == 'gauss_newton':
if 'usc' not in self.__vars:
hess_gn = ct.diagcat(*tuning['H'],
ca.DM.zeros(self.__nx, self.__nx))
else:
hess_block = list(
itertools.chain.from_iterable(
zip(tuning['H'],
[ca.DM.zeros(self.__nsc, self.__nsc)] *
self.__N)))
hess_gn = ct.diagcat(*hess_block,
ca.DM.zeros(self.__nx, self.__nx))
J = ca.sum2(cost_map(*cost_args))
# add cost on slacks
if 'usc' in self.__vars:
J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc'])))
# create solver
prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p}
self.__w = w
self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g])
# create IPOPT-solver instance if needed
if self.__options['ipopt_presolve']:
opts = {
'ipopt': {
'linear_solver': 'ma57',
'print_level': 0
},
'expand': False
}
if Logger.logger.getEffectiveLevel() > 10:
opts['ipopt']['print_level'] = 0
opts['print_time'] = 0
opts['ipopt']['sb'] = 'yes'
self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts)
# create hessian approximation function
if self.__options['hessian_approximation'] == 'gauss_newton':
lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used
hess_approx = ca.Function('hess_approx',
[self.__w, self.__p, lam_g], [hess_gn])
elif self.__options['hessian_approximation'] == 'exact':
hess_approx = 'exact'
# create sqp solver
prob['lbg'] = self.__lbg
prob['ubg'] = self.__ubg
sqp_opts = {
'hessian_approximation': hess_approx,
'max_iter': self.__options['max_iter']
}
self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts)
def step(self, x0):
""" Compute periodic MPC feedback control for given initial condition.
"""
# reset periodic indexing if necessary
self.__index = self.__index % len(self.__ref)
# update nlp parameters
p0 = self.__p(0.0)
p0['x0'] = x0
if self.__type == 'economic':
p0['xN'] = self.__ref[self.__index][-x0.shape[0]:]
elif self.__type == 'tracking':
p0['wref'] = self.__ref[self.__index]
p0['tuning', 'H'] = self.__Href[self.__index]
p0['tuning', 'q'] = self.__qref[self.__index]
# pre-solve NLP with IPOPT for globalization
if self.__options['ipopt_presolve']:
ipopt_sol = self.__solver(x0=self.__w0,
lbg=self.__lbg,
ubg=self.__ubg,
p=p0)
self.__w0 = self.__w(ipopt_sol['x'])
self.__lam_g0 = self.__g(ipopt_sol['lam_g'])
# solve NLP
sol = self.__sqp_solver.solve(self.__w0.cat, p0.cat, self.__lam_g0.cat)
# store solution
self.__g_sol = self.__g(self.__g_fun(sol['x'], p0))
self.__w_sol = self.__w(sol['x'])
self.__extract_solver_stats()
# shift reference
self.__index += 1
# update initial guess
self.__w0, self.__lam_g0 = self.__shift_initial_guess(
self.__w_sol, self.__g(sol['lam_g']))
return self.__w_sol['u', 0]
def step_acados(self, x0):
# reset periodic indexing if necessary
self.__index_acados = self.__index_acados % self.__Nref
# format x0
x0 = np.squeeze(x0.full())
# update NLP parameters
self.__acados_ocp_solver.set(0, "lbx", x0)
self.__acados_ocp_solver.set(0, "ubx", x0)
# update reference and tuning matrices
self.__set_acados_reference()
# solve
status = self.__acados_ocp_solver.solve()
# if status != 0:
# raise Exception('acados solver returned status {}. Exiting.'.format(status))
# save solution
self.__w_sol_acados = self.__w(0.0)
for i in range(self.__N):
self.__w_sol_acados['x', i] = self.__acados_ocp_solver.get(i, "x")
self.__w_sol_acados['u', i] = self.__acados_ocp_solver.get(
i, "u")[:self.__nu]
if 'us' in self.__vars:
self.__w_sol_acados['us', i] = self.__acados_ocp_solver.get(
i, "u")[self.__nu:]
self.__w_sol_acados['x', self.__N] = self.__acados_ocp_solver.get(
self.__N, "x")
# feedback policy
u0 = self.__acados_ocp_solver.get(0, "u")[:self.__nu]
# update initial guess
self.__shift_initial_guess_acados()
# shift index
self.__index_acados += 1
return u0
def generate(self, dae, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] == 'IRK':
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu], z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.')
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
else:
# set weighting matrices
if self.__type == 'tracking':
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(), axis=1)
ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1)
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator
def __create_reference(self, wref, tuning, lam_g_ref):
""" Create periodic reference and tuning data.
"""
# period of reference
self.__Nref = len(wref['u'])
# create reference and tuning sequence
# for each starting point in period
ref_pr = []
ref_du = []
ref_du_struct = []
H = []
q = []
for k in range(self.__Nref):
# reference primal solution
refk = []
for j in range(self.__N):
refk += [
wref['x', (k + j) % self.__Nref],
wref['u', (k + j) % self.__Nref]
]
if 'us' in self.__vars:
refk += [wref['us', (k + j) % self.__Nref]]
refk.append(wref['x', (k + self.__N) % self.__Nref])
# reference dual solution
lamgk = self.__g(0.0)
lamgk['init'] = -lam_g_ref['dyn', (k - 1) % self.__Nref]
for j in range(self.__N):
lamgk['dyn', j] = lam_g_ref['dyn', (k + j) % self.__Nref]
if 'g' in list(lamgk.keys()):
lamgk['g', j] = lam_g_ref['g', (k + j) % self.__Nref]
if 'h' in list(lamgk.keys()):
lam_h = [lam_g_ref['h', (k + j) % self.__Nref]]
if 'usc' in self.__vars:
lam_h += [-self.__scost] # TODO not entirely correct
lamgk['h', j] = ct.vertcat(*lam_h)
lamgk['term'] = self.__p_operator(
lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref])
ref_pr.append(ct.vertcat(*refk))
ref_du.append(lamgk.cat)
ref_du_struct.append(lamgk)
if tuning is not None:
H.append([
tuning['H'][(k + j) % self.__Nref] for j in range(self.__N)
])
q.append([
tuning['q'][(k + j) % self.__Nref] for j in range(self.__N)
])
self.__ref = ref_pr
self.__ref_du = ref_du
self.__ref_du_struct = ref_du_struct
self.__Href = H
self.__qref = q
return None
def __initialize_log(self):
self.__log = {
'cpu': [],
'iter': [],
'f': [],
'status': [],
'sol_x': [],
'lam_x': [],
'lam_g': [],
'u0': [],
'nACtot': [],
'nAC': [],
'idx_AC': [],
'nAS': []
}
return None
def __extract_solver_stats(self):
info = self.__sqp_solver.stats
self.__log['cpu'].append(info['t_wall_total'])
self.__log['iter'].append(info['iter_count'])
self.__log['status'].append(info['return_status'])
self.__log['sol_x'].append(info['x'])
self.__log['lam_g'].append(info['lam_g'])
self.__log['f'].append(info['f'])
self.__log['u0'].append(self.__w(info['x'])['u', 0])
self.__log['nACtot'].append(info['nAC'])
nAC, idx_AC = self.__detect_AC(self.__g(info['lam_g']))
self.__log['nAC'].append(nAC)
self.__log['idx_AC'].append(nAC)
self.__log['nAS'].append(info['nAS'])
return None
def __detect_AC(self, lam_g_opt):
# optimal active set
if 'h' in lam_g_opt.keys():
idx_opt = [
k for k in range(self.__h.size1_out(0) - self.__nsc)
if lam_g_opt['h', 0][k] != 0
]
lam_g_ref = self.__g(self.__ref_du[self.__index])
idx_ref = [
k for k in range(self.__h.size1_out(0) - self.__nsc)
if lam_g_ref['h', 0][k] != 0
]
else:
idx_opt = []
idx_ref = []
# get number of active set changes
nAC = len([k for k in idx_opt if k not in idx_ref])
nAC += len([k for k in idx_ref if k not in idx_opt])
return nAC, idx_opt
def reset(self):
self.__index = 0
self.__index_acados = 0
self.__initialize_log()
self.__set_initial_guess()
return None
def __shift_initial_guess(self, w0, lam_g0):
w_shifted = self.__w(0.0)
lam_g_shifted = self.__g(0.0)
lam_g_shifted['init'] = lam_g0['dyn', 0]
# shift states and controls
for i in range(self.__N):
# shift primal solution
w_shifted['x', i] = w0['x', i + 1]
if i < self.__N - 1:
w_shifted['u', i] = w0['u', i + 1]
if 'us' in self.__vars:
w_shifted['us', i] = w0['us', i + 1]
if 'usc' in self.__vars:
w_shifted['usc', i] = w0['usc', i + 1]
# shift dual solution
lam_g_shifted['dyn', i] = lam_g0['dyn', i + 1]
for constr in ['g', 'h']:
if constr in lam_g0.keys():
lam_g_shifted[constr, i] = lam_g0[constr, i + 1]
# copy final interval
w_shifted['x', self.__N] = w_shifted['x', self.__N - 1]
w_shifted['u', self.__N - 1] = w_shifted['u', self.__N - 2]
if 'us' in self.__vars:
w_shifted['us', self.__N - 1] = w_shifted['us', self.__N - 2]
if 'usc' in self.__vars:
w_shifted['usc', self.__N - 1] = w_shifted['usc', self.__N - 2]
lam_g_shifted['dyn', self.__N - 1] = lam_g_shifted['dyn', self.__N - 2]
for constr in ['g', 'h']:
if constr in lam_g0.keys():
lam_g_shifted[constr,
self.__N - 1] = lam_g_shifted[constr,
self.__N - 2]
lam_g_shifted['term'] = lam_g0['term']
return w_shifted, lam_g_shifted
def __shift_initial_guess_acados(self):
for i in range(self.__N):
x_prev = np.squeeze(self.__w_sol_acados['x', i + 1].full(), axis=1)
self.__acados_ocp_solver.set(i, "x", x_prev)
if i < self.__N - 1:
u_prev = np.squeeze(self.__w_sol_acados['u', i + 1].full(),
axis=1)
if 'us' in self.__vars:
u_prev = np.squeeze(ct.vertcat(
u_prev, self.__w_sol_acados['us', i + 1]).full(),
axis=1)
self.__acados_ocp_solver.set(i, "u", u_prev)
# initial guess in terminal stage on periodic trajectory
idx = (self.__index_acados + self.__N) % self.__Nref
# reference
xref = np.squeeze(self.__ref[(idx + 1) % self.__Nref][:self.__nx],
axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
self.__acados_ocp_solver.set(self.__N, "x", xref)
self.__acados_ocp_solver.set(self.__N - 1, "u", uref)
return None
def __set_initial_guess(self):
# create initial guess at steady state
wref = self.__wref(self.__ref[self.__index])
w0 = self.__w(0.0)
w0['x'] = wref['x']
w0['u'] = wref['u']
if 'us' in self.__vars:
w0['us'] = wref['us']
self.__w0 = w0
# initial guess for multipliers
self.__lam_g0 = self.__g(self.__ref_du[self.__index])
# acados solver initialization at reference
if self.__acados_ocp_solver is not None:
self.__set_acados_initial_guess()
return None
def __set_acados_reference(self):
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
# construct output reference with gradient term
yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(
np.linalg.inv(self.__Href[idx][0]), # inverse of weighting matrix
self.__qref[idx][0].T).full(), # gradient term
axis = 1
)
self.__acados_ocp_solver.set(i, 'yref', yref)
# update tuning matrix
self.__acados_ocp_solver.cost_set(i, 'W', self.__Href[idx][0])
# update constraint bounds
C = self.__S['C'][idx][:, :self.__nx]
D = self.__S['C'][idx][:, self.__nx:]
lg = -self.__S['e'][idx] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][idx] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
self.__acados_ocp_solver.constraints_set(i, 'lg',
np.squeeze(lg, axis=1))
self.__acados_ocp_solver.constraints_set(i, 'ug',
np.squeeze(ug, axis=1))
# update terminal constraint
idx = (self.__index_acados + self.__N) % self.__Nref
x_term = np.squeeze(self.__p_operator(self.__ref[idx][:self.__nx]),
axis=1)
self.__acados_ocp_solver.set(self.__N, 'lbx', x_term)
self.__acados_ocp_solver.set(self.__N, 'ubx', x_term)
return None
def __set_acados_initial_guess(self):
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# initialize at reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
# set initial guess
self.__acados_ocp_solver.set(i, "x", xref)
self.__acados_ocp_solver.set(i, "u", uref)
# set dual initial guess
ref_dual = self.__ref_du_struct[idx]
self.__acados_ocp_solver.set(i, "pi",
np.squeeze(ref_dual['dyn', i].full()))
# the inequalities are internally organized in the following order:
# [ lbu lbx lg lh ubu ubx ug uh ]
lam_h = []
t = []
if i == 0:
lam_h.append(ref_dual['init']) # lbx_0
t.append(np.zeros((self.__nx, )))
if 'h' in list(ref_dual.keys()):
lam_lh = -ref_dual['h',
0][:ref_dual['h', 0].shape[0] - self.__nsc]
lam_h.append(lam_lh) # lg
t.append(self.__S['e'][idx % self.__Nref])
if 'g' in list(ref_dual.keys()):
lam_h.append(ref_dual['g', 0]) # lh
t.append(np.zeros((ref_dual['g', 0].shape[0], )))
if i == 0:
lam_h.append(np.zeros((self.__nx, ))) # ubx_0
t.append(np.zeros((self.__nx, )))
if 'h' in list(ref_dual.keys()):
lam_h.append(
np.zeros((ref_dual['h', 0].shape[0] - self.__nsc, ))) # ug
t.append(1e8 *
np.ones((ref_dual['h', 0].shape[0] - self.__nsc, 1)) -
self.__S['e'][idx])
if 'g' in list(ref_dual.keys()):
lam_h.append(np.zeros((ref_dual['g', 0].shape[0], ))) # uh
t.append(np.zeros((ref_dual['g', 0].shape[0], )))
if self.__nsc > 0:
lam_sl = self.__scost - ct.mtimes(lam_lh.T, self.__Jsg).T
lam_h.append(lam_sl) # ls
lam_h.append(self.__scost) # us
t.append(np.zeros((self.__nsc, ))) # slg > 0
t.append(np.zeros((self.__nsc, ))) # sug > 0
self.__acados_ocp_solver.set(i, "lam",
np.squeeze(ct.vertcat(*lam_h).full()))
self.__acados_ocp_solver.set(i, "t",
np.squeeze(ct.vertcat(*t).full()))
# terminal state
idx = (self.__index_acados + self.__N) % self.__Nref
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
self.__acados_ocp_solver.set(self.__N, "x", xref)
# terminal multipliers
lam_term = np.squeeze(
ct.vertcat(ref_dual['term'], np.zeros(
(ref_dual['term'].shape[0], ))).full())
self.__acados_ocp_solver.set(self.__N, "lam", lam_term)
return None
@property
def w(self):
return self.__w
@property
def g_sol(self):
return self.__g_sol
@property
def w_sol(self):
return self.__w_sol
@property
def log(self):
return self.__log
@property
def index(self):
return self.__index
@property
def acados_ocp_solver(self):
return self.__acados_ocp_solver
@property
def acados_integrator(self):
return self.__acados_integrator
@property
def w_sol_acados(self):
return self.__w_sol_acados