diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 7facbb3ad2..7cae6b5344 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -188,6 +188,14 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_python/furuta_pendulum python main_closed_loop.py + - name: Python evaluator test + working-directory: ${{runner.workspace}}/acados/build + shell: bash + run: | + source ${{runner.workspace}}/acados/acadosenv/bin/activate + cd ${{runner.workspace}}/acados/examples/acados_python/evaluation + python minimal_example_evaluation.py + MATLAB_test: needs: core_build runs-on: ubuntu-22.04 diff --git a/examples/acados_python/evaluation/minimal_example_evaluation.py b/examples/acados_python/evaluation/minimal_example_evaluation.py new file mode 100644 index 0000000000..e05a25f573 --- /dev/null +++ b/examples/acados_python/evaluation/minimal_example_evaluation.py @@ -0,0 +1,295 @@ +# -*- coding: future_fstrings -*- +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# +import sys + +sys.path.insert(0, '../getting_started') + +from matplotlib import pyplot as plt + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver, AcadosSim, ACADOS_INFTY +from acados_template.mpc_utils import AcadosCostConstraintEvaluator +from pendulum_model import export_pendulum_ode_model +from utils_eval import plot_pendulum_eval +import numpy as np +import scipy.linalg +import math +from casadi import vertcat, SX + +# Define constraints to make evaluation more challenging +constraint_par = {'omega_dot_min_1': -4, + 'omega_dot_min_2': -6, + 'omega_dot_max_1': 1., + 'omega_dot_max_2': 2., + 'iter_omega_change': 6, + 'v_max': 5, + 'F_max': 80} + + +def setup(x0, N_horizon, Tf, td, RTI=False, parametric_constraints=False): + # create ocp object to formulate the OCP + global constraint_par + ocp = AcadosOcp() + + # set model + ocp.model = model = export_pendulum_ode_model() + + if parametric_constraints: + omega_dot_min = SX.sym('omega_dot_min') + omega_dot_max = SX.sym('omega_dot_max') + + ocp.model.p = omega_dot_min + ocp.model.p_global = omega_dot_max + ocp.model.con_h_expr = vertcat(omega_dot_min - model.x[3], # assuming ub=0, lb=-inf + model.x[3] - omega_dot_max) # assuming ub=0, lb=-inf + ocp.parameter_values = np.array([constraint_par['omega_dot_min_1']]) + ocp.p_global_values = np.array([constraint_par['omega_dot_max_1']]) + + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx + + # set cost module + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + + Q_mat = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2 * np.diag([1e-2]) + + ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) + ocp.cost.W_e = Q_mat * 10 + + ocp.model.cost_y_expr = vertcat(model.x, model.u) + ocp.model.cost_y_expr_e = model.x + ocp.cost.yref = np.zeros((ny,)) + ocp.cost.yref_e = np.zeros((ny_e,)) + + # set constraints + ocp.constraints.lbu = np.array([-constraint_par['F_max']]) + ocp.constraints.ubu = np.array([+constraint_par['F_max']]) + + ocp.constraints.idxbx = np.array([2]) + ocp.constraints.lbx = np.array([-constraint_par['v_max']]) + ocp.constraints.ubx = np.array([constraint_par['v_max']]) + + ocp.constraints.idxbx_e = np.array([2]) + ocp.constraints.lbx_e = np.array([-constraint_par['v_max']]) + ocp.constraints.ubx_e = np.array([constraint_par['v_max']]) + + ocp.constraints.idxsbx = np.array([0]) + ocp.constraints.lsbx = np.zeros((1,)) + ocp.constraints.usbx = np.zeros((1,)) + + if parametric_constraints: + ocp.constraints.uh = np.array([0, 0]) + ocp.constraints.lh = np.array([-ACADOS_INFTY, -ACADOS_INFTY]) + ocp.constraints.idxsh = np.array([0, 1]) + ocp.cost.zu = ocp.cost.zl = 2e3 * np.ones((3,)) + ocp.cost.Zu = ocp.cost.Zl = 5e3 * np.ones((3,)) + else: + ocp.cost.zu = ocp.cost.zl = 2e3 * np.ones((1,)) + ocp.cost.Zu = ocp.cost.Zl = 5e3 * np.ones((1,)) + + ocp.constraints.x0 = x0 + ocp.constraints.idxbu = np.array([0]) + + # set prediction horizon + ocp.solver_options.N_horizon = N_horizon + ocp.solver_options.tf = Tf + + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'IRK' + ocp.solver_options.sim_method_newton_iter = 10 + + if RTI: + ocp.solver_options.nlp_solver_type = 'SQP_RTI' + else: + ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + ocp.solver_options.nlp_solver_max_iter = 150 + + ocp.solver_options.qp_solver_cond_N = N_horizon + + solver_json = 'acados_ocp_' + model.name + '.json' + acados_ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, save_p_global=True) + + # create an integrator with the same settings as used in the OCP solver. + # attention, with p_global, integrator cannot be built from OCP + sim = AcadosSim() + sim.model = export_pendulum_ode_model() + + # set simulation time + sim.solver_options.T = td + # set options + sim.solver_options.integrator_type = 'IRK' + sim.solver_options.num_stages = 3 + sim.solver_options.num_steps = 3 + sim.solver_options.newton_iter = 3 # for implicit integrator + sim.solver_options.collocation_type = "GAUSS_RADAU_IIA" + acados_integrator = AcadosSimSolver(sim) + + acados_evaluator = AcadosCostConstraintEvaluator(ocp, with_parametric_bounds=False) + + return acados_ocp_solver, acados_integrator, acados_evaluator + + +def update_comprehensive_dict(comprehensive_dict, input_dict): + """ + Updates the global comprehensive_dict with the values from the input dictionary. + Values for each key are stored in lists, which are appended to with each call. + + :param input_dict: A dictionary with keys and values to add to the comprehensive_dict. + """ + for key, value in input_dict.items(): + if key not in comprehensive_dict: + comprehensive_dict[key] = [] # Initialize the list if the key doesn't exist + comprehensive_dict[key].append(value) # Append the value to the list + + return comprehensive_dict + + +def main(use_RTI: bool = False, parametric_constraints: bool = True, plot_results: bool = False): + global constraint_par + x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + Tf = .8 + N_horizon = 40 + td = Tf / N_horizon + + ocp_solver, integrator, evaluator = setup( + x0, N_horizon, Tf, + td=Tf / N_horizon, + RTI=use_RTI, + parametric_constraints=parametric_constraints) + + nx = ocp_solver.acados_ocp.dims.nx + nu = ocp_solver.acados_ocp.dims.nu + + Nsim = 100 + simX = np.zeros((Nsim + 1, nx)) + simU = np.zeros((Nsim, nu)) + eval_dict = {} + + simX[0, :] = x0 + + # do some initial iterations to start with a good initial guess + num_iter_initial = 5 + for _ in range(num_iter_initial): + ocp_solver.solve_for_x0(x0_bar=x0) + + evaluator.update_all(ocp_solver) + + # closed loop + for i in range(Nsim): + + # change constraint parameter in the middle of the simulation + if i == constraint_par['iter_omega_change'] and parametric_constraints: + new_omega_dot_min = np.array([constraint_par['omega_dot_min_2']]) + for j in range(ocp_solver.acados_ocp.dims.N): + ocp_solver.set(j, "p", new_omega_dot_min) + ocp_solver.set_p_global_and_precompute_dependencies(np.array([constraint_par['omega_dot_max_2']])) + evaluator.update_all(ocp_solver) + + if use_RTI: + # preparation phase + ocp_solver.options_set('rti_phase', 1) + ocp_solver.solve() + + # set initial state + ocp_solver.set(0, "lbx", simX[i, :]) + ocp_solver.set(0, "ubx", simX[i, :]) + + # feedback phase + ocp_solver.options_set('rti_phase', 2) + ocp_solver.solve() + + simU[i, :] = ocp_solver.get(0, "u") + + else: + # solve ocp and get next control input + simU[i, :] = ocp_solver.solve_for_x0(x0_bar=simX[i, :]) + + # evaluate the cost of the full trajectory + solution_obj = ocp_solver.store_iterate_to_obj() + cost_ext_eval = evaluator.evaluate_ocp_cost(solution_obj) + cost_int_eval = ocp_solver.get_cost() + abs_error = np.abs(cost_ext_eval - cost_int_eval) + + # formatted print relative error up to 3 decimal places + print(f'cost_err_abs: {abs_error:.9f}') + assert math.isclose(cost_ext_eval, cost_int_eval, abs_tol=1e-8, rel_tol=1e-8) + + # simulate system + simX[i + 1, :] = integrator.simulate(x=simX[i, :], u=simU[i, :]) + eval_iter = evaluator.evaluate(x=simX[i, :], u=simU[i, :]) + eval_dict = update_comprehensive_dict(eval_dict, eval_iter) + + # plot results + if not plot_results: + return + model = ocp_solver.acados_ocp.model + + fix, axes = plot_pendulum_eval( + np.linspace(0, td * Nsim, Nsim + 1), + simU, + simX, + eval_dict, + latexify=False, + time_label=model.t_label, + x_labels=model.x_labels, + u_labels=model.u_labels) + + axes[2].axhline(constraint_par['v_max'], alpha=0.7, color='tab:red') + axes[2].axhline(-constraint_par['v_max'], alpha=0.7, color='tab:red') + + if parametric_constraints: + constr_omega_dot_min = np.empty(Nsim) + constr_omega_dot_min[:constraint_par['iter_omega_change'] + 1] = constraint_par['omega_dot_min_1'] + constr_omega_dot_min[constraint_par['iter_omega_change'] + 1:] = constraint_par['omega_dot_min_2'] + constr_omega_dot_max = np.empty(Nsim) + constr_omega_dot_max[:constraint_par['iter_omega_change'] + 1] = constraint_par['omega_dot_max_1'] + constr_omega_dot_max[constraint_par['iter_omega_change'] + 1:] = constraint_par['omega_dot_max_2'] + axes[3].plot(np.linspace(0, td * Nsim, Nsim), constr_omega_dot_min, alpha=0.7, color='tab:red') + axes[3].plot(np.linspace(0, td * Nsim, Nsim), constr_omega_dot_max, alpha=0.7, color='tab:red') + + + axes[-1].set_ylim([-1.2 * constraint_par['F_max'], 1.2 * constraint_par['F_max']]) + axes[-1].axhline(constraint_par['F_max'], alpha=0.7, color='tab:red') + axes[-1].axhline(-constraint_par['F_max'], alpha=0.7, color='tab:red') + plt.show() + + +if __name__ == '__main__': + + for parametric_constraints in [True, False]: + print(f'Parametric_constraints: {parametric_constraints}\n') + main(use_RTI=True, parametric_constraints=parametric_constraints, plot_results=False) diff --git a/examples/acados_python/evaluation/utils_eval.py b/examples/acados_python/evaluation/utils_eval.py new file mode 100644 index 0000000000..29e620690d --- /dev/null +++ b/examples/acados_python/evaluation/utils_eval.py @@ -0,0 +1,82 @@ +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; + +from typing import Dict +import matplotlib.pyplot as plt +import numpy as np +from acados_template import latexify_plot + + +def plot_pendulum_eval(t, U, X_true, eval: Dict, latexify=False, time_label='$t$', x_labels=None, u_labels=None): + """ + Plots pendulum with evaluation of cost function. + Params: + t: time values of the discretization + U: arrray with shape (N_sim-1, nu) or (N_sim, nu) + X_true: arrray with shape (N_sim, nx) + eval: evaluation dictionary + latexify: latex style plots + """ + + if latexify: + latexify_plot() + + nx = X_true.shape[1] + fig, axes = plt.subplots(nx + 2, 1, sharex=True) + + for i in range(nx): + axes[i].plot(t, X_true[:, i]) + axes[i].grid() + if x_labels is not None: + axes[i].set_ylabel(x_labels[i]) + else: + axes[i].set_ylabel(f'$x_{i}$') + + axes[-2].plot(t[:-1], eval['cost_without_slacks'], label='cost w/o slacks') + axes[-2].plot(t[:-1], eval['cost'], label='cost with slacks') + axes[-2].grid() + axes[-2].legend() + axes[-2].set_ylabel('cost') + + axes[-1].step(t, np.append([U[0]], U)) + + if u_labels is not None: + axes[-1].set_ylabel(u_labels[0]) + else: + axes[-1].set_ylabel('$u$') + + axes[-1].set_xlim(t[0], t[-1]) + axes[-1].set_xlabel(time_label) + axes[-1].grid() + + plt.subplots_adjust(left=None, bottom=None, right=None, top=None, hspace=0.4) + + fig.align_ylabels() + + return fig, axes diff --git a/examples/acados_python/getting_started/utils.py b/examples/acados_python/getting_started/utils.py index ad29639d1d..eb2e4ba5f6 100644 --- a/examples/acados_python/getting_started/utils.py +++ b/examples/acados_python/getting_started/utils.py @@ -27,7 +27,6 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # - import matplotlib.pyplot as plt import numpy as np from acados_template import latexify_plot diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index afaf2714db..f626e48d6f 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -191,10 +191,18 @@ def create_cython_solver(cls, json_file): acados_ocp_json['solver_options']['nlp_solver_type'], acados_ocp_json['dims']['N']) + @property + def save_p_global(self) -> bool: + return self.__save_p_global - def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file=None, simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True): + def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file=None, simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True, save_p_global=False): self.solver_created = False + self.__save_p_global = save_p_global + if save_p_global: + self.__p_global_values = acados_ocp.p_global_values + else: + self.__p_global_values = np.array([]) if not (isinstance(acados_ocp, AcadosOcp) or isinstance(acados_ocp, AcadosMultiphaseOcp)): raise Exception('acados_ocp should be of type AcadosOcp or AcadosMultiphaseOcp.') @@ -991,11 +999,19 @@ def get_flat(self, field_: str) -> np.ndarray: """ Get concatenation of all stages of last solution of the solver. - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global'] + + .. note:: The parameter 'p_global' has no stage-wise structure and is processed in a memory saving manner by default. \n + In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n """ - if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: + if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global']: raise Exception(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + if field_ == 'p_global': + if not self.__save_p_global: + raise Exception(f'The field \'{field_}\' is not stored within the solver by default. Please set the option \'save_p_global=True\' when instantiating the solver.') + return self.__p_global_values + field = field_.encode('utf-8') dims = self.__acados_lib.ocp_nlp_dims_get_total_from_attr(self.nlp_config, self.nlp_dims, field) @@ -2235,6 +2251,8 @@ def set_p_global_and_precompute_dependencies(self, data_: np.ndarray): raise Exception(f'data must have length {np_global}, got {data_len}.') status = getattr(self.shared_lib, f"{self.name}_acados_set_p_global_and_precompute_dependencies")(self.capsule, c_data, data_len) + if self.__save_p_global: + self.__p_global_values = data_ return status diff --git a/interfaces/acados_template/acados_template/mpc_utils.py b/interfaces/acados_template/acados_template/mpc_utils.py index 220d3cce52..64d2b8f99a 100644 --- a/interfaces/acados_template/acados_template/mpc_utils.py +++ b/interfaces/acados_template/acados_template/mpc_utils.py @@ -28,63 +28,451 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # - +from array import array from copy import deepcopy -from typing import Tuple +from typing import Tuple, Optional import casadi as ca import numpy as np +from . import AcadosOcpIterate, AcadosOcpSolver from .acados_model import AcadosModel from .acados_ocp import AcadosOcp, AcadosOcpConstraints from .utils import casadi_length, is_empty -def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarray]: +class AcadosCostConstraintEvaluator: """ - Creates a new AcadosModel with an extra state `cost_state`, - which has the dynamics of the cost function and slack penalties corresponding to the intermediate shooting nodes. + This class provides convenience methods to evaluate the cost and constraints related to an AcadosOcp definition. + A typical use case would be a closed-loop evaluation with the same standard costs and slack costs as defined + in the original AcadosOcp. - Note: In contrast cost_discretization='INTEGRATOR', this allows to integrate also the slack penalties. - Since l1 slack penalties are nondifferentiable, an accurate cost integration with the model created by this function should use many integrator steps, when slack penalties are part of the OCP formulation. + The evaluator must be updated by the method 'update_all(acados_solver)', if parameters in the solvers are changed. - Returns the augmented model and the parameter values of the given AcadosOcp. + Two methods can be used for evaluation: + - evaluate(x, u, step): evaluates the cost and constraints at a given stage of the OCP. For a closed-loop evaluation + the stage is typically 0. + - evaluate_ocp_cost(acados_ocp_iterate): evaluates the cost of a whole OCP trajectory, as evaluated inside acados. + + Limitation: values of numerical properties, such as bound values, W, zl, zu, Zu, Zl, yref, + etc. are taken from original AcadosOcp. Their changes during runtime are not taken into account in the evaluator. """ - model = deepcopy(ocp.model) - symbol = model.get_casadi_symbol() - cost_state = symbol("cost_state") - cost_state_dot = symbol("cost_state_dot") + def __init__(self, ocp: AcadosOcp, with_parametric_bounds: bool = False): + ocp.make_consistent() + + if with_parametric_bounds: + raise NotImplementedError( + "AcadosCostConstraintEvaluatator: with_parametric_bounds not implemented.") + # p_bounds = ca.MX.sym('p_bounds', 2*(ocp.dims.nh+ocp.dims.nbx+dims.nbu+dims.ng+dims.nphi), 1) + + self.__ocp = deepcopy(ocp) + + model = ocp.model + constraints = ocp.constraints + dims = ocp.dims + if casadi_length(ocp.model.z) > 0: + raise NotImplementedError( + "AcadosCostConstraintEvaluatator: not implemented for models with z.") + + self.__parameter_values = np.tile(ocp.parameter_values, (ocp.dims.np, ocp.dims.N)) + self.__p_global_values = ocp.p_global_values + + self.time_steps = ocp.solver_options.time_steps + + # setup casadi functions for constraints and cost + cost_expr = get_path_cost_expression(ocp) + cost_expr_e = get_terminal_cost_expression(ocp) + + p_global = ocp.model.p_global + cost_fun_args = [ocp.model.x, ocp.model.u, ocp.model.p, p_global] + + self.cost_fun = ca.Function( + 'cost_fun', + cost_fun_args, + [cost_expr] + ) + cost_fun_args_e = [ocp.model.x, ocp.model.p, p_global] + + self.terminal_cost_fun = ca.Function( + 'cost_fun_e', + cost_fun_args_e, + [cost_expr_e] + ) + + # constraints + bu_expr = model.u[constraints.idxbu] + bx_expr = model.x[constraints.idxbx] + bx_expr_e = model.x[constraints.idxbx_e] + + if not is_empty(constraints.C): + g_expr = constraints.C @ model.x + constraints.D @ model.u + else: + g_expr = ca.DM.zeros(0, 1) + + if not is_empty(constraints.C_e): + g_expr_e = constraints.C_e @ model.x + else: + g_expr_e = ca.DM.zeros(0, 1) + + h_expr = model.con_h_expr + h_expr_e = model.con_h_expr_e + + if ocp.dims.nphi > 0: + raise NotImplementedError("AcadosCostConstraintEvaluatator: not implemented for nontrivial phi.") + + constraint_args = [bu_expr, bx_expr, g_expr, h_expr] + constraint_args_e = [bx_expr_e, g_expr_e, h_expr_e] + + constraint_expr = ca.vertcat(*constraint_args) + upper_bound = ca.vertcat( + constraints.ubu, + constraints.ubx, + constraints.ug, + constraints.uh, + constraints.uphi + ) + lower_bound = ca.vertcat( + constraints.lbu, + constraints.lbx, + constraints.lg, + constraints.lh, + constraints.lphi + ) + + constraint_expr_e = ca.vertcat(*constraint_args_e) + upper_bound_e = ca.vertcat( + constraints.ubx_e, + constraints.ug_e, + constraints.uh_e, + constraints.uphi_e + ) + lower_bound_e = ca.vertcat( + constraints.lbx_e, + constraints.lg_e, + constraints.lh_e, + constraints.lphi_e + ) + + lower_violation = ca.fmax(lower_bound - constraint_expr, 0) + upper_violation = ca.fmax(constraint_expr - upper_bound, 0) + + lower_violation_e = ca.fmax(lower_bound_e - constraint_expr_e, 0) + upper_violation_e = ca.fmax(constraint_expr_e - upper_bound_e, 0) + + slack_indices = np.concatenate(( + constraints.idxsbu, + constraints.idxsbx + dims.nbu, + constraints.idxsg + dims.nbu + dims.nbx, + constraints.idxsh + dims.nbu + dims.nbx + dims.ng, + constraints.idxsphi + dims.nbu + dims.nbx + dims.ng + dims.nh + )) + + slack_indices_e = np.concatenate(( + constraints.idxsbx_e, + constraints.idxsg_e + dims.nbx_e, + constraints.idxsh_e + dims.nbx_e + dims.ng_e, + constraints.idxsphi_e + dims.nbx_e + dims.ng_e + dims.nh_e + )) + + self.nonslacked_indices = np.setdiff1d( + np.arange(constraint_expr.shape[0]), + slack_indices + ) + + self.nonslacked_indices_e = np.setdiff1d( + np.arange(constraint_expr_e.shape[0]), + slack_indices_e + ) + + lower_slack_expression = lower_violation[slack_indices] + upper_slack_expression = upper_violation[slack_indices] + + lower_slack_expression_e = lower_violation_e[slack_indices_e] + upper_slack_expression_e = upper_violation_e[slack_indices_e] + + self.constraint_function = ca.Function( + 'constraint_function', + cost_fun_args, + [lower_violation, upper_violation, lower_slack_expression, upper_slack_expression] + ) + + self.constraint_function_e = ca.Function( + 'constraint_function_e', + cost_fun_args_e, + [lower_violation_e, upper_violation_e, lower_slack_expression_e, upper_slack_expression_e] + ) + + @property + def parameter_values(self): + """:math:`p` - initial values for parameter vector - can be updated stage-wise""" + return self.__parameter_values + + @property + def p_global_values(self): + r"""initial values for :math:`p_\text{global}` vector, + Type: `numpy.ndarray` of shape `(np_global, )`. + """ + return self.__p_global_values + + def update_all(self, acados_solver: AcadosOcpSolver): + """ + Update the parameter values and global parameter values from the acados solver. + ATTENTION: Currently only parameter values are updated. Reference values, bounds, etc. are not updated. + """ + N = self.__ocp.dims.N + if self.__ocp.dims.np > 0: + for i in range(N): + self.__parameter_values[:, i] = acados_solver.get(i, 'p') + + if acados_solver.save_p_global is False: + print('\nCan not set \'p_global\', since the solver does not store these values by default. ' + 'In order to update \'p_global\', please set the option \'save_p_global=True\' in the ' + 'instantiation of the acados solver.') + else: + self.__p_global_values = acados_solver.get_flat('p_global') + + def _get_check_parameters( + self, p: np.ndarray = None, + p_global: np.ndarray = None) -> Tuple[np.ndarray, np.ndarray]: + parameter_values_return = self.__parameter_values + p_global_values_return = self.__p_global_values + + if p is not None: + if p.shape == self.__parameter_values.shape: + parameter_values_return = p + else: + raise ValueError( + f"Parameter vector 'p' has wrong shape {p.shape} instead of {self.__parameter_values.shape}.") + + if p_global is not None: + if p_global.shape == self.__p_global_values.shape: + p_global_values_return = p_global + else: + raise ValueError( + f"Global parameter vector 'p_global' has wrong shape {p_global.shape} instead of {self.__p_global_values.shape}.") + return parameter_values_return, p_global_values_return + + def evaluate(self, x: np.ndarray, + u: np.ndarray, + step: int = 0, + p: np.ndarray = None, + p_global: np.ndarray = None, + ) -> dict: + """ + Evaluates the cost and constraints at a given stage of the OCP. For a closed-loop evaluation the stage is + typically 0. If parameter values are provided, they are also set in the evaluator. + @param x: state vector + @param u: control input vector + @param step: stage index (0 <= stage < N) + @param p: parameter vector used for evaluation (optional) + @param p_global: global parameter vector used for evaluation (optional) + + @return: dictionary with the following keys: + - 'cost': total cost of the transition + - 'cost_without_slacks': total cost of transition without slack penalties + - 'cost_slacks': total cost of slack penalties + - 'violation_soft_constraints': individual violation of soft constraints (equal to slacks) + - 'violation_hard_constraints': individual violation of hard constraints + """ + parameter_values, p_global_values = self._get_check_parameters(p, p_global) + + if len(parameter_values) > 0: + parameter_values = parameter_values[:, step] + else: + parameter_values = parameter_values + cost_fun_args = [x, u, parameter_values, p_global_values] + + # evaluate cost + cost_without_slacks = self.cost_fun(*cost_fun_args).full() * self.time_steps[step] + + # evaluate constraints + lower_violation, upper_violation, lower_slack, upper_slack = ( + self.constraint_function(x, u, + parameter_values, + p_global_values)) + violation_hard_constraints = np.concatenate( + (lower_violation[self.nonslacked_indices], upper_violation[self.nonslacked_indices])) + + # evaluate cost of soft constraints + lower_slack_cost, upper_slack_cost = 0.,0. + + if self.__ocp.cost.Zl.size > 0: + lower_slack_cost += 0.5 * self.__ocp.cost.Zl @ (lower_slack.full() * lower_slack.full()) + if self.__ocp.cost.zl.size > 0: + lower_slack_cost += self.__ocp.cost.zl @ lower_slack.full() + + if self.__ocp.cost.Zu.size > 0: + upper_slack_cost += 0.5 * self.__ocp.cost.Zu @ (upper_slack.full()* upper_slack.full()) + if self.__ocp.cost.zu.size > 0: + upper_slack_cost += self.__ocp.cost.zu @ upper_slack.full() + + slack_cost = (lower_slack_cost + upper_slack_cost) * self.time_steps[step] + + + if len(slack_cost) == 0: + cost = cost_without_slacks + else: + cost = cost_without_slacks + slack_cost + + # evaluate sum + result = { + 'cost': cost.item(), + 'cost_without_slacks': cost_without_slacks.item(), + 'cost_slacks': slack_cost.item(), + 'violation_soft_constraints': np.concatenate((lower_slack.full(), upper_slack.full())), + 'violation_hard_constraints': violation_hard_constraints, + } + return result + + def evaluate_ocp_cost( + self, + acados_ocp_iterate: AcadosOcpIterate, + p: np.ndarray = None, + p_global: np.ndarray = None): + """ + Evaluates the cost of a whole OCP trajectory, as evaluated inside acados. + If parameter values are provided, they are also set in the evaluator. + @param acados_ocp_iterate: acados OCP iterate object + @param p: parameter vector used for evaluation (optional) + @param p_global: global parameter vector used for evaluation (optional) + """ + parameter_values, p_global_values = self._get_check_parameters(p, p_global) + cost = 0 + + # the cost on the first step is different in the OCP + step = 0 + result = self.evaluate(acados_ocp_iterate.x_traj[0], acados_ocp_iterate.u_traj[0], step=step) + cost += result['cost_without_slacks'] + step += 1 + + for x_traj, u_traj in zip(acados_ocp_iterate.x_traj[1:], acados_ocp_iterate.u_traj[1:]): + result = self.evaluate(x_traj, u_traj, step=step) + cost += result['cost'] + step += 1 + + if len(parameter_values) > 0: + parameter_values = parameter_values[:, -1] + else: + parameter_values = parameter_values + + cost_fun_args = [acados_ocp_iterate.x_traj[-1], parameter_values, p_global_values] + cost += self.terminal_cost_fun(*cost_fun_args).full() + lower_violation_e, upper_violation_e, lower_slack_e, upper_slack_e = ( + self.constraint_function_e(acados_ocp_iterate.x_traj[-1], + parameter_values, + p_global_values)) + + violation_hard_constraints = np.concatenate( + (lower_violation_e[self.nonslacked_indices_e], + upper_violation_e[self.nonslacked_indices_e])) + + # evaluate cost of soft constraints + lower_slack_cost_e, upper_slack_cost_e = np.array([0.]), np.array([0.]) + if self.__ocp.cost.Zl_e.size > 0: + lower_slack_cost_e += 0.5 * lower_slack_e.full().transpose()@self.__ocp.cost.Zl_e @ lower_slack_e.full() + + if self.__ocp.cost.zl_e.size > 0: + lower_slack_cost_e += self.__ocp.cost.zl_e @ lower_slack_e.full() + + + if self.__ocp.cost.Zu_e.size > 0: + upper_slack_cost_e += 0.5 * upper_slack_e.full().transpose()@self.__ocp.cost.Zu_e @ upper_slack_e.full() + + if self.__ocp.cost.zu_e.size > 0: + upper_slack_cost_e += self.__ocp.cost.zu_e @ upper_slack_e.full() + + if lower_slack_e.full().size > 0: + cost += lower_slack_cost_e + if upper_slack_e.full().size > 0: + cost += upper_slack_cost_e + + return cost[0][0] + + +def get_path_cost_expression(ocp: AcadosOcp): + # multiply path costs with td, nonuniform grid + # multiply with cost scaling + # cost scaling ueberschreibt tds, falls gesetzt + model = ocp.model if ocp.cost.cost_type == "LINEAR_LS": y = ocp.cost.Vx @ model.x + ocp.cost.Vu @ model.u + if casadi_length(model.z) > 0: - ocp.cost.Vz @ model.z + y += ocp.cost.Vz @ model.z residual = y - ocp.cost.yref - cost_dot = 0.5*(residual.T @ ocp.cost.W @ residual) + cost_dot = 0.5 * (residual.T @ ocp.cost.W @ residual) elif ocp.cost.cost_type == "NONLINEAR_LS": residual = model.cost_y_expr - ocp.cost.yref - cost_dot = 0.5*(residual.T @ ocp.cost.W @ residual) + cost_dot = 0.5 * (residual.T @ ocp.cost.W @ residual) elif ocp.cost.cost_type == "EXTERNAL": cost_dot = model.cost_expr_ext_cost elif ocp.cost.cost_type == "CONVEX_OVER_NONLINEAR": - cost_dot = ca.substitute( - model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) - + raise NotImplementedError( + "get_terminal_cost_expression: not implemented for CONVEX_OVER_NONLINEAR.") + #cost_dot = ca.substitute( + # model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) else: raise Exception("create_model_with_cost_state: Unknown cost type.") + return cost_dot + + +def get_terminal_cost_expression(ocp: AcadosOcp): + model = ocp.model + if ocp.cost.cost_type_e == "LINEAR_LS": + y = ocp.cost.Vx_e @ model.x + residual = y - ocp.cost.yref_e + cost_dot = 0.5 * (residual.T @ ocp.cost.W_e @ residual) + + elif ocp.cost.cost_type == "NONLINEAR_LS": + residual = model.cost_y_expr_e - ocp.cost.yref_e + cost_dot = 0.5 * (residual.T @ ocp.cost.W_e @ residual) + + elif ocp.cost.cost_type == "EXTERNAL": + cost_dot = model.cost_expr_ext_cost_e + + elif ocp.cost.cost_type == "CONVEX_OVER_NONLINEAR": + raise NotImplementedError( + "get_terminal_cost_expression: not implemented for CONVEX_OVER_NONLINEAR.") + #cost_dot = ca.substitute( + # model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) + else: + raise Exception("create_model_with_cost_state: Unknown terminal cost type.") + + return cost_dot + + +def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarray]: + """ + Creates a new AcadosModel with an extra state `cost_state`, + which has the dynamics of the cost function and slack penalties corresponding to the intermediate shooting nodes. + + Note: In contrast cost_discretization='INTEGRATOR', this allows to integrate also the slack penalties. + Since l1 slack penalties are nondifferentiable, an accurate cost integration with the model created by this function should use many integrator steps, when slack penalties are part of the OCP formulation. + + Returns the augmented model and the parameter values of the given AcadosOcp. + """ + + model = deepcopy(ocp.model) + symbol = model.get_casadi_symbol() + cost_state = symbol("cost_state") + cost_state_dot = symbol("cost_state_dot") + + cost_dot = get_path_cost_expression(ocp) + i_slack = 0 for ibu in ocp.constraints.idxsbu: iu = ocp.constraints.idxbu[ibu] lower_violation = ca.fmax(ocp.constraints.lbu[ibu] - model.u[iu], 0) upper_violation = ca.fmax(model.u[iu] - ocp.constraints.ubu[ibu], 0) cost_dot += ocp.cost.zl[i_slack] * lower_violation + \ - ocp.cost.Zl[i_slack] * lower_violation ** 2 + ocp.cost.Zl[i_slack] * lower_violation ** 2 cost_dot += ocp.cost.zu[i_slack] * upper_violation + \ - ocp.cost.Zu[i_slack] * upper_violation ** 2 + ocp.cost.Zu[i_slack] * upper_violation ** 2 i_slack += 1 for ibx in ocp.constraints.idxsbx: @@ -92,9 +480,9 @@ def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarra lower_violation = ca.fmax(ocp.constraints.lbx[ibx] - model.x[ix], 0) upper_violation = ca.fmax(model.x[ix] - ocp.constraints.ubx[ibx], 0) cost_dot += ocp.cost.zl[i_slack] * lower_violation + \ - ocp.cost.Zl[i_slack] * lower_violation ** 2 + ocp.cost.Zl[i_slack] * lower_violation ** 2 cost_dot += ocp.cost.zu[i_slack] * upper_violation + \ - ocp.cost.Zu[i_slack] * upper_violation ** 2 + ocp.cost.Zu[i_slack] * upper_violation ** 2 i_slack += 1 if not is_empty(ocp.constraints.C): @@ -103,9 +491,9 @@ def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarra lower_violation = ca.fmax(ocp.constraints.lg[ig] - g[ig], 0) upper_violation = ca.fmax(g[ig] - ocp.constraints.ug[ig], 0) cost_dot += ocp.cost.zl[i_slack] * lower_violation + \ - ocp.cost.Zl[i_slack] * lower_violation ** 2 + ocp.cost.Zl[i_slack] * lower_violation ** 2 cost_dot += ocp.cost.zu[i_slack] * upper_violation + \ - ocp.cost.Zu[i_slack] * upper_violation ** 2 + ocp.cost.Zu[i_slack] * upper_violation ** 2 i_slack += 1 for ih in ocp.constraints.idxsh: @@ -114,9 +502,9 @@ def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarra upper_violation = ca.fmax( ocp.model.con_h_expr[ih] - ocp.constraints.uh[ih], 0) cost_dot += ocp.cost.zl[i_slack] * lower_violation + \ - ocp.cost.Zl[i_slack] * lower_violation ** 2 + ocp.cost.Zl[i_slack] * lower_violation ** 2 cost_dot += ocp.cost.zu[i_slack] * upper_violation + \ - ocp.cost.Zu[i_slack] * upper_violation ** 2 + ocp.cost.Zu[i_slack] * upper_violation ** 2 i_slack += 1 if not is_empty(ocp.constraints.idxsphi): @@ -126,7 +514,7 @@ def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarra model.x = ca.vertcat(model.x, cost_state) model.xdot = ca.vertcat(model.xdot, cost_state_dot) model.f_expl_expr = ca.vertcat(model.f_expl_expr, cost_dot) - model.f_impl_expr = ca.vertcat(model.f_impl_expr, cost_state_dot-cost_dot) + model.f_impl_expr = ca.vertcat(model.f_impl_expr, cost_state_dot - cost_dot) return model, ocp.parameter_values @@ -196,12 +584,12 @@ def detect_constraint_structure(model: AcadosModel, constraints: AcadosOcpConstr constr_expr_h = ca.vertcat(constr_expr_h, c) lh.append(lb[ii]) uh.append(ub[ii]) - print(f'constraint {ii+1} is kept as nonlinear constraint.') + print(f'constraint {ii + 1} is kept as nonlinear constraint.') print(c) print(' ') else: # c is linear in x and u Jc_fun = ca.Function('Jc_fun', [x, u], [ - ca.jacobian(c, ca.vertcat(x, u))]) + ca.jacobian(c, ca.vertcat(x, u))]) Jc = Jc_fun(0, 0) if np.sum(Jc != 0) == 1: # c is bound @@ -212,7 +600,7 @@ def detect_constraint_structure(model: AcadosModel, constraints: AcadosOcpConstr Jbx[-1, idb] = 1 lbx.append(lb[ii] / Jc[idb]) ubx.append(ub[ii] / Jc[idb]) - print(f'constraint {ii+1} is reformulated as bound on x.') + print(f'constraint {ii + 1} is reformulated as bound on x.') print(c) print(' ') else: @@ -221,7 +609,7 @@ def detect_constraint_structure(model: AcadosModel, constraints: AcadosOcpConstr Jbu[-1, idb - nx] = 1 lbu.append(lb[ii] / Jc[idb]) ubu.append(ub[ii] / Jc[idb]) - print(f'constraint {ii+1} is reformulated as bound on u.') + print(f'constraint {ii + 1} is reformulated as bound on u.') print(c) print(' ') else: @@ -231,7 +619,7 @@ def detect_constraint_structure(model: AcadosModel, constraints: AcadosOcpConstr lg.append(lb[ii]) ug.append(ub[ii]) print( - f'constraint {ii+1} is reformulated as general linear constraint.') + f'constraint {ii + 1} is reformulated as general linear constraint.') print(c) print(' ') @@ -326,9 +714,9 @@ def J_to_idx(J): this_idx = ca.DM(J[i, :].sparsity()).full().flatten().nonzero()[0] if len(this_idx) != 1: raise ValueError( - f'J_to_idx: Invalid J matrix. Exiting. Found more than one nonzero in row {i+1}.') + f'J_to_idx: Invalid J matrix. Exiting. Found more than one nonzero in row {i + 1}.') if J[i, this_idx] != 1: raise ValueError( - f'J_to_idx: J matrices can only contain 1s, got J({i+1}, {this_idx}) = {J[i, this_idx]}') + f'J_to_idx: J matrices can only contain 1s, got J({i + 1}, {this_idx}) = {J[i, this_idx]}') idx.append(this_idx[0]) return np.array(idx)