acados · FreyJo · Oct 4, 2024 · Oct 2, 2024 · Oct 2, 2024 · Oct 2, 2024
diff --git a/examples/acados_matlab_octave/p_global_example/create_ocp_formulation_without_opts.m b/examples/acados_matlab_octave/p_global_example/create_ocp_formulation_without_opts.m
@@ -50,6 +50,8 @@
         model.p = vertcat(model.p, p_global);
         model.p_global = [];
         ocp.parameter_values = [ocp.parameter_values; p_global_values];
+    else
+        ocp.p_global_values = p_global_values;
     end
 end
 

diff --git a/examples/acados_matlab_octave/p_global_example/main.m b/examples/acados_matlab_octave/p_global_example/main.m
@@ -252,7 +252,6 @@ function run_example_ocp_simulink_p_global()
 
     N_horizon_1 = 10;
     N_horizon_2 = 10;
-    N_horizon = N_horizon_1 + N_horizon_2;
     mocp = AcadosMultiphaseOcp([N_horizon_1, N_horizon_2]);
     ocp_phase_1 = create_ocp_formulation_without_opts(p_global, m, l, C, lut, use_p_global, p_global_values);
     ocp_phase_2 = create_ocp_formulation_without_opts(p_global, m, l, C, lut, use_p_global, p_global_values);
@@ -262,6 +261,9 @@ function run_example_ocp_simulink_p_global()
     mocp.set_phase(ocp_phase_1, 1);
     mocp.set_phase(ocp_phase_2, 2);
 
+    if use_p_global
+        mocp.p_global_values = p_global_values;
+    end
 end
 
 

diff --git a/examples/acados_python/p_global_example/example_p_global.py b/examples/acados_python/p_global_example/example_p_global.py
@@ -200,6 +200,8 @@ def main(use_cython=False, lut=True, use_p_global=True, blazing=True):
 
     if not use_p_global:
         ocp.parameter_values = np.concatenate([ocp.parameter_values, p_global_values])
+    else:
+        ocp.p_global_values = p_global_values
 
     Tf = 1.0
     N_horizon = 20
@@ -271,6 +273,8 @@ def main_mocp(lut=True, use_p_global=True):
     if not use_p_global:
         for ip in range(n_phases):
             mocp.parameter_values[ip] = np.concatenate([mocp.parameter_values[ip], p_global_values])
+    else:
+        mocp.p_global_values = p_global_values
 
     # set options
     mocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES

diff --git a/interfaces/acados_matlab_octave/AcadosModel.m b/interfaces/acados_matlab_octave/AcadosModel.m
@@ -219,11 +219,17 @@ function make_consistent(obj, dims)
                 dims.np_global = 0;
                 obj.p_global = empty_var;
             elseif iscolumn(obj.p_global) || (isa(obj.p_global, 'casadi.SX') == isSX && length(obj.p_global) == 0)
+                if any(which_depends(obj.p_global, obj.p))
+                    error('model.p_global must not depend on model.p')
+                end
                 dims.np_global = size(obj.p_global, 1);
             else
                 error('model.p_global should be column vector.');
             end
 
+            if dims.np_global > 0 && ~isa(obj.p_global, 'casadi.MX')
+                error('model.p_global needs to be casadi.MX')
+            end
             if isempty(obj.xdot)
                 obj.xdot = empty_var;
             elseif ~(isa(obj.xdot, 'casadi.SX') == isSX && length(obj.xdot) == 0) && (~iscolumn(obj.xdot) || size(obj.xdot, 1) ~= dims.nx)

diff --git a/interfaces/acados_matlab_octave/AcadosMultiphaseOcp.m b/interfaces/acados_matlab_octave/AcadosMultiphaseOcp.m
@@ -44,6 +44,7 @@
 
         model
         parameter_values % initial value of the parameter
+        p_global_values % initial value of global parameters
         problem_class
         simulink_opts
         name
@@ -86,14 +87,15 @@
                 obj.model{i} = AcadosModel();
             end
 
-            self.dummy_ocp_list = cell(n_phases, 1);
+            obj.dummy_ocp_list = cell(n_phases, 1);
 
             obj.solver_options = AcadosOcpOptions();
             obj.solver_options.N_horizon = obj.N_horizon; % NOTE: to not change options when making ocp consistent
 
             obj.mocp_opts = AcadosMultiphaseOptions();
 
             obj.parameter_values = cell(n_phases, 1);
+            obj.p_global_values = [];
             obj.problem_class = 'MOCP';
             obj.simulink_opts = [];
             obj.cython_include_dirs = [];
@@ -135,6 +137,10 @@ function set_phase(self, ocp, phase_idx)
             self.cost{phase_idx} = ocp.cost;
             self.constraints{phase_idx} = ocp.constraints;
             self.parameter_values{phase_idx} = ocp.parameter_values;
+
+            if ~isempty(self.p_global_values)
+                fprintf('WARNING: set_phase: Phase %d contains p_global values which will be ignored.\n', phase_idx);
+            end
         end
 
         function make_consistent(self)
@@ -199,6 +205,7 @@ function make_consistent(self)
                 ocp.constraints = self.constraints{i};
                 ocp.cost = self.cost{i};
                 ocp.parameter_values = self.parameter_values{i};
+                ocp.p_global_values = self.p_global_values;
                 ocp.solver_options = self.solver_options;
 
                 % set phase dependent options
@@ -335,6 +342,7 @@ function dump_to_json(self)
             end
 
             % prepare struct for json dump
+            out_struct.p_global_values = reshape(num2cell(self.p_global_values), [1, self.phases_dims{1}.np_global]);
             for i=1:self.n_phases
                 out_struct.parameter_values{i} = reshape(num2cell(self.parameter_values{i}), [1, self.phases_dims{i}.np]);
                 out_struct.model{i} = orderfields(self.model{i}.convert_to_struct_for_json_dump());
@@ -345,7 +353,7 @@ function dump_to_json(self)
             out_struct.solver_options = orderfields(self.solver_options.convert_to_struct_for_json_dump(self.N_horizon));
             out_struct.mocp_opts = orderfields(self.mocp_opts.struct());
 
-            vector_fields = {'model', 'phases_dims', 'cost', 'constraints', 'parameter_values'};
+            vector_fields = {'model', 'phases_dims', 'cost', 'constraints', 'parameter_values', 'p_global_values'};
             out_struct = prepare_struct_for_json_dump(out_struct, vector_fields, {});
 
             % add full path to json file

diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m
@@ -37,6 +37,7 @@
         solver_options
         model
         parameter_values % initial value of the parameter
+        p_global_values % initial value of the parameter
         acados_include_path
         acados_lib_path
         problem_class
@@ -60,6 +61,7 @@
             obj.model = AcadosModel();
 
             obj.parameter_values = [];
+            obj.p_global_values = [];
             obj.problem_class = 'OCP';
             obj.simulink_opts = [];
             obj.cython_include_dirs = [];
@@ -124,7 +126,19 @@ function make_consistent(self)
                 end
                 self.parameter_values = zeros(self.dims.np,1);
             elseif length(self.parameter_values) ~= self.dims.np
-                error(['parameters_values has the wrong shape. Expected: ' num2str(self.dims.np)])
+                error(['parameter_values has the wrong shape. Expected: ' num2str(self.dims.np)])
+            end
+
+
+            % parameters
+            if isempty(self.p_global_values)
+                if dims.np_global > 0
+                    warning(['self.p_global_values are not set.', ...
+                            10 'Using zeros(np_global,1) by default.' 10 'You can update them later using set().']);
+                end
+                self.p_global_values = zeros(self.dims.np_global,1);
+            elseif length(self.p_global_values) ~= self.dims.np_global
+                error(['p_global_values has the wrong shape. Expected: ' num2str(self.dims.np_global)])
             end
 
             %% cost
@@ -1039,6 +1053,7 @@ function dump_to_json(self, json_file)
 
             % prepare struct for json dump
             out_struct.parameter_values = reshape(num2cell(self.parameter_values), [1, self.dims.np]);
+            out_struct.p_global_values = reshape(num2cell(self.p_global_values), [1, self.dims.np_global]);
             out_struct.model = orderfields(self.model.convert_to_struct_for_json_dump());
             out_struct.dims = orderfields(out_struct.dims.struct());
             out_struct.cost = orderfields(out_struct.cost.convert_to_struct_for_json_dump());

diff --git a/interfaces/acados_matlab_octave/AcadosSim.m b/interfaces/acados_matlab_octave/AcadosSim.m
@@ -89,6 +89,10 @@ function make_consistent(self)
             % model
             self.model.make_consistent(self.dims);
 
+            if self.dims.np_global > 0
+                error('p_global is not supported for AcadosSim.')
+            end
+
             % detect GNSF structure
             if strcmp(self.solver_options.integrator_type, 'GNSF')
                 % TODO: interface these options

diff --git a/interfaces/acados_matlab_octave/acados_ocp_opts.m b/interfaces/acados_matlab_octave/acados_ocp_opts.m
@@ -51,6 +51,7 @@
             obj.opts_struct.shooting_nodes = [];
             obj.opts_struct.time_steps = [];
             obj.opts_struct.parameter_values = [];
+            obj.opts_struct.p_global_values = [];
 
             obj.opts_struct.nlp_solver = 'sqp';
             obj.opts_struct.nlp_solver_exact_hessian = 'false';
@@ -237,6 +238,8 @@
                 obj.opts_struct.globalization = value;
             elseif (strcmp(field, 'parameter_values'))
                 obj.opts_struct.parameter_values = value;
+            elseif (strcmp(field, 'p_global_values'))
+                obj.opts_struct.p_global_values = value;
             elseif (strcmp(field, 'store_iterates'))
                 obj.opts_struct.store_iterates = value;
             elseif (strcmp(field, 'ext_fun_compile_flags'))

diff --git a/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m b/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m
@@ -165,6 +165,7 @@
 
     % parameters
     ocp.parameter_values = opts_struct.parameter_values;
+    ocp.p_global_values = opts_struct.p_global_values;
 
     %% constraints
     constraints_fields_map = struct(...

diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py
@@ -167,6 +167,7 @@ def __init__(self, N_list: list):
         self.cython_include_dirs = [np.get_include(), get_paths()['include']]
 
         self.__parameter_values = [np.array([]) for _ in range(n_phases)]
+        self.__p_global_values = np.array([])
         self.__problem_class = "MOCP"
         self.__json_file = 'mocp.json'
 
@@ -194,6 +195,22 @@ def parameter_values(self, parameter_values):
             raise Exception('parameter_values must be a list of length n_phases.')
         self.__parameter_values = parameter_values
 
+
+    @property
+    def p_global_values(self):
+        """initial values for :math:`p_\\text{global}` vector, see `AcadosModel.p_global` - can be updated.
+        NOTE: `p_global` is shared between all phases.
+        Type: `numpy.ndarray` of shape `(np_global, )`.
+        """
+        return self.__p_global_values
+
+    @p_global_values.setter
+    def p_global_values(self, p_global_values):
+        if not isinstance(p_global_values, np.ndarray):
+            raise Exception('p_global_values must be a single numpy.ndarrays.')
+        self.__p_global_values = p_global_values
+
+
     @property
     def json_file(self):
         """Name of the json file where the problem description is stored."""
@@ -227,6 +244,10 @@ def set_phase(self, ocp: AcadosOcp, phase_idx: int) -> None:
         self.cost[phase_idx] = ocp.cost
         self.constraints[phase_idx] = ocp.constraints
         self.parameter_values[phase_idx] = ocp.parameter_values
+
+        if ocp.p_global_values.size > 0:
+            print(f"WARNING: set_phase: Phase {phase_idx} contains p_global_values which will be ignored.")
+
         return
 
     def make_consistent(self) -> None:
@@ -279,6 +300,7 @@ def make_consistent(self) -> None:
             ocp.constraints = self.constraints[i]
             ocp.cost = self.cost[i]
             ocp.parameter_values = self.parameter_values[i]
+            ocp.p_global_values = self.p_global_values
             ocp.solver_options = self.solver_options
 
             # set phase dependent options

diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py
@@ -75,6 +75,7 @@ class AcadosOcp:
         - :py:attr:`shared_lib_ext` (set automatically)
         - :py:attr:`acados_lib_path` (set automatically)
         - :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
+        - :py:attr:`p_global_values` - used to initialize the global parameters (can be changed)
     """
     def __init__(self, acados_path=''):
         """
@@ -109,6 +110,7 @@ def __init__(self, acados_path=''):
         self.cython_include_dirs = [np.get_include(), get_paths()['include']]
 
         self.__parameter_values = np.array([])
+        self.__p_global_values = np.array([])
         self.__problem_class = 'OCP'
         self.__json_file = "acados_ocp.json"
 
@@ -134,6 +136,21 @@ def parameter_values(self, parameter_values):
             raise Exception('Invalid parameter_values value. ' +
                             f'Expected numpy array, got {type(parameter_values)}.')
 
+    @property
+    def p_global_values(self):
+        """initial values for :math:`p_\\text{global}` vector, see `AcadosModel.p_global` - can be updated.
+        Type: `numpy.ndarray` of shape `(np_global, )`.
+        """
+        return self.__p_global_values
+
+    @p_global_values.setter
+    def p_global_values(self, p_global_values):
+        if isinstance(p_global_values, np.ndarray):
+            self.__p_global_values = p_global_values
+        else:
+            raise Exception('Invalid p_global_values value. ' +
+                            f'Expected numpy array, got {type(p_global_values)}.')
+
     @property
     def json_file(self):
         """Name of the json file where the problem description is stored."""
@@ -161,6 +178,11 @@ def make_consistent(self) -> None:
             raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
                 f'\nGot np = {dims.np}, self.parameter_values.shape = {self.parameter_values.shape[0]}\n')
 
+        # p_global_vlaue
+        if self.p_global_values.shape[0] != dims.np_global:
+            raise Exception('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \
+                f'\nGot np_global = {dims.np_global}, self.p_global_values.shape = {self.p_global_values.shape[0]}\n')
+
         ## cost
         # initial stage - if not set, copy fields from path constraints
         if cost.cost_type_0 is None:
@@ -1459,4 +1481,5 @@ def augment_with_t0_param(self) -> None:
         self.model.t0 = ca.SX.sym("t0")
         self.model.p = ca.vertcat(self.model.p, self.model.t0)
         self.parameter_values = np.append(self.parameter_values, [0.0])
+        self.p_global_values = np.append(self.p_global_values, [0.0])
         return
diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c
@@ -811,8 +811,8 @@ void {{ name }}_acados_create_setup_functions({{ name }}_solver_capsule* capsule
 void {{ name }}_acados_create_set_default_parameters({{ name }}_solver_capsule* capsule) {
 
     double* p = calloc({{ np_max }}, sizeof(double));
-{%- for jj in range(end=n_phases) %}{# phases loop !#}
     // initialize parameters to nominal value
+{%- for jj in range(end=n_phases) %}{# phases loop !#}
     {%- for item in parameter_values[jj] %}
         {%- if item != 0 %}
     p[{{ loop.index0 }}] = {{ item }};
@@ -824,6 +824,20 @@ void {{ name }}_acados_create_set_default_parameters({{ name }}_solver_capsule*
     }
 {%- endfor %}
     free(p);
+
+{%- if phases_dims[0].np_global > 0 %}
+    double* p_global = calloc({{ phases_dims[0].np_global }}, sizeof(double));
+    // initialize global parameters to nominal value
+    {%- for item in p_global_values %}
+        {%- if item != 0 %}
+    p_global[{{ loop.index0 }}] = {{ item }};
+        {%- endif %}
+    {%- endfor %}
+
+    {{ name }}_acados_set_p_global_and_precompute_dependencies(capsule, p_global, {{ phases_dims[0].np_global }});
+
+    free(p_global);
+{%- endif %}
 }
 
 

diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c
@@ -76,6 +76,7 @@
 #define NZ     {{ model.name | upper }}_NZ
 #define NU     {{ model.name | upper }}_NU
 #define NP     {{ model.name | upper }}_NP
+#define NP_GLOBAL     {{ model.name | upper }}_NP_GLOBAL
 #define NY0    {{ model.name | upper }}_NY0
 #define NY     {{ model.name | upper }}_NY
 #define NYN    {{ model.name | upper }}_NYN
@@ -848,6 +849,22 @@ void {{ model.name }}_acados_create_set_default_parameters({{ model.name }}_solv
 {%- else %}
     // no parameters defined
 {%- endif %}{# if dims.np #}
+
+{%- if dims.np_global > 0 %}
+    // initialize global parameters to nominal value
+    double* p_global = calloc(NP_GLOBAL, sizeof(double));
+    {%- for item in p_global_values %}
+        {%- if item != 0 %}
+    p_global[{{ loop.index0 }}] = {{ item }};
+        {%- endif %}
+    {%- endfor %}
+
+    {{ name }}_acados_set_p_global_and_precompute_dependencies(capsule, p_global, NP_GLOBAL);
+
+    free(p_global);
+{%- else %}
+    // no global parameters defined
+{%- endif %}{# if dims.np_global #}
 }
 
 

diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.h b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.h
@@ -40,6 +40,7 @@
 #define {{ model.name | upper }}_NZ     {{ dims.nz }}
 #define {{ model.name | upper }}_NU     {{ dims.nu }}
 #define {{ model.name | upper }}_NP     {{ dims.np }}
+#define {{ model.name | upper }}_NP_GLOBAL     {{ dims.np_global }}
 #define {{ model.name | upper }}_NBX    {{ dims.nbx }}
 #define {{ model.name | upper }}_NBX0   {{ dims.nbx_0 }}
 #define {{ model.name | upper }}_NBU    {{ dims.nbu }}