diff --git a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py index a6c57356e1..786f32649a 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py @@ -255,6 +255,11 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', plot= ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.02, 2000, 2000, 0.02, 0.02, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([2000, 2000, 0.02, 0.02, ])) + # test set cost scaling + if formulation_type == 'ocp': + for i in range(N): + ocp_solver.cost_set(i, "scaling", ocp.solver_options.time_steps[i]) + simX = np.zeros((N+1, NX)) simU = np.zeros((N, NU)) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index be93405ff8..4174444dde 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -913,6 +913,10 @@ void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, dims_out[0] = dims->nx[stage] + dims->nu[stage]; dims_out[1] = dims->nx[stage] + dims->nu[stage]; } + else if (!strcmp(field, "scaling")) + { + dims_out[0] = 1; + } else { printf("\nerror: ocp_nlp_cost_dims_get_from_attr: field %s not available\n", field); diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 16ca7cd6cb..b357fa75dc 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1269,8 +1269,11 @@ def cost_set(self, stage_: int, field_: str, value_, api='warn'): Set numerical data in the cost module of the solver. :param stage: integer corresponding to shooting node - :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu' + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu', 'scaling' :param value: of appropriate size + + Note: by default the cost is scaled with the time step, and the terminal cost term scaled with 1. + This can be overwritten by setting the 'scaling' field. """ # cast value_ to avoid conversion issues if isinstance(value_, (float, int)): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx index d075d3d0e5..d2284c33f4 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx @@ -592,7 +592,7 @@ cdef class AcadosOcpSolverCython: Set numerical data in the cost module of the solver. :param stage: integer corresponding to shooting node - :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'scaling' :param value: of appropriate size """ if not isinstance(value_, np.ndarray):