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):
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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")
Ejemplo n.º 5
0
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