From 126bbb88e2d6e9ace103d54a18e2ffe9331122e6 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 17 Jul 2023 12:54:14 +0200
Subject: [PATCH 01/57] Added possibility to specify simple dynamics.

---
 src/hippopt/base/dynamics.py   | 47 +++++++++++++++++++++++++++-------
 test/test_multiple_shooting.py | 14 +++++++++-
 2 files changed, 51 insertions(+), 10 deletions(-)

diff --git a/src/hippopt/base/dynamics.py b/src/hippopt/base/dynamics.py
index f2fed503..0168857b 100644
--- a/src/hippopt/base/dynamics.py
+++ b/src/hippopt/base/dynamics.py
@@ -10,17 +10,21 @@
 
 @dataclasses.dataclass
 class DynamicsRHS:
-    _f: cs.Function = dataclasses.field(default=None)
+    _f: cs.Function | list[str] = dataclasses.field(default=None)
     _names_map: dict[str, str] = dataclasses.field(default=None)
     _names_map_inv: dict[str, str] = dataclasses.field(default=None)
-    f: dataclasses.InitVar[cs.Function] = None
+    f: dataclasses.InitVar[cs.Function | str | list[str]] = None
     names_map_in: dataclasses.InitVar[dict[str, str]] = None
 
-    def __post_init__(self, f: cs.Function, names_map_in: dict[str, str]):
+    def __post_init__(
+        self, f: cs.Function | str | list[str], names_map_in: dict[str, str]
+    ):
         """
         Create the DynamicsRHS object
         :param f: The CasADi function describing the dynamics. The output order should match the list provided
-         in the dot function.
+         in the dot function. As an alternative, if the dynamics is trivial (e.g dot(x) = y),
+         it is possible to pass directly the name of the variable in the right-hand-side, or the list of variables
+         in case the left-hand-side is a list.
         :param names_map_in: A dict describing how to switch from the input names to those used in the function.
          The key is the name provided by the user, while the value is the input name expected by the function.
          It is also possible to specify labels for nested variables using ".", e.g. "a.b" corresponds
@@ -29,7 +33,7 @@ def __post_init__(self, f: cs.Function, names_map_in: dict[str, str]):
          If time is an input, its label needs to be provided using the "dot" function.
         :return: Nothing
         """
-        self._f = f
+        self._f = [f] if isinstance(f, str) else f
         self._names_map = names_map_in if names_map_in else {}
         self._names_map_inv = {v: k for k, v in self._names_map.items()}  # inverse dict
 
@@ -48,10 +52,19 @@ def evaluate(
             key = name if name not in self._names_map else self._names_map[name]
             input_dict[key] = variables[name]
 
+        if isinstance(self._f, list):
+            return input_dict
+
+        assert isinstance(self._f, cs.Function)
         return self._f(**input_dict)
 
     def input_names(self) -> list[str]:
-        function_inputs = self._f.name_in()
+        if isinstance(self._f, list):
+            function_inputs = self._f
+        else:
+            assert isinstance(self._f, cs.Function)
+            function_inputs = self._f.name_in()
+
         output = []
         for el in function_inputs:
             output_name = self._names_map_inv[el] if el in self._names_map_inv else el
@@ -60,6 +73,10 @@ def input_names(self) -> list[str]:
         return output
 
     def outputs(self) -> list[str]:
+        if isinstance(self._f, list):
+            return self._f
+
+        assert isinstance(self._f, cs.Function)
         return self._f.name_out()
 
 
@@ -82,7 +99,9 @@ def __post_init__(self, x: list[str] | str, t_label: str):
         self._x = x if isinstance(x, list) else [x]
         self._t_label = t_label if isinstance(t_label, str) else "t"
 
-    def equal(self, f: cs.Function, names_map: dict[str, str] = None) -> TDynamics:
+    def equal(
+        self, f: cs.Function | str | list[str], names_map: dict[str, str] = None
+    ) -> TDynamics:
         rhs = DynamicsRHS(f=f, names_map_in=names_map)
         if len(rhs.outputs()) != len(self._x):
             raise ValueError(
@@ -91,12 +110,22 @@ def equal(self, f: cs.Function, names_map: dict[str, str] = None) -> TDynamics:
         return TypedDynamics(lhs=self, rhs=rhs)
 
     def __eq__(
-        self, other: cs.Function | tuple[cs.Function, dict[str, str]]
+        self,
+        other: cs.Function
+        | str
+        | list[str]
+        | tuple[cs.Function, dict[str, str]]
+        | tuple[str, dict[str, str]]
+        | tuple[list[str], dict[str, str]],
     ) -> TDynamics:
         if isinstance(other, tuple):
             return self.equal(f=other[0], names_map=other[1])
 
-        assert isinstance(other, cs.Function)
+        assert (
+            isinstance(other, cs.Function)
+            or isinstance(other, str)
+            or isinstance(other, list)
+        )
         return self.equal(f=other)
 
     def state_variables(self) -> list[str]:
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 7d74a9a6..1aec059d 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -221,7 +221,7 @@ class MassFallingTestVariables(OptimizationObject):
     def __post_init__(self):
         self.g = -9.81 * np.ones(1)
         self.masses = []
-        for _ in range(2):
+        for _ in range(3):
             self.masses.append(MassFallingState())
 
 
@@ -261,6 +261,14 @@ def test_multiple_shooting():
         x0_name="initial_condition",
     )
 
+    problem.add_dynamics(
+        dot(["masses[2].x", "masses[2].v"]) == ["masses[2].v", "g"],
+        dt=dt,
+        x0={"masses[2].x": initial_position, "masses[2].v": initial_velocity},
+        integrator=ForwardEuler,
+        x0_name="initial_condition_simple",
+    )
+
     problem.set_initial_guess(guess)
 
     sol = problem.solve()
@@ -273,6 +281,8 @@ def test_multiple_shooting():
     assert "initial_condition{0}" in problem.get_cost_expressions()
     assert "initial_condition{1}" in problem.get_cost_expressions()
     assert "initial_position" in sol.constraint_multipliers
+    assert "initial_condition_simple{0}" in sol.constraint_multipliers
+    assert "initial_condition_simple{1}" in sol.constraint_multipliers
 
     expected_position = initial_position
     expected_velocity = initial_velocity
@@ -282,5 +292,7 @@ def test_multiple_shooting():
         assert float(sol.values.masses[0][i].v) == pytest.approx(expected_velocity)
         assert float(sol.values.masses[1][i].x) == pytest.approx(expected_position)
         assert float(sol.values.masses[1][i].v) == pytest.approx(expected_velocity)
+        assert float(sol.values.masses[2][i].x) == pytest.approx(expected_position)
+        assert float(sol.values.masses[2][i].v) == pytest.approx(expected_velocity)
         expected_position += dt * expected_velocity
         expected_velocity += dt * guess.g

From 4c7ef03d80f4c6f4d70cd7b1858ddf98045bee11 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 24 Jul 2023 16:18:41 +0200
Subject: [PATCH 02/57] Avoid to add the integrators in the main namespace

---
 src/hippopt/__init__.py             | 2 --
 src/hippopt/integrators/__init__.py | 2 ++
 test/test_integrators.py            | 3 ++-
 test/test_multiple_shooting.py      | 8 ++++----
 4 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index 57281618..c675bafc 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -22,5 +22,3 @@
     step,
 )
 from .base.variable import TVariable, Variable
-from .integrators.forward_euler import ForwardEuler
-from .integrators.implicit_trapezoid import ImplicitTrapezoid
diff --git a/src/hippopt/integrators/__init__.py b/src/hippopt/integrators/__init__.py
index efef41b4..0d50829e 100644
--- a/src/hippopt/integrators/__init__.py
+++ b/src/hippopt/integrators/__init__.py
@@ -1 +1,3 @@
 from . import forward_euler, implicit_trapezoid
+from .forward_euler import ForwardEuler
+from .implicit_trapezoid import ImplicitTrapezoid
diff --git a/test/test_integrators.py b/test/test_integrators.py
index abbfbb6f..a17fa060 100644
--- a/test/test_integrators.py
+++ b/test/test_integrators.py
@@ -4,7 +4,8 @@
 import casadi as cs
 import pytest
 
-from hippopt import ForwardEuler, ImplicitTrapezoid, dot, step
+from hippopt import dot, step
+from hippopt.integrators import ForwardEuler, ImplicitTrapezoid
 
 
 def get_test_function() -> cs.Function:
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 1aec059d..bffe5b2a 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -6,7 +6,6 @@
 
 from hippopt import (
     ExpressionType,
-    ForwardEuler,
     MultipleShootingSolver,
     OptimalControlProblem,
     OptimizationObject,
@@ -16,6 +15,7 @@
     Variable,
     default_storage_field,
     dot,
+    integrators,
     time_varying_metadata,
 )
 
@@ -243,7 +243,7 @@ def test_multiple_shooting():
         dot(["masses[0].x", "masses[0].v"])
         == (MassFallingState.get_dynamics(), {"masses[0].x": "x", "masses[0].v": "v"}),
         dt=dt,
-        integrator=ForwardEuler,
+        integrator=integrators.ForwardEuler,
     )
 
     initial_position_constraint = var.masses[0][0].x == initial_position
@@ -256,7 +256,7 @@ def test_multiple_shooting():
         == (MassFallingState.get_dynamics(), {"masses[1].x": "x", "masses[1].v": "v"}),
         dt=dt,
         x0={"masses[1].x": initial_position, "masses[1].v": initial_velocity},
-        integrator=ForwardEuler,
+        integrator=integrators.ForwardEuler,
         mode=ExpressionType.minimize,
         x0_name="initial_condition",
     )
@@ -265,7 +265,7 @@ def test_multiple_shooting():
         dot(["masses[2].x", "masses[2].v"]) == ["masses[2].v", "g"],
         dt=dt,
         x0={"masses[2].x": initial_position, "masses[2].v": initial_velocity},
-        integrator=ForwardEuler,
+        integrator=integrators.ForwardEuler,
         x0_name="initial_condition_simple",
     )
 

From 5d1f9320236910d5bf06affca91b0dff30016320 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 18 Jul 2023 14:12:22 +0200
Subject: [PATCH 03/57] Added liecasadi and adam-robotics dependencies

---
 .github/workflows/ci_cd.yml | 4 +++-
 setup.cfg                   | 4 ++++
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index cfc98c37..50e1e455 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -31,11 +31,13 @@ jobs:
         with:
           miniforge-variant: Mambaforge
           miniforge-version: latest
+          channels: conda-forge,robotology
+          channel-priority: true
 
       - name: Dependencies
         shell: bash -l {0}
         run: |
-          mamba install python=${{ matrix.python }} casadi pytest
+          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics
 
       - name: Install
         shell: bash  -l {0}
diff --git a/setup.cfg b/setup.cfg
index fb353f48..545f594b 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -59,9 +59,13 @@ style =
     isort
 testing=
     pytest
+robot_planning=
+    liecasadi
+    adam-robotics
 all =
     %(style)s
     %(testing)s
+    %(robot_planning)s
 
 [options.packages.find]
 where = src

From 910d007d7e701ca8b313704149731af234d5d444 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 26 Jul 2023 10:37:56 +0200
Subject: [PATCH 04/57] Updated gitignore to not consider VSCode files

---
 .gitignore | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/.gitignore b/.gitignore
index ea7da93b..3cec1127 100644
--- a/.gitignore
+++ b/.gitignore
@@ -130,3 +130,6 @@ dmypy.json
 
 # Pycharm files
 .idea/
+
+# VSCode files
+.vscode/

From 5100160525079023f3f3be78ee512d13a0803ad8 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 27 Jul 2023 16:48:54 +0200
Subject: [PATCH 05/57] Stylistic improvements to multiple shooting solver

---
 src/hippopt/base/multiple_shooting_solver.py | 104 ++++++++++++-------
 1 file changed, 68 insertions(+), 36 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 2e6d70ee..354a6fcd 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -110,7 +110,8 @@ def _extend_structure_to_horizon(
                             "Field "
                             + field.name
                             + "is not a Numpy array. Cannot expand it to the horizon."
-                            ' Consider using "TimeExpansion.List" as time_expansion strategy.'
+                            ' Consider using "TimeExpansion.List"'
+                            " as time_expansion strategy."
                         )
 
                     if field_value.ndim > 1 and field_value.shape[1] > 1:
@@ -118,11 +119,14 @@ def _extend_structure_to_horizon(
                             "Cannot expand "
                             + field.name
                             + " since it is already a matrix."
-                            ' Consider using "TimeExpansion.List" as time_expansion strategy.'
+                            ' Consider using "TimeExpansion.List"'
+                            " as time_expansion strategy."
                         )
+                    # This is only needed to get the structure
+                    # for the optimization variables.
                     output.__setattr__(
                         field.name, np.zeros((field_value.shape[0], horizon_length))
-                    )  # This is only needed to get the structure for the optimization variables.
+                    )
                 else:
                     output_value = []
                     for _ in range(horizon_length):
@@ -136,7 +140,8 @@ def _extend_structure_to_horizon(
                 OptimizationObject.TimeDependentField not in field.metadata
                 and not custom_horizon
             ):
-                continue  # We expand nested variables (following two cases) only if time dependent
+                continue
+            # We expand nested variables (following two cases) only if time dependent
 
             if isinstance(field_value, OptimizationObject):
                 output_value = []
@@ -148,7 +153,9 @@ def _extend_structure_to_horizon(
 
             if isinstance(field_value, list) and all(
                 isinstance(elem, OptimizationObject) for elem in field_value
-            ):  # Nested variables are extended only if it is set as time dependent or if it has custom horizon
+            ):
+                # Nested variables are extended only if it is set as time dependent
+                #  or if it has custom horizon
                 if not len(field_value):  # skip empty lists
                     continue
 
@@ -234,8 +241,9 @@ def _generate_flattened_optimization_objects(
             if OptimizationObject.StorageTypeField in field.metadata:  # storage
                 if not time_dependent:
                     if base_iterator is not None:
-                        # generators cannot be rewound, but we might need to reuse them. Hence, we store
-                        # a lambda that can return a generator. Since in python it is not possible
+                        # generators cannot be rewound, but we might need to reuse them.
+                        # Hence, we store a lambda that can return a generator.
+                        #  Since in python it is not possible
                         # to have capture lists as in C++, we use partial from functools
                         # to store the inputs of the lambda within itself
                         # (inspired from https://stackoverflow.com/a/10101476)
@@ -359,7 +367,8 @@ def _generate_flattened_optimization_objects(
                 if not all_same:
                     continue
 
-                # If we are time dependent (and hence top_level has to be true), there is no base generator
+                # If we are time dependent (and hence top_level has to be true),
+                # there is no base generator
                 new_generator = partial(
                     (lambda value: (val for val in value)), field_value
                 )
@@ -435,34 +444,51 @@ def add_dynamics(
     ) -> None:
         """
         Add a dynamics to the optimal control problem
-        :param dynamics: The dynamics to add. The variables involved need to have a name corresponding to the name of
-                         a flattened variable. If the variable is nested, you can use "." as separator (e.g. "a.b" will
-                         look for variable b within a). If there is a list, you can use "[k]" with "k" the element
-                         to pick. For example "a.b[k].c" will look for variable "c" defined in the k-th element of "b"
-                         within "a". Only the top level variables can be time dependent. In this case, "a" could be time
-                         dependent and being a list, but this is automatically detected, and there is no need to specify
-                         the time-dependency. The "[k]" keyword is used only in case the list is not time-dependent.
-        :param x0: The initial state. It is a dictionary with the key equal to the state variable name. If no dict
-                   is provided, or a given variable is not found in the dictionary, the initial condition is not set.
+        :param dynamics: The dynamics to add. The variables involved need to have a
+                         name corresponding to the name of a flattened variable.
+                         If the variable is nested, you can use "." as separator
+                         (e.g. "a.b" will look for variable b within a).
+                         If there is a list, you can use "[k]" with "k" the element
+                         to pick. For example "a.b[k].c" will look for variable "c"
+                         defined in the k-th element of "b" within "a".
+                         Only the top level variables can be time dependent.
+                         In this case, "a" could be time dependent and being a list,
+                         but this is automatically detected, and there is no need to
+                         specify the time-dependency. The "[k]" keyword is used only
+                         in case the list is not time-dependent.
+        :param x0: The initial state. It is a dictionary with the key equal to the state
+                   variable name. If no dict is provided, or a given variable is not
+                   found in the dictionary, the initial condition is not set.
         :param t0: The initial time
-        :param mode: Optional argument to set the mode with which the dynamics is added to the problem.
+        :param mode: Optional argument to set the mode with which the
+                     dynamics is added to the problem.
                      Default: constraint
         :param name: The name used when adding the dynamics expression.
         :param x0_name: The name used when adding the initial condition expression.
         :param kwargs: Additional arguments. There are some required arguments:
-                                            - "dt": the integration time delta. It can either be a float in case it is
-                                                    constant, or a string to indicate the (flattened) name of the
+                                            - "dt": the integration time delta.
+                                                    It can either be a float in case it
+                                                    is constant, or a string to indicate
+                                                    the (flattened) name of the
                                                     variable to use.
 
                                             Optional arguments:
-                                            - "top_level_index": this defines the index to use in case we have a
-                                                                 list of optimization objects. This is used only if
-                                                                 the top level is a list.
-                                            - "max_steps": the number of integration steps. If not specified, the entire
-                                                           horizon of the specific state variable is used
-                                            - "integrator": specify the `SingleStepIntegrator` to use. This needs to be
-                                                            a type
-                                            - optional arguments of the `Problem.add_expression` method.
+                                            - "top_level_index": this defines the index
+                                                                 to use in case we have
+                                                                 a list of optimization
+                                                                 objects. This is used
+                                                                 only if the top level
+                                                                 is a list.
+                                            - "max_steps": the number of integration
+                                                           steps. If not specified, the
+                                                           entire horizon of the
+                                                           specific state variable
+                                                           is used
+                                            - "integrator": specify the
+                                                            `SingleStepIntegrator` to
+                                                            use. This needs to be a type
+                                            - optional arguments of the
+                                              `Problem.add_expression` method.
         :return: None
         """
         if "dt" not in kwargs:
@@ -474,7 +500,8 @@ def add_dynamics(
         if isinstance(self.get_optimization_objects(), list):
             if "top_level_index" not in kwargs:
                 raise ValueError(
-                    "The optimization objects are in a list, but top_level_index has not been specified."
+                    "The optimization objects are in a list, but top_level_index"
+                    " has not been specified."
                 )
             top_level_index = kwargs["top_level_index"]
 
@@ -487,7 +514,8 @@ def add_dynamics(
 
             if not isinstance(max_n, int) or max_n < 2:
                 raise ValueError(
-                    "max_steps is specified, but it needs to be an integer greater than 1"
+                    "max_steps is specified, but it needs to be an integer"
+                    " greater than 1"
                 )
 
         dt_size = 1
@@ -513,7 +541,8 @@ def add_dynamics(
 
         if not issubclass(integrator, SingleStepIntegrator):
             raise ValueError(
-                "The integrator has been defined, but it is not a subclass of SingleStepIntegrator"
+                "The integrator has been defined, but it is not "
+                "a subclass of SingleStepIntegrator"
             )
 
         variables = {}
@@ -559,7 +588,8 @@ def add_dynamics(
                     raise ValueError(
                         "The input "
                         + inp
-                        + " is time dependent, but it has a too small prediction horizon."
+                        + " is time dependent, but it has a too small "
+                        "prediction horizon."
                     )
 
                 additional_inputs[inp] = (inp_tuple[0], inp_tuple[1]())
@@ -579,7 +609,8 @@ def add_dynamics(
                 if var in x0:
                     initial_conditions[var] = x0[var]
 
-            # In the following, we add the initial condition expressions through the problem interface.
+            # In the following, we add the initial condition expressions
+            # through the problem interface.
             # In this way, we can exploit the machinery handling the generators,
             # and we can switch the dynamics from constraints to costs
             self.get_problem().add_expression(
@@ -608,9 +639,10 @@ def add_dynamics(
 
             name = base_name + "[" + str(i) + "]"
 
-            # In the following, we add the dynamics expressions through the problem interface, rather than the
-            # solver interface. In this way, we can exploit the machinery handling the generators,
-            # and we can switch the dynamics from constraints to costs
+            # In the following, we add the dynamics expressions through the problem
+            # interface, rather than the solver interface. In this way, we can exploit
+            # the machinery handling the generators, and we can switch the dynamics
+            # from constraints to costs
             self.get_problem().add_expression(
                 mode=mode,
                 expression=(cs.MX(x_next[name] == integrated[name]) for name in x_next),

From 720c5ee4227955667e0e7a590a9f229e329fa498 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 28 Jul 2023 13:04:32 +0200
Subject: [PATCH 06/57] Added functionality to create a symbolic structure in
 multiple shooting solver

The structure is equivalent to the input structure and should help in simplifying the definition of costs and constraints over the horizon
---
 src/hippopt/base/multiple_shooting_solver.py | 130 +++++++++++++++----
 1 file changed, 102 insertions(+), 28 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 354a6fcd..2bba1c34 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -18,20 +18,29 @@
 from .problem import ExpressionType, Problem
 from .single_step_integrator import SingleStepIntegrator, step
 
+FlattenedVariableIterator = Callable[[], Iterator[cs.MX]]
+FlattenedVariableTuple = tuple[int, FlattenedVariableIterator]
+FlattenedVariableDict = dict[str, FlattenedVariableTuple]
+
 
 @dataclasses.dataclass
 class MultipleShootingSolver(OptimalControlSolver):
     optimization_solver: dataclasses.InitVar[OptimizationSolver] = dataclasses.field(
         default=None
     )
+
     _optimization_solver: TOptimizationSolver = dataclasses.field(default=None)
 
     default_integrator: dataclasses.InitVar[SingleStepIntegrator] = dataclasses.field(
         default=None
     )
+
     _default_integrator: SingleStepIntegrator = dataclasses.field(default=None)
-    _flattened_variables: list[
-        dict[str, tuple[int, Callable[[], Iterator[cs.MX]]]]
+
+    _flattened_variables: list[FlattenedVariableDict] = dataclasses.field(default=None)
+
+    _symbolic_structure: TOptimizationObject | list[
+        TOptimizationObject
     ] = dataclasses.field(default=None)
 
     def __post_init__(
@@ -176,6 +185,7 @@ def generate_optimization_objects(
     ) -> TOptimizationObject | list[TOptimizationObject]:
         if isinstance(input_structure, list):
             expanded_structure = []
+            self._symbolic_structure = []
             for element in input_structure:
                 expanded_structure.append(
                     self._extend_structure_to_horizon(input_structure=element, **kwargs)
@@ -186,9 +196,11 @@ def generate_optimization_objects(
             )
 
             for var in variables:
-                self._flattened_variables.append(
-                    self._generate_flattened_optimization_objects(object_in=var)
+                flattened, symbolic = self._generate_flattened_and_symbolic_objects(
+                    object_in=var
                 )
+                self._flattened_variables.append(flattened)
+                self._symbolic_structure.append(symbolic)
 
         else:
             expanded_structure = self._extend_structure_to_horizon(
@@ -197,25 +209,30 @@ def generate_optimization_objects(
             variables = self._optimization_solver.generate_optimization_objects(
                 input_structure=expanded_structure, **kwargs
             )
-            self._flattened_variables.append(
-                self._generate_flattened_optimization_objects(object_in=variables)
+            flattened, symbolic = self._generate_flattened_and_symbolic_objects(
+                object_in=variables
             )
+            self._flattened_variables.append(flattened)
+            self._symbolic_structure = symbolic
 
         return variables
 
-    def _generate_flattened_optimization_objects(
+    def _generate_flattened_and_symbolic_objects(
         self,
-        object_in: TOptimizationObject | list[TOptimizationObject],
+        object_in: TOptimizationObject,
         top_level: bool = True,
         base_string: str = "",
         base_iterator: tuple[
             int, Callable[[], Iterator[TOptimizationObject | list[TOptimizationObject]]]
         ] = None,
-    ) -> dict[str, tuple[int, Callable[[], Iterator[cs.MX]]]]:
+    ) -> tuple[FlattenedVariableDict, TOptimizationObject]:
         assert (bool(top_level) != bool(base_iterator is not None)) or (
             not top_level and base_iterator is None
         )  # Cannot be top level and have base iterator
-        output = {}
+
+        output_dict = {}
+        output_symbolic = copy.deepcopy(object_in)
+
         for field in dataclasses.fields(object_in):
             field_value = object_in.__getattribute__(field.name)
 
@@ -257,7 +274,7 @@ def _generate_flattened_optimization_objects(
                             field.name,
                             base_iterator[1],
                         )
-                        output[base_string + field.name] = (
+                        output_dict[base_string + field.name] = (
                             base_iterator[0],
                             new_generator,
                         )
@@ -265,10 +282,20 @@ def _generate_flattened_optimization_objects(
                         constant_generator = partial(
                             (lambda value: itertools.repeat(value)), field_value
                         )
-                        output[base_string + field.name] = (
+                        output_dict[base_string + field.name] = (
                             1,
                             constant_generator,
                         )
+
+                    output_symbolic.__setattr__(
+                        field.name,
+                        cs.MX.sym(
+                            base_string + field.name,
+                            field_value.rows(),
+                            field_value.columns(),
+                        ),
+                    )
+
                     continue
 
                 if expand_storage:
@@ -278,19 +305,37 @@ def _generate_flattened_optimization_objects(
                         field_value,
                         n,
                     )
-                    output[base_string + field.name] = (n, new_generator)
+                    output_dict[base_string + field.name] = (n, new_generator)
+                    output_symbolic.__setattr__(
+                        field.name,
+                        cs.MX.sym(
+                            base_string + field.name,
+                            field_value.rows(),
+                            1,
+                        ),
+                    )
                     continue
 
                 assert isinstance(
                     field_value, list
                 )  # time dependent and not expand_storage
                 n = len(field_value)  # list case
+                assert n > 0
                 new_generator = partial(
                     (lambda value, dim: (value[i] for i in range(dim))),
                     field_value,
                     n,
                 )
-                output[base_string + field.name] = (n, new_generator)
+                output_dict[base_string + field.name] = (n, new_generator)
+                first_element = field_value[0]  # Assume all elements have same dims
+                output_symbolic.__setattr__(
+                    field.name,
+                    cs.MX.sym(
+                        base_string + field.name,
+                        first_element.rows(),
+                        first_element.columns(),
+                    ),
+                )
                 continue
 
             if isinstance(
@@ -311,7 +356,10 @@ def _generate_flattened_optimization_objects(
                     else None
                 )
 
-                output = output | self._generate_flattened_optimization_objects(
+                (
+                    inner_dict,
+                    inner_symbolic,
+                ) = self._generate_flattened_and_symbolic_objects(
                     object_in=field_value,
                     top_level=False,
                     base_string=base_string + field.name + ".",
@@ -319,12 +367,19 @@ def _generate_flattened_optimization_objects(
                     if generator is not None
                     else None,
                 )
+
+                output_dict = output_dict | inner_dict
+                output_symbolic.__setattr__(
+                    field.name,
+                    inner_symbolic,
+                )
                 continue
 
             if isinstance(field_value, list) and all(
                 isinstance(elem, OptimizationObject) for elem in field_value
             ):  # list[aggregate]
                 if not time_dependent:
+                    symbolic_list = output_symbolic.__getattribute__(field.name)
                     for k in range(len(field_value)):
                         generator = (
                             partial(
@@ -341,7 +396,11 @@ def _generate_flattened_optimization_objects(
                             if base_iterator is not None
                             else None
                         )
-                        output = output | self._generate_flattened_optimization_objects(
+
+                        (
+                            inner_dict,
+                            inner_symbolic,
+                        ) = self._generate_flattened_and_symbolic_objects(
                             object_in=field_value[k],
                             top_level=False,
                             base_string=base_string
@@ -353,6 +412,10 @@ def _generate_flattened_optimization_objects(
                             if generator is not None
                             else None,
                         )
+
+                        output_dict = output_dict | inner_dict
+                        symbolic_list[k] = inner_symbolic
+
                     continue
 
                 if not len(field_value):
@@ -360,27 +423,32 @@ def _generate_flattened_optimization_objects(
 
                 iterable = iter(field_value)
                 first = next(iterable)
-                all_same = all(
+                assert all(
                     isinstance(el, type(first)) for el in iterable
                 )  # check that each element has same type
 
-                if not all_same:
-                    continue
-
                 # If we are time dependent (and hence top_level has to be true),
                 # there is no base generator
                 new_generator = partial(
                     (lambda value: (val for val in value)), field_value
                 )
 
-                output = output | self._generate_flattened_optimization_objects(
-                    object_in=first,  # since they are al equal, expand only the first and exploit base_iterator
+                (
+                    inner_dict,
+                    inner_symbolic,
+                ) = self._generate_flattened_and_symbolic_objects(
+                    # since they are al equal, expand only the first
+                    # and exploit the base_iterator
+                    object_in=first,
                     top_level=False,
                     base_string=base_string
                     + field.name
                     + ".",  # we don't flatten the list
                     base_iterator=(len(field_value), new_generator),
                 )
+
+                output_dict = output_dict | inner_dict
+                output_symbolic.__setattr__(field.name, inner_symbolic)
                 continue
 
             if (
@@ -388,6 +456,8 @@ def _generate_flattened_optimization_objects(
                 and time_dependent
                 and all(isinstance(elem, list) for elem in field_value)
             ):  # list[list[aggregate]], only time dependent
+                # The inner list is the time dependency
+                symbolic_list = output_symbolic.__getattribute__(field.name)
                 for k in range(len(field_value)):
                     inner_list = field_value[k]
 
@@ -396,25 +466,29 @@ def _generate_flattened_optimization_objects(
 
                     iterable = iter(inner_list)
                     first = next(iterable)
-                    all_same = all(
+                    assert all(
                         isinstance(el, type(first)) for el in iterable
                     )  # check that each element has same type
 
-                    if not all_same:
-                        break
-
                     new_generator = partial(
                         (lambda value: (val for val in value)), inner_list
                     )
-                    output = output | self._generate_flattened_optimization_objects(
+
+                    (
+                        inner_dict,
+                        inner_symbolic,
+                    ) = self._generate_flattened_and_symbolic_objects(
                         object_in=first,
                         top_level=False,
                         base_string=base_string + field.name + "[" + str(k) + "].",
                         base_iterator=(len(inner_list), new_generator),
                     )
+
+                    output_dict = output_dict | inner_dict
+                    symbolic_list[k] = inner_symbolic
                 continue
 
-        return output
+        return output_dict, output_symbolic
 
     def get_optimization_objects(
         self,

From 1f5d12c1639a61e7426d18689b9a13bc93ca2bca Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 28 Jul 2023 14:32:17 +0200
Subject: [PATCH 07/57] Removed top_level_index when flattening variables

---
 src/hippopt/base/multiple_shooting_solver.py | 41 +++++-------
 test/test_multiple_shooting.py               | 65 +++++++++++++-------
 2 files changed, 58 insertions(+), 48 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 2bba1c34..68be935b 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -37,7 +37,7 @@ class MultipleShootingSolver(OptimalControlSolver):
 
     _default_integrator: SingleStepIntegrator = dataclasses.field(default=None)
 
-    _flattened_variables: list[FlattenedVariableDict] = dataclasses.field(default=None)
+    _flattened_variables: FlattenedVariableDict = dataclasses.field(default=None)
 
     _symbolic_structure: TOptimizationObject | list[
         TOptimizationObject
@@ -59,7 +59,7 @@ def __post_init__(
             if isinstance(default_integrator, SingleStepIntegrator)
             else ImplicitTrapezoid()
         )
-        self._flattened_variables = []
+        self._flattened_variables = {}
 
     def _extend_structure_to_horizon(
         self, input_structure: TOptimizationObject | list[TOptimizationObject], **kwargs
@@ -195,11 +195,11 @@ def generate_optimization_objects(
                 input_structure=expanded_structure, **kwargs
             )
 
-            for var in variables:
+            for k in range(len(variables)):
                 flattened, symbolic = self._generate_flattened_and_symbolic_objects(
-                    object_in=var
+                    object_in=variables[k], base_string="[" + str(k) + "]."
                 )
-                self._flattened_variables.append(flattened)
+                self._flattened_variables = self._flattened_variables | flattened
                 self._symbolic_structure.append(symbolic)
 
         else:
@@ -212,7 +212,7 @@ def generate_optimization_objects(
             flattened, symbolic = self._generate_flattened_and_symbolic_objects(
                 object_in=variables
             )
-            self._flattened_variables.append(flattened)
+            self._flattened_variables = flattened
             self._symbolic_structure = symbolic
 
         return variables
@@ -530,6 +530,8 @@ def add_dynamics(
                          but this is automatically detected, and there is no need to
                          specify the time-dependency. The "[k]" keyword is used only
                          in case the list is not time-dependent.
+                         In case we have a list of optimization objects, prepend "[k]."
+                         to name of the variable, with k the top level index.
         :param x0: The initial state. It is a dictionary with the key equal to the state
                    variable name. If no dict is provided, or a given variable is not
                    found in the dictionary, the initial condition is not set.
@@ -547,12 +549,6 @@ def add_dynamics(
                                                     variable to use.
 
                                             Optional arguments:
-                                            - "top_level_index": this defines the index
-                                                                 to use in case we have
-                                                                 a list of optimization
-                                                                 objects. This is used
-                                                                 only if the top level
-                                                                 is a list.
                                             - "max_steps": the number of integration
                                                            steps. If not specified, the
                                                            entire horizon of the
@@ -570,15 +566,6 @@ def add_dynamics(
                 "MultipleShootingSolver needs dt to be specified when adding a dynamics"
             )
 
-        top_level_index = 0
-        if isinstance(self.get_optimization_objects(), list):
-            if "top_level_index" not in kwargs:
-                raise ValueError(
-                    "The optimization objects are in a list, but top_level_index"
-                    " has not been specified."
-                )
-            top_level_index = kwargs["top_level_index"]
-
         dt_in = kwargs["dt"]
 
         max_n = 0
@@ -597,11 +584,11 @@ def add_dynamics(
         if isinstance(dt_in, float):
             dt_generator = itertools.repeat(cs.MX(dt_in))
         elif isinstance(dt_in, str):
-            if dt_in not in self._flattened_variables[top_level_index]:
+            if dt_in not in self._flattened_variables:
                 raise ValueError(
                     "The specified dt name is not found in the optimization variables"
                 )
-            dt_var_tuple = self._flattened_variables[top_level_index][dt_in]
+            dt_var_tuple = self._flattened_variables[dt_in]
             dt_size = dt_var_tuple[0]
             dt_generator = dt_var_tuple[1]
         else:
@@ -622,11 +609,11 @@ def add_dynamics(
         variables = {}
         n = max_n
         for var in dynamics.state_variables():
-            if var not in self._flattened_variables[top_level_index]:
+            if var not in self._flattened_variables:
                 raise ValueError(
                     "Variable " + var + " not found in the optimization variables."
                 )
-            var_tuple = self._flattened_variables[top_level_index][var]
+            var_tuple = self._flattened_variables[var]
             var_n = var_tuple[0]
             if n == 0:
                 if var_n < 2:
@@ -648,13 +635,13 @@ def add_dynamics(
 
         additional_inputs = {}
         for inp in dynamics.input_names():
-            if inp not in self._flattened_variables[top_level_index]:
+            if inp not in self._flattened_variables:
                 raise ValueError(
                     "Variable " + inp + " not found in the optimization variables."
                 )
 
             if inp not in variables:
-                inp_tuple = self._flattened_variables[top_level_index][inp]
+                inp_tuple = self._flattened_variables[inp]
 
                 inp_n = inp_tuple[0]
 
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index bffe5b2a..f296aea3 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -115,15 +115,15 @@ def test_flattened_variables_simple():
 
     var_flat = problem.solver().get_flattened_optimization_objects()
     assert "string" not in var_flat
-    assert var_flat[0]["variable"][0] == horizon_len
-    assert var_flat[0]["parameter"][0] == 1
-    assert next(var_flat[0]["parameter"][1]()) is var.parameter
+    assert var_flat["variable"][0] == horizon_len
+    assert var_flat["parameter"][0] == 1
+    assert next(var_flat["parameter"][1]()) is var.parameter
     assert (
-        next(var_flat[0]["parameter"][1]()) is var.parameter
+        next(var_flat["parameter"][1]()) is var.parameter
     )  # check that we can use the generator twice
-    variable_gen = var_flat[0]["variable"][1]()
+    variable_gen = var_flat["variable"][1]()
     assert all(next(variable_gen) is v for v in var.variable)
-    variable_gen = var_flat[0]["variable"][1]()
+    variable_gen = var_flat["variable"][1]()
     assert all(
         next(variable_gen) is v for v in var.variable
     )  # check that we can use the generator twice
@@ -142,46 +142,69 @@ def test_flattened_variables_composite():
 
     var_flat = problem.solver().get_flattened_optimization_objects()
 
-    assert len(var_flat) == 3
     assert len(var) == 3
 
     for j in range(3):
-        assert var_flat[j]["composite.variable"][0] == horizon_len
-        assert var_flat[j]["composite.parameter"][0] == horizon_len
-        par_gen = var_flat[j]["composite.parameter"][1]()
+        assert var_flat["[" + str(j) + "].composite.variable"][0] == horizon_len
+        assert var_flat["[" + str(j) + "].composite.parameter"][0] == horizon_len
+        par_gen = var_flat["[" + str(j) + "].composite.parameter"][1]()
         assert all(next(par_gen) is c.parameter for c in var[j].composite)
-        variable_gen = var_flat[j]["composite.variable"][1]()
+        variable_gen = var_flat["[" + str(j) + "].composite.variable"][1]()
         assert all(next(variable_gen) is c.variable for c in var[j].composite)
-        assert next(var_flat[j]["fixed.variable"][1]()) is var[j].fixed.variable
-        assert next(var_flat[j]["fixed.parameter"][1]()) is var[j].fixed.parameter
+        assert (
+            next(var_flat["[" + str(j) + "].fixed.variable"][1]())
+            is var[j].fixed.variable
+        )
+        assert (
+            next(var_flat["[" + str(j) + "].fixed.parameter"][1]())
+            is var[j].fixed.parameter
+        )
         for i in range(3):
             assert all(isinstance(c.variable, cs.MX) for c in var[j].composite_list[i])
-            variable_gen = var_flat[j]["composite_list[" + str(i) + "].variable"][1]()
+            variable_gen = var_flat[
+                "[" + str(j) + "].composite_list[" + str(i) + "].variable"
+            ][1]()
             assert (
-                var_flat[j]["composite_list[" + str(i) + "].variable"][0] == horizon_len
+                var_flat["[" + str(j) + "].composite_list[" + str(i) + "].variable"][0]
+                == horizon_len
             )
             assert all(
                 next(variable_gen) is c.variable for c in var[j].composite_list[i]
             )
             assert all(isinstance(c.parameter, cs.MX) for c in var[j].composite_list[i])
-            parameter_gen = var_flat[j]["composite_list[" + str(i) + "].parameter"][1]()
+            parameter_gen = var_flat[
+                "[" + str(j) + "].composite_list[" + str(i) + "].parameter"
+            ][1]()
             assert all(
                 next(parameter_gen) is c.parameter for c in var[j].composite_list[i]
             )
             assert (
-                var_flat[j]["composite_list[" + str(i) + "].parameter"][0]
+                var_flat["[" + str(j) + "].composite_list[" + str(i) + "].parameter"][0]
                 == horizon_len
             )
             assert (
-                next(var_flat[j]["fixed_list[" + str(i) + "].variable"][1]())
+                next(
+                    var_flat["[" + str(j) + "].fixed_list[" + str(i) + "].variable"][
+                        1
+                    ]()
+                )
                 is var[j].fixed_list[i].variable
             )
-            assert var_flat[j]["fixed_list[" + str(i) + "].variable"][0] == 1
             assert (
-                next(var_flat[j]["fixed_list[" + str(i) + "].parameter"][1]())
+                var_flat["[" + str(j) + "].fixed_list[" + str(i) + "].variable"][0] == 1
+            )
+            assert (
+                next(
+                    var_flat["[" + str(j) + "].fixed_list[" + str(i) + "].parameter"][
+                        1
+                    ]()
+                )
                 is var[j].fixed_list[i].parameter
             )
-            assert var_flat[j]["fixed_list[" + str(i) + "].parameter"][0] == 1
+            assert (
+                var_flat["[" + str(j) + "].fixed_list[" + str(i) + "].parameter"][0]
+                == 1
+            )
 
 
 @dataclasses.dataclass

From 0c758a46b24859d1bab09b9e7e99177aaa7d3ac9 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 28 Jul 2023 15:21:38 +0200
Subject: [PATCH 08/57] Output the symbolic structure when creating a ocp

---
 src/hippopt/__init__.py                      |  7 ++--
 src/hippopt/base/multiple_shooting_solver.py |  5 ++-
 src/hippopt/base/optimal_control_problem.py  | 38 ++++++++++++++++++--
 src/hippopt/base/optimal_control_solver.py   |  5 +++
 src/hippopt/base/optimization_problem.py     | 31 ++++++++++++++--
 test/test_multiple_shooting.py               |  6 ++--
 6 files changed, 82 insertions(+), 10 deletions(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index c675bafc..cda4ddb6 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -2,7 +2,10 @@
 from .base.dynamics import Dynamics, TypedDynamics, dot
 from .base.multiple_shooting_solver import MultipleShootingSolver
 from .base.opti_solver import OptiFailure, OptiSolver
-from .base.optimal_control_problem import OptimalControlProblem
+from .base.optimal_control_problem import (
+    OptimalControlProblem,
+    OptimalControlProblemInstance,
+)
 from .base.optimization_object import (
     OptimizationObject,
     StorageType,
@@ -12,7 +15,7 @@
     default_storage_metadata,
     time_varying_metadata,
 )
-from .base.optimization_problem import OptimizationProblem
+from .base.optimization_problem import OptimizationProblem, OptimizationProblemInstance
 from .base.optimization_solver import SolutionNotAvailableException
 from .base.parameter import Parameter, TParameter
 from .base.problem import ExpressionType, ProblemNotSolvedException
diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 68be935b..3eccb6a1 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -503,9 +503,12 @@ def get_problem(self) -> Problem:
 
     def get_flattened_optimization_objects(
         self,
-    ) -> list[dict[str, tuple[int, Callable[[], Iterator[cs.MX]]]]]:
+    ) -> FlattenedVariableDict:
         return self._flattened_variables
 
+    def get_symbolic_structure(self) -> TOptimizationObject | list[TOptimizationObject]:
+        return self._symbolic_structure
+
     def add_dynamics(
         self,
         dynamics: TDynamics,
diff --git a/src/hippopt/base/optimal_control_problem.py b/src/hippopt/base/optimal_control_problem.py
index 615ae3f3..40dac6b2 100644
--- a/src/hippopt/base/optimal_control_problem.py
+++ b/src/hippopt/base/optimal_control_problem.py
@@ -16,6 +16,36 @@
 )
 
 
+@dataclasses.dataclass
+class OptimalControlProblemInstance:
+    problem: TOptimalControlProblem = dataclasses.field(default=None)
+    all_variables: TInputObjects = dataclasses.field(default=None)
+    symbolic_structure: TInputObjects = dataclasses.field(default=None)
+
+    _problem: dataclasses.InitVar[TOptimalControlProblem] = dataclasses.field(
+        default=None
+    )
+    _all_variables: dataclasses.InitVar[TInputObjects] = dataclasses.field(default=None)
+    _symbolic_structure: dataclasses.InitVar[TInputObjects] = dataclasses.field(
+        default=None
+    )
+
+    def __post_init__(
+        self,
+        _problem: TOptimalControlProblem,
+        _all_variables: TInputObjects,
+        _symbolic_structure: TInputObjects,
+    ):
+        self.problem = _problem
+        self.all_variables = _all_variables
+        self.symbolic_structure = _symbolic_structure
+
+    def __iter__(self):
+        return iter([self.problem, self.all_variables, self.symbolic_structure])
+        # Cannot use astuple here since it would perform a deepcopy
+        # and would include InitVars too
+
+
 @dataclasses.dataclass
 class OptimalControlProblem(Problem[TOptimalControlSolver, TInputObjects]):
     optimal_control_solver: dataclasses.InitVar[
@@ -39,14 +69,18 @@ def create(
         input_structure: TInputObjects,
         optimal_control_solver: TOptimalControlSolver = None,
         **kwargs
-    ) -> tuple[TOptimalControlProblem, TInputObjects]:
+    ) -> OptimalControlProblemInstance:
         new_problem = cls(
             optimal_control_solver=optimal_control_solver,
         )
         new_problem._solver.generate_optimization_objects(
             input_structure=input_structure, **kwargs
         )
-        return new_problem, new_problem._solver.get_optimization_objects()
+        return OptimalControlProblemInstance(
+            _problem=new_problem,
+            _all_variables=new_problem._solver.get_optimization_objects(),
+            _symbolic_structure=new_problem._solver.get_symbolic_structure(),
+        )
 
     def add_dynamics(
         self,
diff --git a/src/hippopt/base/optimal_control_solver.py b/src/hippopt/base/optimal_control_solver.py
index 713d3072..a53ed0d1 100644
--- a/src/hippopt/base/optimal_control_solver.py
+++ b/src/hippopt/base/optimal_control_solver.py
@@ -5,6 +5,7 @@
 import casadi as cs
 
 from .dynamics import TDynamics
+from .optimization_object import TOptimizationObject
 from .optimization_solver import OptimizationSolver
 from .problem import ExpressionType
 
@@ -25,3 +26,7 @@ def add_dynamics(
         **kwargs
     ) -> None:
         pass
+
+    @abc.abstractmethod
+    def get_symbolic_structure(self) -> TOptimizationObject | list[TOptimizationObject]:
+        pass
diff --git a/src/hippopt/base/optimization_problem.py b/src/hippopt/base/optimization_problem.py
index 218fe214..b7726473 100644
--- a/src/hippopt/base/optimization_problem.py
+++ b/src/hippopt/base/optimization_problem.py
@@ -8,6 +8,30 @@
 TOptimizationProblem = TypeVar("TOptimizationProblem", bound="OptimizationProblem")
 
 
+@dataclasses.dataclass
+class OptimizationProblemInstance:
+    problem: TOptimizationProblem = dataclasses.field(default=None)
+    variables: TInputObjects = dataclasses.field(default=None)
+
+    _problem: dataclasses.InitVar[TOptimizationProblem] = dataclasses.field(
+        default=None
+    )
+    _variables: dataclasses.InitVar[TInputObjects] = dataclasses.field(default=None)
+
+    def __post_init__(
+        self,
+        _problem: TOptimizationProblem,
+        _variables: TInputObjects,
+    ):
+        self.problem = _problem
+        self.variables = _variables
+
+    def __iter__(self):
+        return iter([self.problem, self.variables])
+        # Cannot use astuple here since it would perform a deepcopy
+        # and would include InitVars too
+
+
 @dataclasses.dataclass
 class OptimizationProblem(Problem[TOptimizationSolver, TInputObjects]):
     optimization_solver: dataclasses.InitVar[OptimizationSolver] = dataclasses.field(
@@ -31,9 +55,12 @@ def create(
         input_structure: TInputObjects,
         optimization_solver: TOptimizationSolver = None,
         **kwargs
-    ) -> tuple[TOptimizationProblem, TInputObjects]:
+    ) -> OptimizationProblemInstance:
         new_problem = cls(optimization_solver=optimization_solver)
         new_problem._solver.generate_optimization_objects(
             input_structure=input_structure, **kwargs
         )
-        return new_problem, new_problem._solver.get_optimization_objects()
+        return OptimizationProblemInstance(
+            _problem=new_problem,
+            _variables=new_problem._solver.get_optimization_objects(),
+        )
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index f296aea3..7835d8c3 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -109,7 +109,7 @@ def test_composite_variables_custom_horizon():
 def test_flattened_variables_simple():
     horizon_len = 10
 
-    problem, var = OptimalControlProblem.create(
+    problem, var, _ = OptimalControlProblem.create(
         input_structure=MyTestVarMS(), horizon=horizon_len
     )
 
@@ -136,7 +136,7 @@ def test_flattened_variables_composite():
     for _ in range(3):
         structure.append(MyCompositeTestVar())
 
-    problem, var = OptimalControlProblem.create(
+    problem, var, _ = OptimalControlProblem.create(
         input_structure=structure, horizon=horizon_len
     )
 
@@ -257,7 +257,7 @@ def test_multiple_shooting():
     initial_position = 1.0
     initial_velocity = 0
 
-    problem, var = OptimalControlProblem.create(
+    problem, var, _ = OptimalControlProblem.create(
         input_structure=MassFallingTestVariables(),
         horizon=horizon,
     )

From 77d1cdbaf454013ce4ff38f1d315a4a2c7cd6ef8 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 28 Jul 2023 16:05:13 +0200
Subject: [PATCH 09/57] Fixed style issues in dynamics.py

---
 src/hippopt/base/dynamics.py | 27 ++++++++++++++++-----------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/src/hippopt/base/dynamics.py b/src/hippopt/base/dynamics.py
index 0168857b..d4427e2f 100644
--- a/src/hippopt/base/dynamics.py
+++ b/src/hippopt/base/dynamics.py
@@ -21,14 +21,17 @@ def __post_init__(
     ):
         """
         Create the DynamicsRHS object
-        :param f: The CasADi function describing the dynamics. The output order should match the list provided
-         in the dot function. As an alternative, if the dynamics is trivial (e.g dot(x) = y),
-         it is possible to pass directly the name of the variable in the right-hand-side, or the list of variables
-         in case the left-hand-side is a list.
-        :param names_map_in: A dict describing how to switch from the input names to those used in the function.
-         The key is the name provided by the user, while the value is the input name expected by the function.
-         It is also possible to specify labels for nested variables using ".", e.g. "a.b" corresponds
-         to the variable "b" within "a".
+        :param f: The CasADi function describing the dynamics. The output order should
+         match the list provided in the dot function. As an alternative, if the
+         dynamics is trivial (e.g dot(x) = y), it is possible to pass directly the name
+         of the variable in the right-hand-side, or the list of variables in case the
+         left-hand-side is a list.
+        :param names_map_in: A dict describing how to switch from the input names to
+         those used in the function.
+         The key is the name provided by the user, while the value is the input name
+         expected by the function.
+         Refer to the specific optimal control problem for a specification of the
+         label convention.
          This is valid only for the keys.
          If time is an input, its label needs to be provided using the "dot" function.
         :return: Nothing
@@ -91,8 +94,9 @@ def __post_init__(self, x: list[str] | str, t_label: str):
         """
         Constructs the DynamicsLHS object
         :param x: List of variable names on the left hand side of dot{x} = f(y).
-          The list can contain empty strings if some output of f needs to be discarded. If one output
-          needs to be mapped to a nested item, use "." as separator, e.g. "a.b"
+          The list can contain empty strings if some output of f needs to be discarded.
+          Refer to the specific optimal control problem for a specification of the
+          label convention.
         :param t_label: The label of the time variable. Default "t"
         :return: Nothing
         """
@@ -105,7 +109,8 @@ def equal(
         rhs = DynamicsRHS(f=f, names_map_in=names_map)
         if len(rhs.outputs()) != len(self._x):
             raise ValueError(
-                "The number of outputs of the dynamics function does not match the specified number of state variables."
+                "The number of outputs of the dynamics function does not match"
+                " the specified number of state variables."
             )
         return TypedDynamics(lhs=self, rhs=rhs)
 

From aaa4d8fe131db789b0c9ba3e9cac0bbf31db7f9c Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 28 Jul 2023 18:36:00 +0200
Subject: [PATCH 10/57] Allowed specifying the dynamics using MX.sym

---
 src/hippopt/base/dynamics.py | 68 ++++++++++++++++++++++++++++++------
 test/test_integrators.py     | 12 +++++++
 2 files changed, 69 insertions(+), 11 deletions(-)

diff --git a/src/hippopt/base/dynamics.py b/src/hippopt/base/dynamics.py
index d4427e2f..0e6166f3 100644
--- a/src/hippopt/base/dynamics.py
+++ b/src/hippopt/base/dynamics.py
@@ -13,11 +13,11 @@ class DynamicsRHS:
     _f: cs.Function | list[str] = dataclasses.field(default=None)
     _names_map: dict[str, str] = dataclasses.field(default=None)
     _names_map_inv: dict[str, str] = dataclasses.field(default=None)
-    f: dataclasses.InitVar[cs.Function | str | list[str]] = None
+    f: dataclasses.InitVar[cs.Function | str | list[str] | cs.MX] = None
     names_map_in: dataclasses.InitVar[dict[str, str]] = None
 
     def __post_init__(
-        self, f: cs.Function | str | list[str], names_map_in: dict[str, str]
+        self, f: cs.Function | str | list[str] | cs.MX, names_map_in: dict[str, str]
     ):
         """
         Create the DynamicsRHS object
@@ -36,7 +36,19 @@ def __post_init__(
          If time is an input, its label needs to be provided using the "dot" function.
         :return: Nothing
         """
-        self._f = [f] if isinstance(f, str) else f
+        if isinstance(f, str):
+            self._f = [f]
+        elif isinstance(f, list) or isinstance(f, cs.Function):
+            self._f = f
+        elif isinstance(f, cs.MX):
+            inputs = cs.symvar(f)
+            input_names = [el.name() for el in inputs]
+            self._f = cs.Function(
+                "dynamics_rhs", inputs, [f], input_names, ["dynamics_rhs_output"]
+            )
+        else:
+            raise ValueError("Unsupported input f")
+
         self._names_map = names_map_in if names_map_in else {}
         self._names_map_inv = {v: k for k, v in self._names_map.items()}  # inverse dict
 
@@ -87,21 +99,52 @@ def outputs(self) -> list[str]:
 class DynamicsLHS:
     _x: list[str] = dataclasses.field(default=None)
     x: dataclasses.InitVar[list[str] | str] = None
-    _t_label: str = "t"
-    t_label: dataclasses.InitVar[str] = None
+    _t_label: str | cs.MX = "t"
+    t_label: dataclasses.InitVar[str | cs.MX] = None
 
-    def __post_init__(self, x: list[str] | str, t_label: str):
+    def __post_init__(
+        self, x: str | list[str] | cs.MX | list[cs.MX], t_label: str | cs.MX = None
+    ):
         """
         Constructs the DynamicsLHS object
         :param x: List of variable names on the left hand side of dot{x} = f(y).
           The list can contain empty strings if some output of f needs to be discarded.
           Refer to the specific optimal control problem for a specification of the
           label convention.
-        :param t_label: The label of the time variable. Default "t"
+          The input can also be of type cs.MX. This allows using the symbolic
+          structure provided by the optimal control solver. The input cannot be an
+          expression. Also in the case, the input can be a list too, and can contain
+          None if some outputs of f have to be discarded.
+        :param t_label: The label of the time variable. Default "t". It can also be a
+          cs.MX. In this case, its name will be used
         :return: Nothing
         """
-        self._x = x if isinstance(x, list) else [x]
-        self._t_label = t_label if isinstance(t_label, str) else "t"
+
+        def input_to_string(
+            input_value: str | cs.MX, default_string: str = None
+        ) -> str:
+            if isinstance(input_value, str):
+                return input_value
+
+            if input_value is None:
+                return ""
+
+            if not isinstance(input_value, cs.MX):
+                if default_string is not None:
+                    return default_string
+
+                raise ValueError("The input can be only a string, a cs.MX, or None.")
+
+            if not input_value.is_symbolic():
+                raise ValueError("The input MX has to be symbolic.")
+            return input_value.name()
+
+        if isinstance(x, list):
+            self._x = [input_to_string(el) for el in x]
+        else:
+            self._x = [input_to_string(x)]
+
+        self._t_label = input_to_string(input_value=t_label, default_string="t")
 
     def equal(
         self, f: cs.Function | str | list[str], names_map: dict[str, str] = None
@@ -119,9 +162,11 @@ def __eq__(
         other: cs.Function
         | str
         | list[str]
+        | cs.MX
         | tuple[cs.Function, dict[str, str]]
         | tuple[str, dict[str, str]]
-        | tuple[list[str], dict[str, str]],
+        | tuple[list[str], dict[str, str]]
+        | tuple[cs.MX, dict[str, str]],
     ) -> TDynamics:
         if isinstance(other, tuple):
             return self.equal(f=other[0], names_map=other[1])
@@ -130,6 +175,7 @@ def __eq__(
             isinstance(other, cs.Function)
             or isinstance(other, str)
             or isinstance(other, list)
+            or isinstance(other, cs.MX)
         )
         return self.equal(f=other)
 
@@ -140,7 +186,7 @@ def time_label(self) -> str:
         return self._t_label
 
 
-def dot(x: str | list[str], t: str = "t") -> TDynamicsLHS:
+def dot(x: str | list[str] | cs.MX | list[cs.MX], t: str | cs.MX = "t") -> TDynamicsLHS:
     return DynamicsLHS(x=x, t_label=t)
 
 
diff --git a/test/test_integrators.py b/test/test_integrators.py
index a17fa060..963e01e7 100644
--- a/test/test_integrators.py
+++ b/test/test_integrators.py
@@ -54,6 +54,18 @@ def test_dynamics_creation():
     assert dynamics.time_name() == "time"
 
 
+def test_dynamics_creation_with_mx():
+    _x = cs.MX.sym("x", 1)
+    _lambda = cs.MX.sym("lambda", 1)
+    _time = cs.MX.sym("time", 1)
+
+    dynamics = dot(_x, _time).equal(_lambda * _x, {"lam": "lambda"})
+
+    assert dynamics.state_variables() == ["x"]
+    assert dynamics.input_names() == ["lam", "x"]
+    assert dynamics.time_name() == "time"
+
+
 def test_forward_euler():
     f = get_test_function()
 

From 36f3bac5df093dd48276baecdba45436cec06c95 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 30 Jul 2023 11:48:40 +0200
Subject: [PATCH 11/57] Added possibility to add expression over horizon

---
 src/hippopt/base/multiple_shooting_solver.py | 86 +++++++++++++++++++-
 src/hippopt/base/optimal_control_problem.py  | 16 ++++
 src/hippopt/base/optimal_control_solver.py   | 11 +++
 3 files changed, 112 insertions(+), 1 deletion(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 3eccb6a1..6a3733f3 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -595,7 +595,7 @@ def add_dynamics(
             dt_size = dt_var_tuple[0]
             dt_generator = dt_var_tuple[1]
         else:
-            raise ValueError("Unsupported dt type")
+            raise ValueError("Unsupported dt type")  # TODO: allow setting dt from MX
 
         dt_tuple = (dt_size, dt_generator)
 
@@ -717,6 +717,90 @@ def add_dynamics(
             x_k = x_next
             u_k = u_next
 
+    def add_expression_to_horizon(  # TODO: add tests
+        self,
+        expression: cs.MX,
+        mode: ExpressionType = ExpressionType.subject_to,
+        apply_to_first_elements: bool = False,
+        name: str = None,
+        **kwargs
+    ) -> None:
+        """
+        Add an expression to the whole horizon of the optimal control problem
+        :param expression: The expression to add. Use the symbolic_structure to setup
+                           expression
+        :param mode: Optional argument to set the mode with which the
+                     dynamics is added to the problem.
+                     Default: constraint
+        :param apply_to_first_elements: Flag to define if the constraint need to be
+                                        applied also to the first elements. If True
+                                        the expression will be applied also to the first
+                                        elements. Default: False
+        :param name: The name used when adding the expression.
+        :param kwargs: Optional arguments:
+                             - "max_steps": the number of integration
+                                            steps. If not specified, the
+                                            number of steps is determined from the
+                                            variables involved.
+                             - optional arguments of the
+                               `Problem.add_expression` method.
+        :return: None
+        """
+
+        input_variables = cs.symvar(expression)
+        input_variables_names = [var.name() for var in input_variables]
+        base_name = name if name is not None else str(expression)
+
+        input_function = cs.Function(
+            base_name, input_variables, [expression], input_variables_names, ["output"]
+        )
+
+        max_n = 0
+
+        if "max_steps" in kwargs:
+            max_n = kwargs["max_steps"]
+
+            if not isinstance(max_n, int) or max_n < 1:
+                raise ValueError(
+                    "max_steps is specified, but it needs to be an integer"
+                    " greater than 0"
+                )
+
+        variables = {}
+        n = max_n
+        all_constant = True
+        for var in input_variables_names:
+            if var not in self._flattened_variables:
+                raise ValueError(
+                    "Variable " + var + " not found in the optimization variables."
+                )
+            var_tuple = self._flattened_variables[var]
+            var_n = var_tuple[0]
+            all_constant = all_constant and var_n == 1
+            n = var_n if 1 < var_n < n else n
+
+            # With var_tuple[1]() we get a new generator for the specific variable
+            variables[var] = (var_tuple[0], var_tuple[1]())
+
+        if all_constant:
+            n = 1
+
+        for i in range(n):
+            x_k = {name: next(variables[name][1]) for name in variables}
+
+            name = base_name + "[" + str(i) + "]"
+
+            if i == 0 and not apply_to_first_elements and not all_constant:
+                continue
+
+            # In the following, we add the expressions through the problem
+            # interface, rather than the solver interface. In this way, we can exploit
+            # the machinery handling the generators, and we can switch the expression
+            # from constraints to costs
+            self.get_problem().add_expression(
+                mode=mode, expression=input_function(x_k)["output"], name=name, **kwargs
+            )
+
     def set_initial_guess(
         self, initial_guess: TOptimizationObject | list[TOptimizationObject]
     ):
diff --git a/src/hippopt/base/optimal_control_problem.py b/src/hippopt/base/optimal_control_problem.py
index 40dac6b2..1eef5992 100644
--- a/src/hippopt/base/optimal_control_problem.py
+++ b/src/hippopt/base/optimal_control_problem.py
@@ -101,3 +101,19 @@ def add_dynamics(
             x0_name=x0_name,
             **kwargs
         )
+
+    def add_expression_to_horizon(
+        self,
+        expression: cs.MX,
+        mode: ExpressionType = ExpressionType.subject_to,
+        apply_to_first_elements: bool = False,
+        name: str = None,
+        **kwargs
+    ) -> None:
+        self.solver().add_expression_to_horizon(
+            expression=expression,
+            mode=mode,
+            apply_to_first_elements=apply_to_first_elements,
+            name=name,
+            **kwargs
+        )
diff --git a/src/hippopt/base/optimal_control_solver.py b/src/hippopt/base/optimal_control_solver.py
index a53ed0d1..593ed2fd 100644
--- a/src/hippopt/base/optimal_control_solver.py
+++ b/src/hippopt/base/optimal_control_solver.py
@@ -30,3 +30,14 @@ def add_dynamics(
     @abc.abstractmethod
     def get_symbolic_structure(self) -> TOptimizationObject | list[TOptimizationObject]:
         pass
+
+    @abc.abstractmethod
+    def add_expression_to_horizon(
+        self,
+        expression: cs.MX,
+        mode: ExpressionType = ExpressionType.subject_to,
+        apply_to_first_elements: bool = False,
+        name: str = None,
+        **kwargs
+    ) -> None:
+        pass

From c7e07ab0afba64da367e638e4b8966199c58d802 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 30 Jul 2023 11:56:27 +0200
Subject: [PATCH 12/57] Added possibility to set dt from cs.MX

---
 src/hippopt/base/multiple_shooting_solver.py | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 6a3733f3..b47a0d8c 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -546,10 +546,12 @@ def add_dynamics(
         :param x0_name: The name used when adding the initial condition expression.
         :param kwargs: Additional arguments. There are some required arguments:
                                             - "dt": the integration time delta.
-                                                    It can either be a float in case it
+                                                    It can be a float in case it
                                                     is constant, or a string to indicate
                                                     the (flattened) name of the
-                                                    variable to use.
+                                                    variable to use, or a cs.MX when
+                                                    using the corresponding variable in
+                                                    the symbolic structure
 
                                             Optional arguments:
                                             - "max_steps": the number of integration
@@ -586,7 +588,8 @@ def add_dynamics(
 
         if isinstance(dt_in, float):
             dt_generator = itertools.repeat(cs.MX(dt_in))
-        elif isinstance(dt_in, str):
+        elif isinstance(dt_in, str) or isinstance(dt_in, cs.MX):
+            dt_in = dt_in.name() if isinstance(dt_in, cs.MX) else dt_in
             if dt_in not in self._flattened_variables:
                 raise ValueError(
                     "The specified dt name is not found in the optimization variables"
@@ -595,7 +598,7 @@ def add_dynamics(
             dt_size = dt_var_tuple[0]
             dt_generator = dt_var_tuple[1]
         else:
-            raise ValueError("Unsupported dt type")  # TODO: allow setting dt from MX
+            raise ValueError("Unsupported dt type")
 
         dt_tuple = (dt_size, dt_generator)
 

From 48792b51902fefbe867dc8f2be57364459c4544f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 30 Jul 2023 12:58:29 +0200
Subject: [PATCH 13/57] Added test for add_expression_to_horizon

---
 src/hippopt/base/multiple_shooting_solver.py | 26 ++++++++------------
 src/hippopt/base/opti_solver.py              |  5 +++-
 test/test_multiple_shooting.py               | 16 +++++++++++-
 3 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index b47a0d8c..86e9904a 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -754,11 +754,7 @@ def add_expression_to_horizon(  # TODO: add tests
         input_variables_names = [var.name() for var in input_variables]
         base_name = name if name is not None else str(expression)
 
-        input_function = cs.Function(
-            base_name, input_variables, [expression], input_variables_names, ["output"]
-        )
-
-        max_n = 0
+        max_n = 1
 
         if "max_steps" in kwargs:
             max_n = kwargs["max_steps"]
@@ -769,9 +765,8 @@ def add_expression_to_horizon(  # TODO: add tests
                     " greater than 0"
                 )
 
-        variables = {}
+        variables_generators = []
         n = max_n
-        all_constant = True
         for var in input_variables_names:
             if var not in self._flattened_variables:
                 raise ValueError(
@@ -779,21 +774,17 @@ def add_expression_to_horizon(  # TODO: add tests
                 )
             var_tuple = self._flattened_variables[var]
             var_n = var_tuple[0]
-            all_constant = all_constant and var_n == 1
-            n = var_n if 1 < var_n < n else n
+            n = var_n if n == 1 or 1 < var_n < n else n
 
             # With var_tuple[1]() we get a new generator for the specific variable
-            variables[var] = (var_tuple[0], var_tuple[1]())
-
-        if all_constant:
-            n = 1
+            variables_generators.append(var_tuple[1]())
 
         for i in range(n):
-            x_k = {name: next(variables[name][1]) for name in variables}
+            x_k = [next(var) for var in variables_generators]
 
             name = base_name + "[" + str(i) + "]"
 
-            if i == 0 and not apply_to_first_elements and not all_constant:
+            if i == 0 and not apply_to_first_elements and not n == 1:
                 continue
 
             # In the following, we add the expressions through the problem
@@ -801,7 +792,10 @@ def add_expression_to_horizon(  # TODO: add tests
             # the machinery handling the generators, and we can switch the expression
             # from constraints to costs
             self.get_problem().add_expression(
-                mode=mode, expression=input_function(x_k)["output"], name=name, **kwargs
+                mode=mode,
+                expression=cs.substitute([expression], input_variables, x_k)[0],
+                name=name,
+                **kwargs
             )
 
     def set_initial_guess(
diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index a73ae00b..c6e0682f 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -285,7 +285,10 @@ def _set_initial_guess_internal(
                             "The guess for the field "
                             + base_name
                             + field.name
-                            + " is supposed to be a list."
+                            + " is supposed to be a list. "
+                            + "Received "
+                            + str(type(guess))
+                            + " instead."
                         )
 
                     if len(corresponding_value) == len(guess):
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 7835d8c3..23d698a5 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -240,24 +240,27 @@ class MassFallingTestVariables(OptimizationObject):
         metadata=time_varying_metadata(), default=None
     )
     g: StorageType = default_storage_field(Parameter)
+    foo: StorageType = default_storage_field(Variable)
 
     def __post_init__(self):
         self.g = -9.81 * np.ones(1)
         self.masses = []
         for _ in range(3):
             self.masses.append(MassFallingState())
+        self.foo = np.zeros((3, 1))
 
 
 def test_multiple_shooting():
     guess = MassFallingTestVariables()
     guess.masses = None
+    guess.foo = None
 
     horizon = 100
     dt = 0.01
     initial_position = 1.0
     initial_velocity = 0
 
-    problem, var, _ = OptimalControlProblem.create(
+    problem, var, symbolic = OptimalControlProblem.create(
         input_structure=MassFallingTestVariables(),
         horizon=horizon,
     )
@@ -292,6 +295,16 @@ def test_multiple_shooting():
         x0_name="initial_condition_simple",
     )
 
+    problem.add_expression_to_horizon(
+        expression=(symbolic.foo >= 5), apply_to_first_elements=True
+    )
+
+    problem.add_expression_to_horizon(
+        expression=cs.sumsqr(symbolic.foo),
+        apply_to_first_elements=True,
+        mode=ExpressionType.minimize,
+    )
+
     problem.set_initial_guess(guess)
 
     sol = problem.solve()
@@ -317,5 +330,6 @@ def test_multiple_shooting():
         assert float(sol.values.masses[1][i].v) == pytest.approx(expected_velocity)
         assert float(sol.values.masses[2][i].x) == pytest.approx(expected_position)
         assert float(sol.values.masses[2][i].v) == pytest.approx(expected_velocity)
+        assert sol.values.foo[i] == pytest.approx(5)
         expected_position += dt * expected_velocity
         expected_velocity += dt * guess.g

From 2394f8f501263de4afa2d101bbf6c9444b50a81b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 1 Aug 2023 17:43:23 +0200
Subject: [PATCH 14/57] Fixed some stylistic issues in opti_solver

---
 src/hippopt/base/opti_solver.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index c6e0682f..6998333b 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -326,7 +326,8 @@ def _set_initial_guess_internal(
                                 + field.name
                                 + "["
                                 + str(i)
-                                + "] does not match with the corresponding optimization variable"
+                                + "] does not match with the corresponding"
+                                + " optimization variable"
                             )
 
                         self._set_opti_guess(
@@ -355,7 +356,8 @@ def _set_initial_guess_internal(
                         "The guess has the field "
                         + base_name
                         + field.name
-                        + " but its dimension does not match with the corresponding optimization variable"
+                        + " but its dimension does not match with the corresponding"
+                        + " optimization variable"
                     )
 
                 self._set_opti_guess(

From 4ef9d4280e8e529e4f521f4a4bfad3405278fc47 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 2 Aug 2023 14:29:28 +0200
Subject: [PATCH 15/57] The maximum number of step for an expression can be 1

---
 src/hippopt/base/multiple_shooting_solver.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 86e9904a..a14c5665 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -720,7 +720,7 @@ def add_dynamics(
             x_k = x_next
             u_k = u_next
 
-    def add_expression_to_horizon(  # TODO: add tests
+    def add_expression_to_horizon(
         self,
         expression: cs.MX,
         mode: ExpressionType = ExpressionType.subject_to,
@@ -755,10 +755,11 @@ def add_expression_to_horizon(  # TODO: add tests
         base_name = name if name is not None else str(expression)
 
         max_n = 1
+        max_iterations_set = False
 
         if "max_steps" in kwargs:
             max_n = kwargs["max_steps"]
-
+            max_iterations_set = True
             if not isinstance(max_n, int) or max_n < 1:
                 raise ValueError(
                     "max_steps is specified, but it needs to be an integer"
@@ -766,7 +767,7 @@ def add_expression_to_horizon(  # TODO: add tests
                 )
 
         variables_generators = []
-        n = max_n
+        n = 1
         for var in input_variables_names:
             if var not in self._flattened_variables:
                 raise ValueError(
@@ -779,6 +780,9 @@ def add_expression_to_horizon(  # TODO: add tests
             # With var_tuple[1]() we get a new generator for the specific variable
             variables_generators.append(var_tuple[1]())
 
+        if max_iterations_set:
+            n = min(max_n, n)
+
         for i in range(n):
             x_k = [next(var) for var in variables_generators]
 

From 83d5d727ebab9612e54f95240c6b7f9dbb3c9ba2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 4 Aug 2023 18:36:45 +0200
Subject: [PATCH 16/57] Added possibility to have scalar optimization objects

---
 src/hippopt/base/opti_solver.py         | 127 +++++++++++++-----------
 src/hippopt/base/optimization_object.py |   2 +-
 test/test_multiple_shooting.py          |   2 +-
 test/test_opti_generate_objects.py      |   6 ++
 4 files changed, 78 insertions(+), 59 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 6998333b..2fab0c14 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -89,6 +89,9 @@ def _generate_opti_object(
             if value.ndim < 2:
                 value = np.expand_dims(value, axis=1)
 
+        if isinstance(value, float):
+            value = value * np.ones((1, 1))
+
         if storage_type is Variable.StorageTypeValue:
             return self._solver.variable(*value.shape)
 
@@ -280,65 +283,14 @@ def _set_initial_guess_internal(
                 )
 
                 if isinstance(corresponding_value, list):
-                    if not isinstance(guess, list):
-                        raise ValueError(
-                            "The guess for the field "
-                            + base_name
-                            + field.name
-                            + " is supposed to be a list. "
-                            + "Received "
-                            + str(type(guess))
-                            + " instead."
-                        )
-
-                    if len(corresponding_value) == len(guess):
-                        raise ValueError(
-                            "The guess for the field "
-                            + base_name
-                            + field.name
-                            + " is a list of the wrong size. Expected: "
-                            + str(len(corresponding_value))
-                            + ". Guess: "
-                            + str(len(guess))
-                        )
-
-                    for i in range(len(corresponding_value)):
-                        if not isinstance(guess[i], np.ndarray):
-                            raise ValueError(
-                                "The guess for the field "
-                                + base_name
-                                + field.name
-                                + "["
-                                + str(i)
-                                + "] is not an numpy array."
-                            )
-
-                        input_shape = (
-                            guess[i].shape
-                            if len(guess[i].shape) > 1
-                            else (guess[i].shape[0], 1)
-                        )
-
-                        if corresponding_value[i].shape != input_shape:
-                            raise ValueError(
-                                "The dimension of the guess for the field "
-                                + base_name
-                                + field.name
-                                + "["
-                                + str(i)
-                                + "] does not match with the corresponding"
-                                + " optimization variable"
-                            )
-
-                        self._set_opti_guess(
-                            storage_type=field.metadata[
-                                OptimizationObject.StorageTypeField
-                            ],
-                            variable=corresponding_value[i],
-                            value=guess[i],
-                        )
+                    self._set_list_object_guess_internal(
+                        base_name, corresponding_value, field, guess
+                    )
                     continue
 
+                if isinstance(guess, float):
+                    guess = guess * np.ones((1, 1))
+
                 if not isinstance(guess, np.ndarray):
                     raise ValueError(
                         "The guess for the field "
@@ -387,6 +339,67 @@ def _set_initial_guess_internal(
             )
             continue
 
+    def _set_list_object_guess_internal(
+        self,
+        base_name: str,
+        corresponding_value: list,
+        field: dataclasses.Field,
+        guess: list,
+    ) -> None:
+        if not isinstance(guess, list):
+            raise ValueError(
+                "The guess for the field "
+                + base_name
+                + field.name
+                + " is supposed to be a list. "
+                + "Received "
+                + str(type(guess))
+                + " instead."
+            )
+        if len(corresponding_value) == len(guess):
+            raise ValueError(
+                "The guess for the field "
+                + base_name
+                + field.name
+                + " is a list of the wrong size. Expected: "
+                + str(len(corresponding_value))
+                + ". Guess: "
+                + str(len(guess))
+            )
+        for i in range(len(corresponding_value)):
+            value = guess[i]
+            if isinstance(value, float):
+                value = value * np.ones((1, 1))
+
+            if not isinstance(value, np.ndarray):
+                raise ValueError(
+                    "The guess for the field "
+                    + base_name
+                    + field.name
+                    + "["
+                    + str(i)
+                    + "] is supposed to be an array (or even a float if scalar)."
+                )
+
+            input_shape = value.shape if len(value.shape) > 1 else (value.shape[0], 1)
+
+            if corresponding_value[i].shape != input_shape:
+                raise ValueError(
+                    "The dimension of the guess for the field "
+                    + base_name
+                    + field.name
+                    + "["
+                    + str(i)
+                    + "] does not match with the corresponding"
+                    + " optimization variable"
+                )
+
+            self._set_opti_guess(
+                storage_type=field.metadata[OptimizationObject.StorageTypeField],
+                variable=corresponding_value[i],
+                value=value,
+            )
+
     def generate_optimization_objects(
         self, input_structure: TOptimizationObject | list[TOptimizationObject], **kwargs
     ) -> TOptimizationObject | list[TOptimizationObject]:
diff --git a/src/hippopt/base/optimization_object.py b/src/hippopt/base/optimization_object.py
index d852e227..7d20c3d2 100644
--- a/src/hippopt/base/optimization_object.py
+++ b/src/hippopt/base/optimization_object.py
@@ -7,7 +7,7 @@
 import numpy as np
 
 TOptimizationObject = TypeVar("TOptimizationObject", bound="OptimizationObject")
-StorageType = cs.MX | np.ndarray | list[cs.MX] | list[np.ndarray]
+StorageType = cs.MX | np.ndarray | float | list[cs.MX] | list[np.ndarray] | list[float]
 
 
 class TimeExpansion(Enum):
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 23d698a5..172737c5 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -243,7 +243,7 @@ class MassFallingTestVariables(OptimizationObject):
     foo: StorageType = default_storage_field(Variable)
 
     def __post_init__(self):
-        self.g = -9.81 * np.ones(1)
+        self.g = -9.81
         self.masses = []
         for _ in range(3):
             self.masses.append(MassFallingState())
diff --git a/test/test_opti_generate_objects.py b/test/test_opti_generate_objects.py
index 15aa9d18..969a68da 100644
--- a/test/test_opti_generate_objects.py
+++ b/test/test_opti_generate_objects.py
@@ -17,10 +17,12 @@
 class CustomVariable(OptimizationObject):
     variable: StorageType = default_storage_field(cls=Variable)
     parameter: StorageType = default_storage_field(cls=Parameter)
+    scalar: StorageType = default_storage_field(cls=Variable)
 
     def __post_init__(self):
         self.variable = np.ones(shape=3)
         self.parameter = np.ones(shape=3)
+        self.scalar = 1.0
 
 
 @dataclasses.dataclass
@@ -44,6 +46,8 @@ def test_generate_objects():
     assert opti_var.aggregated.variable.shape == (3, 1)
     assert isinstance(opti_var.other_parameter, cs.MX)
     assert opti_var.other_parameter.shape == (3, 1)
+    assert isinstance(opti_var.aggregated.scalar, cs.MX)
+    assert opti_var.aggregated.scalar.shape == (1, 1)
     assert opti_var.other == "untouched"
     assert solver.get_optimization_objects() is opti_var
 
@@ -60,6 +64,8 @@ def test_generate_objects_list():
         assert opti_var.aggregated.parameter.shape == (3, 1)
         assert isinstance(opti_var.aggregated.variable, cs.MX)
         assert opti_var.aggregated.variable.shape == (3, 1)
+        assert isinstance(opti_var.aggregated.scalar, cs.MX)
+        assert opti_var.aggregated.scalar.shape == (1, 1)
         assert isinstance(opti_var.other_parameter, cs.MX)
         assert opti_var.other_parameter.shape == (3, 1)
         assert opti_var.other == "untouched"

From 58df128c50db5630c73a4965941fdebdfaeed8b5 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 7 Aug 2023 11:04:58 +0200
Subject: [PATCH 17/57] Added default_composite_field function

---
 src/hippopt/__init__.py                 |  1 +
 src/hippopt/base/optimization_object.py |  8 ++++++++
 test/test_multiple_shooting.py          | 15 ++++++++-------
 test/test_opti_generate_objects.py      |  3 ++-
 4 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index cda4ddb6..9860ac5a 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -11,6 +11,7 @@
     StorageType,
     TimeExpansion,
     TOptimizationObject,
+    default_composite_field,
     default_storage_field,
     default_storage_metadata,
     time_varying_metadata,
diff --git a/src/hippopt/base/optimization_object.py b/src/hippopt/base/optimization_object.py
index 7d20c3d2..341777ae 100644
--- a/src/hippopt/base/optimization_object.py
+++ b/src/hippopt/base/optimization_object.py
@@ -8,6 +8,7 @@
 
 TOptimizationObject = TypeVar("TOptimizationObject", bound="OptimizationObject")
 StorageType = cs.MX | np.ndarray | float | list[cs.MX] | list[np.ndarray] | list[float]
+TGenericCompositeObject = TypeVar("TGenericCompositeObject")
 
 
 class TimeExpansion(Enum):
@@ -42,3 +43,10 @@ def default_storage_field(cls: Type[OptimizationObject], **kwargs):
 
 def time_varying_metadata(time_varying: bool = True):
     return {OptimizationObject.TimeDependentField: time_varying}
+
+
+def default_composite_field(factory=None, time_varying: bool = True):
+    return dataclasses.field(
+        default_factory=factory,
+        metadata=time_varying_metadata(time_varying),
+    )
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 172737c5..c937c569 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -13,6 +13,7 @@
     StorageType,
     TimeExpansion,
     Variable,
+    default_composite_field,
     default_storage_field,
     dot,
     integrators,
@@ -33,19 +34,19 @@ def __post_init__(self):
 
 @dataclasses.dataclass
 class MyCompositeTestVar(OptimizationObject):
-    composite: MyTestVarMS | list[MyTestVarMS] = dataclasses.field(
-        default_factory=MyTestVarMS, metadata=time_varying_metadata()
+    composite: MyTestVarMS | list[MyTestVarMS] = default_composite_field(
+        factory=MyTestVarMS
     )
-    fixed: MyTestVarMS | list[MyTestVarMS] = dataclasses.field(
-        default_factory=MyTestVarMS
+    fixed: MyTestVarMS | list[MyTestVarMS] = default_composite_field(
+        factory=MyTestVarMS, time_varying=False
     )
     extended: StorageType = default_storage_field(
         cls=Variable, time_expansion=TimeExpansion.Matrix
     )
 
-    composite_list: list[MyTestVarMS] | list[list[MyTestVarMS]] = dataclasses.field(
-        default=None, metadata=time_varying_metadata()
-    )
+    composite_list: list[MyTestVarMS] | list[
+        list[MyTestVarMS]
+    ] = default_composite_field(factory=list[MyTestVarMS])
 
     fixed_list: list[MyTestVarMS] = dataclasses.field(default=None)
 
diff --git a/test/test_opti_generate_objects.py b/test/test_opti_generate_objects.py
index 969a68da..f85f93d1 100644
--- a/test/test_opti_generate_objects.py
+++ b/test/test_opti_generate_objects.py
@@ -9,6 +9,7 @@
     Parameter,
     StorageType,
     Variable,
+    default_composite_field,
     default_storage_field,
 )
 
@@ -27,7 +28,7 @@ def __post_init__(self):
 
 @dataclasses.dataclass
 class AggregateClass(OptimizationObject):
-    aggregated: CustomVariable = dataclasses.field(default_factory=CustomVariable)
+    aggregated: CustomVariable = default_composite_field(factory=CustomVariable)
     other_parameter: StorageType = default_storage_field(cls=Parameter)
     other: str = ""
 

From f35f94c4eca0d468f7cdcb69c4bfaefef8814656 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 7 Aug 2023 15:48:59 +0200
Subject: [PATCH 18/57] Added possibility to set initial conditions from
 symbolic structure

---
 src/hippopt/base/multiple_shooting_solver.py | 33 +++++++++++++-------
 src/hippopt/base/optimal_control_problem.py  |  2 +-
 src/hippopt/base/optimal_control_solver.py   |  2 +-
 test/test_multiple_shooting.py               | 18 ++++++++---
 4 files changed, 37 insertions(+), 18 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index a14c5665..5e4e15d5 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -217,7 +217,7 @@ def generate_optimization_objects(
 
         return variables
 
-    def _generate_flattened_and_symbolic_objects(
+    def _generate_flattened_and_symbolic_objects(  # TODO: remove some indentation
         self,
         object_in: TOptimizationObject,
         top_level: bool = True,
@@ -340,7 +340,7 @@ def _generate_flattened_and_symbolic_objects(
 
             if isinstance(
                 field_value, OptimizationObject
-            ):  # aggregate (cannot be time dependent)
+            ):  # aggregate (cannot be time-dependent)
                 generator = (
                     partial(
                         (
@@ -427,7 +427,7 @@ def _generate_flattened_and_symbolic_objects(
                     isinstance(el, type(first)) for el in iterable
                 )  # check that each element has same type
 
-                # If we are time dependent (and hence top_level has to be true),
+                # If we are time-dependent (and hence top_level has to be true),
                 # there is no base generator
                 new_generator = partial(
                     (lambda value: (val for val in value)), field_value
@@ -512,7 +512,7 @@ def get_symbolic_structure(self) -> TOptimizationObject | list[TOptimizationObje
     def add_dynamics(
         self,
         dynamics: TDynamics,
-        x0: dict[str, cs.MX] = None,
+        x0: dict[str, cs.MX] | dict[cs.MX, cs.MX] | cs.MX = None,
         t0: cs.MX = cs.MX(0.0),
         mode: ExpressionType = ExpressionType.subject_to,
         name: str = None,
@@ -528,15 +528,17 @@ def add_dynamics(
                          If there is a list, you can use "[k]" with "k" the element
                          to pick. For example "a.b[k].c" will look for variable "c"
                          defined in the k-th element of "b" within "a".
-                         Only the top level variables can be time dependent.
-                         In this case, "a" could be time dependent and being a list,
+                         Only the top level variables can be time-dependent.
+                         In this case, "a" could be time-dependent and being a list,
                          but this is automatically detected, and there is no need to
                          specify the time-dependency. The "[k]" keyword is used only
                          in case the list is not time-dependent.
                          In case we have a list of optimization objects, prepend "[k]."
                          to name of the variable, with k the top level index.
         :param x0: The initial state. It is a dictionary with the key equal to the state
-                   variable name. If no dict is provided, or a given variable is not
+                   variable name, or its corresponding symbolic variable.
+                   It can also be a single MX in case there is only one state variable.
+                   If no dict is provided, or a given variable is not
                    found in the dictionary, the initial condition is not set.
         :param t0: The initial time
         :param mode: Optional argument to set the mode with which the
@@ -672,9 +674,17 @@ def add_dynamics(
 
         initial_conditions = {}
         if x0 is not None:
-            for var in x_k:
-                if var in x0:
-                    initial_conditions[var] = x0[var]
+            if not isinstance(x0, dict):
+                if len(x_k) > 1:
+                    raise ValueError(
+                        "The initial condition is a single MX, but the dynamics "
+                        "has more than one state variable."
+                    )
+                x0 = {list(x_k.keys())[0]: x0}
+            for var in x0:
+                var_name = var if isinstance(var, str) else var.name()  # noqa
+                if var_name in x_k:
+                    initial_conditions[var_name] = x0[var]
 
             # In the following, we add the initial condition expressions
             # through the problem interface.
@@ -683,7 +693,8 @@ def add_dynamics(
             self.get_problem().add_expression(
                 mode=mode,
                 expression=(
-                    cs.MX(x_k[name] == x0[name]) for name in initial_conditions
+                    cs.MX(x_k[name] == initial_conditions[name])
+                    for name in initial_conditions
                 ),
                 name=x0_name,
                 **kwargs
diff --git a/src/hippopt/base/optimal_control_problem.py b/src/hippopt/base/optimal_control_problem.py
index 1eef5992..e277d682 100644
--- a/src/hippopt/base/optimal_control_problem.py
+++ b/src/hippopt/base/optimal_control_problem.py
@@ -85,7 +85,7 @@ def create(
     def add_dynamics(
         self,
         dynamics: TDynamics,
-        x0: dict[str, cs.MX] = None,
+        x0: dict[str, cs.MX] | dict[cs.MX, cs.MX] | cs.MX = None,
         t0: cs.MX = cs.MX(0.0),
         mode: ExpressionType = ExpressionType.subject_to,
         name: str = None,
diff --git a/src/hippopt/base/optimal_control_solver.py b/src/hippopt/base/optimal_control_solver.py
index 593ed2fd..b0650178 100644
--- a/src/hippopt/base/optimal_control_solver.py
+++ b/src/hippopt/base/optimal_control_solver.py
@@ -18,7 +18,7 @@ class OptimalControlSolver(OptimizationSolver):
     def add_dynamics(
         self,
         dynamics: TDynamics,
-        x0: dict[str, cs.MX] = None,
+        x0: dict[str, cs.MX] | dict[cs.MX, cs.MX] | cs.MX = None,
         t0: cs.MX = cs.MX(0.0),
         mode: ExpressionType = ExpressionType.subject_to,
         name: str = None,
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index c937c569..20ae084a 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -289,11 +289,19 @@ def test_multiple_shooting():
     )
 
     problem.add_dynamics(
-        dot(["masses[2].x", "masses[2].v"]) == ["masses[2].v", "g"],
+        dot(symbolic.masses[2].x) == symbolic.masses[2].v,
         dt=dt,
-        x0={"masses[2].x": initial_position, "masses[2].v": initial_velocity},
+        x0=initial_position,
         integrator=integrators.ForwardEuler,
-        x0_name="initial_condition_simple",
+        x0_name="initial_condition_simple_x",
+    )
+
+    problem.add_dynamics(
+        dot(symbolic.masses[2].v) == ["g"],
+        dt=dt,
+        x0={symbolic.masses[2].v: initial_velocity},
+        integrator=integrators.ForwardEuler,
+        x0_name="initial_condition_simple_v",
     )
 
     problem.add_expression_to_horizon(
@@ -318,8 +326,8 @@ def test_multiple_shooting():
     assert "initial_condition{0}" in problem.get_cost_expressions()
     assert "initial_condition{1}" in problem.get_cost_expressions()
     assert "initial_position" in sol.constraint_multipliers
-    assert "initial_condition_simple{0}" in sol.constraint_multipliers
-    assert "initial_condition_simple{1}" in sol.constraint_multipliers
+    assert "initial_condition_simple_x{0}" in sol.constraint_multipliers
+    assert "initial_condition_simple_v{0}" in sol.constraint_multipliers
 
     expected_position = initial_position
     expected_velocity = initial_velocity

From 2bae503caf92bb0142f85d1fbd6c5d41a51466c2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 8 Aug 2023 18:05:43 +0200
Subject: [PATCH 19/57] Added names to expressions

In multiple shooting solver, exploit the base name in case the initial condition name is not set
---
 src/hippopt/base/multiple_shooting_solver.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 5e4e15d5..bc60bbf5 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -686,6 +686,9 @@ def add_dynamics(
                 if var_name in x_k:
                     initial_conditions[var_name] = x0[var]
 
+            if x0_name is None and name is not None:
+                x0_name = name + "[0]"
+
             # In the following, we add the initial condition expressions
             # through the problem interface.
             # In this way, we can exploit the machinery handling the generators,
@@ -715,7 +718,7 @@ def add_dynamics(
                 t0=t0 + cs.MX(i) * dt,
             )
 
-            name = base_name + "[" + str(i) + "]"
+            name = base_name + "[" + str(i + 1) + "]"
 
             # In the following, we add the dynamics expressions through the problem
             # interface, rather than the solver interface. In this way, we can exploit

From 9718953f3f5988408408c3ecb12df7b158aea91c Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 9 Aug 2023 13:43:09 +0200
Subject: [PATCH 20/57] Added one missing typehint in dynamics.py

---
 src/hippopt/base/dynamics.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/hippopt/base/dynamics.py b/src/hippopt/base/dynamics.py
index 0e6166f3..5af62621 100644
--- a/src/hippopt/base/dynamics.py
+++ b/src/hippopt/base/dynamics.py
@@ -23,7 +23,7 @@ def __post_init__(
         Create the DynamicsRHS object
         :param f: The CasADi function describing the dynamics. The output order should
          match the list provided in the dot function. As an alternative, if the
-         dynamics is trivial (e.g dot(x) = y), it is possible to pass directly the name
+         dynamics is trivial (e.g. dot(x) = y), it is possible to pass directly the name
          of the variable in the right-hand-side, or the list of variables in case the
          left-hand-side is a list.
         :param names_map_in: A dict describing how to switch from the input names to
@@ -147,7 +147,7 @@ def input_to_string(
         self._t_label = input_to_string(input_value=t_label, default_string="t")
 
     def equal(
-        self, f: cs.Function | str | list[str], names_map: dict[str, str] = None
+        self, f: cs.Function | str | list[str] | cs.MX, names_map: dict[str, str] = None
     ) -> TDynamics:
         rhs = DynamicsRHS(f=f, names_map_in=names_map)
         if len(rhs.outputs()) != len(self._x):

From 1253c749f597b850c1cc41920fdf4ee41292950b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 9 Aug 2023 16:13:45 +0200
Subject: [PATCH 21/57] Added definition of CompositeType

This automatically adds the union of the type with a list of the same type
---
 src/hippopt/__init__.py                 |  1 +
 src/hippopt/base/optimization_object.py |  3 ++-
 test/test_multiple_shooting.py          | 15 +++++++--------
 test/test_opti_generate_objects.py      |  5 ++++-
 4 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index 9860ac5a..f8f24754 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -7,6 +7,7 @@
     OptimalControlProblemInstance,
 )
 from .base.optimization_object import (
+    CompositeType,
     OptimizationObject,
     StorageType,
     TimeExpansion,
diff --git a/src/hippopt/base/optimization_object.py b/src/hippopt/base/optimization_object.py
index 341777ae..c8a9c59a 100644
--- a/src/hippopt/base/optimization_object.py
+++ b/src/hippopt/base/optimization_object.py
@@ -7,8 +7,9 @@
 import numpy as np
 
 TOptimizationObject = TypeVar("TOptimizationObject", bound="OptimizationObject")
-StorageType = cs.MX | np.ndarray | float | list[cs.MX] | list[np.ndarray] | list[float]
 TGenericCompositeObject = TypeVar("TGenericCompositeObject")
+CompositeType = TGenericCompositeObject | list[TGenericCompositeObject]
+StorageType = CompositeType[cs.MX] | CompositeType[np.ndarray] | CompositeType[float]
 
 
 class TimeExpansion(Enum):
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 20ae084a..e2e1a47a 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -5,6 +5,7 @@
 import pytest
 
 from hippopt import (
+    CompositeType,
     ExpressionType,
     MultipleShootingSolver,
     OptimalControlProblem,
@@ -34,19 +35,17 @@ def __post_init__(self):
 
 @dataclasses.dataclass
 class MyCompositeTestVar(OptimizationObject):
-    composite: MyTestVarMS | list[MyTestVarMS] = default_composite_field(
-        factory=MyTestVarMS
-    )
-    fixed: MyTestVarMS | list[MyTestVarMS] = default_composite_field(
+    composite: CompositeType[MyTestVarMS] = default_composite_field(factory=MyTestVarMS)
+    fixed: CompositeType[MyTestVarMS] = default_composite_field(
         factory=MyTestVarMS, time_varying=False
     )
     extended: StorageType = default_storage_field(
         cls=Variable, time_expansion=TimeExpansion.Matrix
     )
 
-    composite_list: list[MyTestVarMS] | list[
-        list[MyTestVarMS]
-    ] = default_composite_field(factory=list[MyTestVarMS])
+    composite_list: CompositeType[list[MyTestVarMS]] = default_composite_field(
+        factory=list[MyTestVarMS]
+    )
 
     fixed_list: list[MyTestVarMS] = dataclasses.field(default=None)
 
@@ -237,7 +236,7 @@ def get_dynamics():
 
 @dataclasses.dataclass
 class MassFallingTestVariables(OptimizationObject):
-    masses: list[MassFallingState] = dataclasses.field(
+    masses: CompositeType[list[MassFallingState]] = dataclasses.field(
         metadata=time_varying_metadata(), default=None
     )
     g: StorageType = default_storage_field(Parameter)
diff --git a/test/test_opti_generate_objects.py b/test/test_opti_generate_objects.py
index f85f93d1..f51027eb 100644
--- a/test/test_opti_generate_objects.py
+++ b/test/test_opti_generate_objects.py
@@ -4,6 +4,7 @@
 import numpy as np
 
 from hippopt import (
+    CompositeType,
     OptimizationObject,
     OptiSolver,
     Parameter,
@@ -28,7 +29,9 @@ def __post_init__(self):
 
 @dataclasses.dataclass
 class AggregateClass(OptimizationObject):
-    aggregated: CustomVariable = default_composite_field(factory=CustomVariable)
+    aggregated: CompositeType[CustomVariable] = default_composite_field(
+        factory=CustomVariable
+    )
     other_parameter: StorageType = default_storage_field(cls=Parameter)
     other: str = ""
 

From 5cbada90f30b9d8ca85f18d6aa88963317843fa2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 9 Aug 2023 17:13:51 +0200
Subject: [PATCH 22/57] Added utility function "initial" to retrieve the first
 element of a variable

---
 src/hippopt/base/multiple_shooting_solver.py | 27 ++++++++++++++++++--
 src/hippopt/base/optimal_control_problem.py  |  5 +++-
 src/hippopt/base/optimal_control_solver.py   |  4 +++
 src/hippopt/base/optimization_problem.py     |  2 +-
 test/test_multiple_shooting.py               |  9 +++++--
 5 files changed, 41 insertions(+), 6 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index bc60bbf5..2d044944 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -239,7 +239,7 @@ def _generate_flattened_and_symbolic_objects(  # TODO: remove some indentation
             time_dependent = (
                 OptimizationObject.TimeDependentField in field.metadata
                 and field.metadata[OptimizationObject.TimeDependentField]
-                and top_level  # only top level variables can be time dependent
+                and top_level  # only top level variables can be time-dependent
             )
 
             expand_storage = (
@@ -744,7 +744,7 @@ def add_expression_to_horizon(
     ) -> None:
         """
         Add an expression to the whole horizon of the optimal control problem
-        :param expression: The expression to add. Use the symbolic_structure to setup
+        :param expression: The expression to add. Use the symbolic_structure to set up
                            expression
         :param mode: Optional argument to set the mode with which the
                      dynamics is added to the problem.
@@ -816,6 +816,29 @@ def add_expression_to_horizon(
                 **kwargs
             )
 
+    def initial(self, variable: str | cs.MX) -> cs.MX:
+        """
+        Get the initial value of a variable
+        :param variable: The name of the flattened variable.
+                         If the variable is nested, you can use "." as separator
+                         (e.g. "a.b" will look for variable b within a).
+                         If there is a list, you can use "[k]" with "k" the element
+                         to pick. For example "a.b[k].c" will look for variable "c"
+                         defined in the k-th element of "b" within "a".
+                         As an alternative it is possible to use the corresponding
+                         variable from the symbolic structure.
+        :return: The first value of the variable
+        """
+        if isinstance(variable, cs.MX):
+            variable = variable.name()
+
+        if variable not in self._flattened_variables:
+            raise ValueError(
+                "Variable " + variable + " not found in the optimization variables."
+            )
+
+        return next(self._flattened_variables[variable][1]())
+
     def set_initial_guess(
         self, initial_guess: TOptimizationObject | list[TOptimizationObject]
     ):
diff --git a/src/hippopt/base/optimal_control_problem.py b/src/hippopt/base/optimal_control_problem.py
index e277d682..2795da40 100644
--- a/src/hippopt/base/optimal_control_problem.py
+++ b/src/hippopt/base/optimal_control_problem.py
@@ -42,7 +42,7 @@ def __post_init__(
 
     def __iter__(self):
         return iter([self.problem, self.all_variables, self.symbolic_structure])
-        # Cannot use astuple here since it would perform a deepcopy
+        # Cannot use convert to tuple here since it would perform a deepcopy
         # and would include InitVars too
 
 
@@ -117,3 +117,6 @@ def add_expression_to_horizon(
             name=name,
             **kwargs
         )
+
+    def initial(self, variable: str | cs.MX) -> cs.MX:
+        return self.solver().initial(variable=variable)
diff --git a/src/hippopt/base/optimal_control_solver.py b/src/hippopt/base/optimal_control_solver.py
index b0650178..4900656e 100644
--- a/src/hippopt/base/optimal_control_solver.py
+++ b/src/hippopt/base/optimal_control_solver.py
@@ -41,3 +41,7 @@ def add_expression_to_horizon(
         **kwargs
     ) -> None:
         pass
+
+    @abc.abstractmethod
+    def initial(self, variable: str | cs.MX) -> cs.MX:
+        pass
diff --git a/src/hippopt/base/optimization_problem.py b/src/hippopt/base/optimization_problem.py
index b7726473..4426eecc 100644
--- a/src/hippopt/base/optimization_problem.py
+++ b/src/hippopt/base/optimization_problem.py
@@ -28,7 +28,7 @@ def __post_init__(
 
     def __iter__(self):
         return iter([self.problem, self.variables])
-        # Cannot use astuple here since it would perform a deepcopy
+        # Cannot convert to tuple here since it would perform a deepcopy
         # and would include InitVars too
 
 
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index e2e1a47a..8c4cd1cb 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -304,9 +304,11 @@ def test_multiple_shooting():
     )
 
     problem.add_expression_to_horizon(
-        expression=(symbolic.foo >= 5), apply_to_first_elements=True
+        expression=(symbolic.foo >= 5), apply_to_first_elements=False
     )
 
+    problem.add_constraint(expression=problem.initial(symbolic.foo) == 0)
+
     problem.add_expression_to_horizon(
         expression=cs.sumsqr(symbolic.foo),
         apply_to_first_elements=True,
@@ -338,6 +340,9 @@ def test_multiple_shooting():
         assert float(sol.values.masses[1][i].v) == pytest.approx(expected_velocity)
         assert float(sol.values.masses[2][i].x) == pytest.approx(expected_position)
         assert float(sol.values.masses[2][i].v) == pytest.approx(expected_velocity)
-        assert sol.values.foo[i] == pytest.approx(5)
+        if i == 0:
+            assert sol.values.foo[i] == pytest.approx(0)
+        else:
+            assert sol.values.foo[i] == pytest.approx(5)
         expected_position += dt * expected_velocity
         expected_velocity += dt * guess.g

From 2c02b5310e6ed2133bad2c36f7ff75b0905ba196 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 9 Aug 2023 17:39:00 +0200
Subject: [PATCH 23/57] Added function to get the last element of a given
 variable

---
 src/hippopt/base/multiple_shooting_solver.py | 32 ++++++++++++++++++++
 src/hippopt/base/optimal_control_problem.py  |  3 ++
 src/hippopt/base/optimal_control_solver.py   |  4 +++
 test/test_multiple_shooting.py               |  5 +++
 4 files changed, 44 insertions(+)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 2d044944..16052fda 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -839,6 +839,38 @@ def initial(self, variable: str | cs.MX) -> cs.MX:
 
         return next(self._flattened_variables[variable][1]())
 
+    def final(self, variable: str | cs.MX) -> cs.MX:
+        """
+        Get the final value of a variable
+        :param variable: The name of the flattened variable.
+                         If the variable is nested, you can use "." as separator
+                         (e.g. "a.b" will look for variable b within a).
+                         If there is a list, you can use "[k]" with "k" the element
+                         to pick. For example "a.b[k].c" will look for variable "c"
+                         defined in the k-th element of "b" within "a".
+                         As an alternative it is possible to use the corresponding
+                         variable from the symbolic structure.
+        :return: The last value of the variable
+        """
+        if isinstance(variable, cs.MX):
+            variable = variable.name()
+
+        if variable not in self._flattened_variables:
+            raise ValueError(
+                "Variable " + variable + " not found in the optimization variables."
+            )
+
+        flattened = self._flattened_variables[variable]
+
+        if flattened[0] == 1:
+            return next(flattened[1]())
+        else:
+            generator = flattened[1]()
+            final_value = None
+            for value in generator:
+                final_value = value
+            return final_value
+
     def set_initial_guess(
         self, initial_guess: TOptimizationObject | list[TOptimizationObject]
     ):
diff --git a/src/hippopt/base/optimal_control_problem.py b/src/hippopt/base/optimal_control_problem.py
index 2795da40..610ad26d 100644
--- a/src/hippopt/base/optimal_control_problem.py
+++ b/src/hippopt/base/optimal_control_problem.py
@@ -120,3 +120,6 @@ def add_expression_to_horizon(
 
     def initial(self, variable: str | cs.MX) -> cs.MX:
         return self.solver().initial(variable=variable)
+
+    def final(self, variable: str | cs.MX) -> cs.MX:
+        return self.solver().final(variable=variable)
diff --git a/src/hippopt/base/optimal_control_solver.py b/src/hippopt/base/optimal_control_solver.py
index 4900656e..83abf79e 100644
--- a/src/hippopt/base/optimal_control_solver.py
+++ b/src/hippopt/base/optimal_control_solver.py
@@ -45,3 +45,7 @@ def add_expression_to_horizon(
     @abc.abstractmethod
     def initial(self, variable: str | cs.MX) -> cs.MX:
         pass
+
+    @abc.abstractmethod
+    def final(self, variable: str | cs.MX) -> cs.MX:
+        pass
diff --git a/test/test_multiple_shooting.py b/test/test_multiple_shooting.py
index 8c4cd1cb..772147b9 100644
--- a/test/test_multiple_shooting.py
+++ b/test/test_multiple_shooting.py
@@ -265,6 +265,8 @@ def test_multiple_shooting():
         horizon=horizon,
     )
 
+    assert problem.initial(symbolic.g) is problem.final(symbolic.g)
+
     problem.add_dynamics(
         dot(["masses[0].x", "masses[0].v"])
         == (MassFallingState.get_dynamics(), {"masses[0].x": "x", "masses[0].v": "v"}),
@@ -308,6 +310,7 @@ def test_multiple_shooting():
     )
 
     problem.add_constraint(expression=problem.initial(symbolic.foo) == 0)
+    problem.add_constraint(expression=problem.final(symbolic.foo) == 6.0)
 
     problem.add_expression_to_horizon(
         expression=cs.sumsqr(symbolic.foo),
@@ -342,6 +345,8 @@ def test_multiple_shooting():
         assert float(sol.values.masses[2][i].v) == pytest.approx(expected_velocity)
         if i == 0:
             assert sol.values.foo[i] == pytest.approx(0)
+        elif i == horizon - 1:
+            assert sol.values.foo[i] == pytest.approx(6)
         else:
             assert sol.values.foo[i] == pytest.approx(5)
         expected_position += dt * expected_velocity

From 943e2dacd53f2d48e7d56dda9d497006e4d4b5c2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 11 Aug 2023 13:23:52 +0200
Subject: [PATCH 24/57] The initial guess is set automatically when setting the
 structure

It is also possible to retrieve the guess back once it has been set
---
 src/hippopt/base/multiple_shooting_solver.py | 10 ++-
 src/hippopt/base/opti_solver.py              | 66 ++++++++++++++++----
 src/hippopt/base/optimization_solver.py      |  4 ++
 src/hippopt/base/problem.py                  |  3 +
 4 files changed, 69 insertions(+), 14 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index 16052fda..c1348b83 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -131,10 +131,11 @@ def _extend_structure_to_horizon(
                             ' Consider using "TimeExpansion.List"'
                             " as time_expansion strategy."
                         )
-                    # This is only needed to get the structure
-                    # for the optimization variables.
+                    # Repeat the vector along the horizon
+                    if field_value.ndim < 2:
+                        field_value = np.expand_dims(field_value, axis=1)
                     output.__setattr__(
-                        field.name, np.zeros((field_value.shape[0], horizon_length))
+                        field.name, np.tile(field_value, (1, horizon_length))
                     )
                 else:
                     output_value = []
@@ -876,6 +877,9 @@ def set_initial_guess(
     ):
         self._optimization_solver.set_initial_guess(initial_guess=initial_guess)
 
+    def get_initial_guess(self) -> TOptimizationObject | list[TOptimizationObject]:
+        return self._optimization_solver.get_initial_guess()
+
     def solve(self) -> None:
         self._optimization_solver.solve()
 
diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 2fab0c14..68e41ec5 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -25,6 +25,15 @@ def __init__(self, message: Exception):
         super().__init__("Opti failed to solve the problem. Message: " + str(message))
 
 
+class InitialGuessFailure(Exception):
+    def __init__(self, message: Exception):
+        super().__init__(
+            f"Failed to set the initial guess. Message: {message}. "
+            "Use 'fill_initial_guess=False' to avoid filling the "
+            "initial guess automatically."
+        )
+
+
 @dataclasses.dataclass
 class OptiSolver(OptimizationSolver):
     DefaultSolverType: ClassVar[str] = "ipopt"
@@ -55,6 +64,9 @@ class OptiSolver(OptimizationSolver):
         default=None
     )
     _problem: Problem = dataclasses.field(default=None)
+    _guess: TOptimizationObject | list[TOptimizationObject] = dataclasses.field(
+        default=None
+    )
 
     def __post_init__(
         self,
@@ -263,6 +275,16 @@ def _set_initial_guess_internal(
                 )
             return
 
+        # Check that the initial guess is an optimization object
+        if not isinstance(initial_guess, OptimizationObject):
+            raise ValueError(
+                "The initial guess for the variable "
+                + base_name
+                + " is not an optimization object."
+                + " It is of type "
+                + str(type(initial_guess))
+            )
+
         for field in dataclasses.fields(initial_guess):
             guess = initial_guess.__getattribute__(field.name)
 
@@ -321,14 +343,16 @@ def _set_initial_guess_internal(
 
             composite_variable_guess = initial_guess.__getattribute__(field.name)
 
-            if isinstance(composite_variable_guess, OptimizationObject):
-                if not hasattr(corresponding_variable, field.name):
-                    raise ValueError(
-                        "The guess has the field "
-                        + base_name
-                        + field.name
-                        + " but it is not present in the optimization structure"
-                    )
+            if not isinstance(composite_variable_guess, OptimizationObject):
+                continue
+
+            if not hasattr(corresponding_variable, field.name):
+                raise ValueError(
+                    "The guess has the field "
+                    + base_name
+                    + field.name
+                    + " but it is not present in the optimization structure"
+                )
 
             self._set_initial_guess_internal(
                 initial_guess=composite_variable_guess,
@@ -356,7 +380,7 @@ def _set_list_object_guess_internal(
                 + str(type(guess))
                 + " instead."
             )
-        if len(corresponding_value) == len(guess):
+        if len(corresponding_value) != len(guess):
             raise ValueError(
                 "The guess for the field "
                 + base_name
@@ -404,8 +428,23 @@ def generate_optimization_objects(
         self, input_structure: TOptimizationObject | list[TOptimizationObject], **kwargs
     ) -> TOptimizationObject | list[TOptimizationObject]:
         if isinstance(input_structure, OptimizationObject):
-            return self._generate_objects_from_instance(input_structure=input_structure)
-        return self._generate_objects_from_list(input_structure=input_structure)
+            output = self._generate_objects_from_instance(
+                input_structure=input_structure
+            )
+        else:
+            output = self._generate_objects_from_list(input_structure=input_structure)
+
+        fill_initial_guess = (
+            kwargs["fill_initial_guess"] if "fill_initial_guess" in kwargs else True
+        )
+
+        if fill_initial_guess:
+            try:
+                self.set_initial_guess(initial_guess=input_structure)
+            except Exception as err:
+                raise InitialGuessFailure(err)
+
+        return output
 
     def get_optimization_objects(
         self,
@@ -427,6 +466,11 @@ def set_initial_guess(
             initial_guess=initial_guess, corresponding_variable=self._variables
         )
 
+        self._guess = initial_guess
+
+    def get_initial_guess(self) -> TOptimizationObject | list[TOptimizationObject]:
+        return self._guess
+
     def set_opti_options(
         self,
         inner_solver: str = None,
diff --git a/src/hippopt/base/optimization_solver.py b/src/hippopt/base/optimization_solver.py
index 03f4c1c6..a4d742dc 100644
--- a/src/hippopt/base/optimization_solver.py
+++ b/src/hippopt/base/optimization_solver.py
@@ -49,6 +49,10 @@ def set_initial_guess(
     ) -> None:
         pass
 
+    @abc.abstractmethod
+    def get_initial_guess(self) -> TOptimizationObject | list[TOptimizationObject]:
+        pass
+
     @abc.abstractmethod
     def solve(self) -> None:
         pass
diff --git a/src/hippopt/base/problem.py b/src/hippopt/base/problem.py
index 6ef990ba..83a66a15 100644
--- a/src/hippopt/base/problem.py
+++ b/src/hippopt/base/problem.py
@@ -66,6 +66,9 @@ def set_initial_guess(
     ) -> None:
         self.solver().set_initial_guess(initial_guess)
 
+    def get_initial_guess(self) -> TOptimizationObject | list[TOptimizationObject]:
+        return self.solver().get_initial_guess()
+
     def add_cost(
         self,
         expression: cs.MX | Generator[cs.MX, None, None],

From b3e4ddb4b7a6f5f87a4d637cb2c8f6ea3f4457cd Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 11 Aug 2023 15:19:07 +0200
Subject: [PATCH 25/57] Added possibility to set casadi options to opti in
 planner

Added method to solve the problem
---
 src/hippopt/__init__.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index f8f24754..c38b68cd 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -20,7 +20,7 @@
 from .base.optimization_problem import OptimizationProblem, OptimizationProblemInstance
 from .base.optimization_solver import SolutionNotAvailableException
 from .base.parameter import Parameter, TParameter
-from .base.problem import ExpressionType, ProblemNotSolvedException
+from .base.problem import ExpressionType, Output, ProblemNotSolvedException
 from .base.single_step_integrator import (
     SingleStepIntegrator,
     TSingleStepIntegrator,

From 36476eda1ce8f4645f114973f9361becaa02f6fe Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 16 Aug 2023 16:03:20 +0200
Subject: [PATCH 26/57] Allow changing the opti inner solver in the post_init

---
 src/hippopt/base/opti_solver.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 68e41ec5..7c1a4943 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -38,6 +38,7 @@ def __init__(self, message: Exception):
 class OptiSolver(OptimizationSolver):
     DefaultSolverType: ClassVar[str] = "ipopt"
     _inner_solver: str = dataclasses.field(default=DefaultSolverType)
+    inner_solver: dataclasses.InitVar[str] = dataclasses.field(default=None)
     problem_type: dataclasses.InitVar[str] = dataclasses.field(default="nlp")
 
     _options_plugin: dict[str, Any] = dataclasses.field(default_factory=dict)
@@ -70,11 +71,15 @@ class OptiSolver(OptimizationSolver):
 
     def __post_init__(
         self,
-        problem_type: str,
+        inner_solver: str = DefaultSolverType,
+        problem_type: str = "nlp",
         options_solver: dict[str, Any] = None,
         options_plugin: dict[str, Any] = None,
     ):
         self._solver = cs.Opti(problem_type)
+        self._inner_solver = (
+            inner_solver if inner_solver is not None else self.DefaultSolverType
+        )
         self._options_solver = (
             options_solver if isinstance(options_solver, dict) else {}
         )

From 2179034cf5e32a22661d7f720360feea0b75b06c Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 16 Aug 2023 16:35:19 +0200
Subject: [PATCH 27/57] Added idyntree as a dependency

This will be necessary to run turnkey planners
---
 .github/workflows/ci_cd.yml | 2 +-
 setup.cfg                   | 3 +++
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index 50e1e455..6dba1d4a 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -37,7 +37,7 @@ jobs:
       - name: Dependencies
         shell: bash -l {0}
         run: |
-          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics
+          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree
 
       - name: Install
         shell: bash  -l {0}
diff --git a/setup.cfg b/setup.cfg
index 545f594b..f7a5785d 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -62,10 +62,13 @@ testing=
 robot_planning=
     liecasadi
     adam-robotics
+turnkey_planners=
+    idyntree
 all =
     %(style)s
     %(testing)s
     %(robot_planning)s
+    %(turnkey_planners)s
 
 [options.packages.find]
 where = src

From 5b7d7636863c64c5cfa2b85d811444625b243d75 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 16 Aug 2023 19:12:58 +0200
Subject: [PATCH 28/57] Avoiding setting the initial guess when calling
 generate_optimization_objects recursively

---
 src/hippopt/base/opti_solver.py | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 7c1a4943..f8e9b1b5 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -103,6 +103,9 @@ def _generate_opti_object(
                 raise ValueError(
                     "Field " + name + " has number of dimensions greater than 2."
                 )
+            if value.ndim == 0:
+                raise ValueError("Field " + name + " is a zero-dimensional vector.")
+
             if value.ndim < 2:
                 value = np.expand_dims(value, axis=1)
 
@@ -136,7 +139,10 @@ def _generate_objects_from_instance(
                 or list_of_optimization_objects
             ):
                 output.__setattr__(
-                    field.name, self.generate_optimization_objects(composite_value)
+                    field.name,
+                    self.generate_optimization_objects(
+                        input_structure=composite_value, fill_initial_guess=False
+                    ),
                 )
                 continue
 
@@ -173,7 +179,9 @@ def _generate_objects_from_list(
 
         output = copy.deepcopy(input_structure)
         for i in range(len(output)):
-            output[i] = self.generate_optimization_objects(output[i])
+            output[i] = self.generate_optimization_objects(
+                input_structure=output[i], fill_initial_guess=False
+            )
 
         self._variables = output
         return output

From 6ea8f58d1067be969140db15f1aced1d1cd241f5 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 16 Aug 2023 19:14:22 +0200
Subject: [PATCH 29/57] Fix of extend_structure_to_horizon when the horizon is
 not specified

---
 src/hippopt/base/multiple_shooting_solver.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index c1348b83..e231c324 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -61,13 +61,12 @@ def __post_init__(
         )
         self._flattened_variables = {}
 
+    @staticmethod
     def _extend_structure_to_horizon(
-        self, input_structure: TOptimizationObject | list[TOptimizationObject], **kwargs
+        input_structure: TOptimizationObject | list[TOptimizationObject], **kwargs
     ) -> TOptimizationObject | list[TOptimizationObject]:
         if "horizon" not in kwargs and "horizons" not in kwargs:
-            return self._optimization_solver.generate_optimization_objects(
-                input_structure=input_structure, **kwargs
-            )
+            return input_structure
 
         default_horizon_length = int(1)
         if "horizon" in kwargs:

From 7bd5b59a65892f7f19179fcdf8d4352cefdc90e3 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 17 Aug 2023 12:48:35 +0200
Subject: [PATCH 30/57] opti_solver converts list of floats to vectors
 automatically

---
 src/hippopt/base/opti_solver.py | 24 +++++++++++++++++++-----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index f8e9b1b5..1e721836 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -133,6 +133,12 @@ def _generate_objects_from_instance(
                 isinstance(elem, OptimizationObject) or isinstance(elem, list)
                 for elem in composite_value
             )
+            list_of_float = is_list and all(
+                isinstance(elem, float) for elem in composite_value
+            )
+            if list_of_float:
+                composite_value = np.array(composite_value)
+                is_list = False
 
             if (
                 isinstance(composite_value, OptimizationObject)
@@ -147,11 +153,7 @@ def _generate_objects_from_instance(
                 continue
 
             if OptimizationObject.StorageTypeField in field.metadata:
-                value_list = []
-                value_field = dataclasses.asdict(output)[field.name]
-                value_list.append(value_field)
-
-                value_list = value_field if is_list else value_list
+                value_list = composite_value if is_list else [composite_value]
                 output_value = []
                 for value in value_list:
                     output_value.append(
@@ -326,6 +328,11 @@ def _set_initial_guess_internal(
                 if isinstance(guess, float):
                     guess = guess * np.ones((1, 1))
 
+                if isinstance(guess, list) and all(
+                    isinstance(elem, float) for elem in guess
+                ):
+                    guess = np.array(guess)
+
                 if not isinstance(guess, np.ndarray):
                     raise ValueError(
                         "The guess for the field "
@@ -440,6 +447,13 @@ def _set_list_object_guess_internal(
     def generate_optimization_objects(
         self, input_structure: TOptimizationObject | list[TOptimizationObject], **kwargs
     ) -> TOptimizationObject | list[TOptimizationObject]:
+        if not isinstance(input_structure, OptimizationObject) and not isinstance(
+            input_structure, list
+        ):
+            raise ValueError(
+                "The input structure is neither an optimization object nor a list."
+            )
+
         if isinstance(input_structure, OptimizationObject):
             output = self._generate_objects_from_instance(
                 input_structure=input_structure

From 004570b5c877a7cce2e4526e14c5ee9ddb6c5dae Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 17 Aug 2023 16:31:09 +0200
Subject: [PATCH 31/57] Fixed use of dt generator in multiple shooting solver

---
 src/hippopt/base/multiple_shooting_solver.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/hippopt/base/multiple_shooting_solver.py b/src/hippopt/base/multiple_shooting_solver.py
index e231c324..a52f19d8 100644
--- a/src/hippopt/base/multiple_shooting_solver.py
+++ b/src/hippopt/base/multiple_shooting_solver.py
@@ -598,7 +598,7 @@ def add_dynamics(
                 )
             dt_var_tuple = self._flattened_variables[dt_in]
             dt_size = dt_var_tuple[0]
-            dt_generator = dt_var_tuple[1]
+            dt_generator = dt_var_tuple[1]()
         else:
             raise ValueError("Unsupported dt type")
 

From 671407af55cc705cb5186094b1cbbddfdb53949b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 12 Sep 2023 15:37:11 +0200
Subject: [PATCH 32/57] Added OverridableParameter and OverridableVariable.

This allows changing an entry from parameter to variable when using it in a composite way
---
 src/hippopt/__init__.py                 |   4 +-
 src/hippopt/base/opti_solver.py         |  93 ++++++++++++++++-----
 src/hippopt/base/optimization_object.py |  18 +++-
 src/hippopt/base/parameter.py           |  13 +++
 src/hippopt/base/variable.py            |  13 +++
 test/test_opti_generate_objects.py      | 106 ++++++++++++++++++++++++
 6 files changed, 223 insertions(+), 24 deletions(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index c38b68cd..8d11dbd0 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -19,11 +19,11 @@
 )
 from .base.optimization_problem import OptimizationProblem, OptimizationProblemInstance
 from .base.optimization_solver import SolutionNotAvailableException
-from .base.parameter import Parameter, TParameter
+from .base.parameter import OverridableParameter, Parameter, TParameter
 from .base.problem import ExpressionType, Output, ProblemNotSolvedException
 from .base.single_step_integrator import (
     SingleStepIntegrator,
     TSingleStepIntegrator,
     step,
 )
-from .base.variable import TVariable, Variable
+from .base.variable import OverridableVariable, TVariable, Variable
diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 1e721836..fe99666a 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -68,6 +68,7 @@ class OptiSolver(OptimizationSolver):
     _guess: TOptimizationObject | list[TOptimizationObject] = dataclasses.field(
         default=None
     )
+    _objects_type_map: dict[cs.MX, str] = dataclasses.field(default=None)
 
     def __post_init__(
         self,
@@ -91,6 +92,7 @@ def __post_init__(
         )
         self._cost_expressions = {}
         self._constraint_expressions = {}
+        self._objects_type_map = {}
 
     def _generate_opti_object(
         self, storage_type: str, name: str, value: StorageType
@@ -113,15 +115,19 @@ def _generate_opti_object(
             value = value * np.ones((1, 1))
 
         if storage_type is Variable.StorageTypeValue:
-            return self._solver.variable(*value.shape)
+            opti_object = self._solver.variable(*value.shape)
+            self._objects_type_map[opti_object] = Variable.StorageTypeValue
+            return opti_object
 
         if storage_type is Parameter.StorageTypeValue:
-            return self._solver.parameter(*value.shape)
+            opti_object = self._solver.parameter(*value.shape)
+            self._objects_type_map[opti_object] = Parameter.StorageTypeValue
+            return opti_object
 
         raise ValueError("Unsupported input storage type")
 
     def _generate_objects_from_instance(
-        self, input_structure: TOptimizationObject
+        self, input_structure: TOptimizationObject, parent_metadata: dict
     ) -> TOptimizationObject:
         output = copy.deepcopy(input_structure)
 
@@ -144,10 +150,34 @@ def _generate_objects_from_instance(
                 isinstance(composite_value, OptimizationObject)
                 or list_of_optimization_objects
             ):
+                new_parent_metadata = parent_metadata
+                has_composite_metadata = (
+                    OptimizationObject.CompositeTypeField in field.metadata
+                    and field.metadata[OptimizationObject.CompositeTypeField]
+                    is not None
+                )
+                if has_composite_metadata:
+                    composite_metadata = field.metadata[
+                        OptimizationObject.CompositeTypeField
+                    ]
+                    use_old_metadata = (
+                        parent_metadata is not None
+                        and OptimizationObject.OverrideIfCompositeField
+                        in composite_metadata
+                        and composite_metadata[
+                            OptimizationObject.OverrideIfCompositeField
+                        ]
+                    )
+
+                    if not use_old_metadata:
+                        new_parent_metadata = composite_metadata
+
                 output.__setattr__(
                     field.name,
                     self.generate_optimization_objects(
-                        input_structure=composite_value, fill_initial_guess=False
+                        input_structure=composite_value,
+                        fill_initial_guess=False,
+                        _parent_metadata=new_parent_metadata,
                     ),
                 )
                 continue
@@ -156,11 +186,26 @@ def _generate_objects_from_instance(
                 value_list = composite_value if is_list else [composite_value]
                 output_value = []
                 for value in value_list:
+                    should_override = (
+                        OptimizationObject.OverrideIfCompositeField in field.metadata
+                        and field.metadata[OptimizationObject.OverrideIfCompositeField]
+                    )
+                    parent_can_override = (
+                        parent_metadata is not None
+                        and OptimizationObject.StorageTypeField in parent_metadata
+                    )
+                    if should_override and parent_can_override:
+                        storage_type = parent_metadata[
+                            OptimizationObject.StorageTypeField
+                        ]
+                    else:
+                        storage_type = field.metadata[
+                            OptimizationObject.StorageTypeField
+                        ]
+
                     output_value.append(
                         self._generate_opti_object(
-                            storage_type=field.metadata[
-                                OptimizationObject.StorageTypeField
-                            ],
+                            storage_type=storage_type,
                             name=field.name,
                             value=value,
                         )
@@ -175,14 +220,16 @@ def _generate_objects_from_instance(
         return output
 
     def _generate_objects_from_list(
-        self, input_structure: list[TOptimizationObject]
+        self, input_structure: list[TOptimizationObject], parent_metadata: dict
     ) -> list[TOptimizationObject]:
         assert isinstance(input_structure, list)
 
         output = copy.deepcopy(input_structure)
         for i in range(len(output)):
             output[i] = self.generate_optimization_objects(
-                input_structure=output[i], fill_initial_guess=False
+                input_structure=output[i],
+                fill_initial_guess=False,
+                _parent_metadata=parent_metadata,
             )
 
         self._variables = output
@@ -243,10 +290,8 @@ def _generate_solution_output(
 
         return output
 
-    def _set_opti_guess(
-        self, storage_type: str, variable: cs.MX, value: np.ndarray
-    ) -> None:
-        match storage_type:
+    def _set_opti_guess(self, variable: cs.MX, value: np.ndarray) -> None:
+        match self._objects_type_map[variable]:
             case Variable.StorageTypeValue:
                 self._solver.set_initial(variable, value)
             case Parameter.StorageTypeValue:
@@ -355,7 +400,6 @@ def _set_initial_guess_internal(
                     )
 
                 self._set_opti_guess(
-                    storage_type=field.metadata[OptimizationObject.StorageTypeField],
                     variable=corresponding_value,
                     value=guess,
                 )
@@ -417,12 +461,13 @@ def _set_list_object_guess_internal(
 
             if not isinstance(value, np.ndarray):
                 raise ValueError(
-                    "The guess for the field "
+                    "The field "
                     + base_name
                     + field.name
                     + "["
                     + str(i)
-                    + "] is supposed to be an array (or even a float if scalar)."
+                    + "] is marked as a variable or a parameter. Its guess "
+                    + "is supposed to be an array (or even a float if scalar)."
                 )
 
             input_shape = value.shape if len(value.shape) > 1 else (value.shape[0], 1)
@@ -439,7 +484,6 @@ def _set_list_object_guess_internal(
                 )
 
             self._set_opti_guess(
-                storage_type=field.metadata[OptimizationObject.StorageTypeField],
                 variable=corresponding_value[i],
                 value=value,
             )
@@ -454,12 +498,18 @@ def generate_optimization_objects(
                 "The input structure is neither an optimization object nor a list."
             )
 
+        parent_metadata = (
+            kwargs["_parent_metadata"] if "_parent_metadata" in kwargs else None
+        )
+
         if isinstance(input_structure, OptimizationObject):
             output = self._generate_objects_from_instance(
-                input_structure=input_structure
+                input_structure=input_structure, parent_metadata=parent_metadata
             )
         else:
-            output = self._generate_objects_from_list(input_structure=input_structure)
+            output = self._generate_objects_from_list(
+                input_structure=input_structure, parent_metadata=parent_metadata
+            )
 
         fill_initial_guess = (
             kwargs["fill_initial_guess"] if "fill_initial_guess" in kwargs else True
@@ -587,3 +637,8 @@ def get_cost_values(self) -> dict[str, float]:
 
     def get_constraint_multipliers(self) -> dict[str, np.ndarray]:
         return self._constraint_values
+
+    def get_object_type(self, obj: cs.MX) -> str:
+        if obj not in self._objects_type_map:
+            raise ValueError("The object is not an optimization object.")
+        return self._objects_type_map[obj]
diff --git a/src/hippopt/base/optimization_object.py b/src/hippopt/base/optimization_object.py
index c8a9c59a..2ba31335 100644
--- a/src/hippopt/base/optimization_object.py
+++ b/src/hippopt/base/optimization_object.py
@@ -23,8 +23,13 @@ class OptimizationObject(abc.ABC):
     StorageTypeField: ClassVar[str] = "StorageType"
     TimeDependentField: ClassVar[str] = "TimeDependent"
     TimeExpansionField: ClassVar[str] = "TimeExpansion"
+    OverrideIfCompositeField: ClassVar[str] = "OverrideIfComposite"
+    CompositeTypeField: ClassVar[str] = "CompositeType"
     StorageTypeMetadata: ClassVar[dict[str, Any]] = dict(
-        StorageType=StorageType, TimeDependent=False, TimeExpansion=TimeExpansion.List
+        StorageType=StorageTypeValue,
+        TimeDependent=False,
+        TimeExpansion=TimeExpansion.List,
+        OverrideIfComposite=False,
     )
 
     @classmethod
@@ -46,8 +51,15 @@ def time_varying_metadata(time_varying: bool = True):
     return {OptimizationObject.TimeDependentField: time_varying}
 
 
-def default_composite_field(factory=None, time_varying: bool = True):
+def default_composite_field(
+    cls: Type[OptimizationObject] = None, factory=None, time_varying: bool = True
+):
+    cls_dict = time_varying_metadata(time_varying)
+    cls_dict[OptimizationObject.CompositeTypeField] = (
+        cls.StorageTypeMetadata if cls is not None else None
+    )
+
     return dataclasses.field(
         default_factory=factory,
-        metadata=time_varying_metadata(time_varying),
+        metadata=cls_dict,
     )
diff --git a/src/hippopt/base/parameter.py b/src/hippopt/base/parameter.py
index 0cc3b26e..933791b3 100644
--- a/src/hippopt/base/parameter.py
+++ b/src/hippopt/base/parameter.py
@@ -16,6 +16,7 @@ class Parameter(OptimizationObject):
         StorageType=StorageTypeValue,
         TimeDependent=False,
         TimeExpansion=TimeExpansion.List,
+        OverrideIfComposite=False,
     )
 
     @classmethod
@@ -29,3 +30,15 @@ def default_storage_metadata(
         cls_dict[OptimizationObject.TimeExpansionField] = time_expansion
 
         return cls_dict
+
+
+@dataclasses.dataclass
+class OverridableParameter(Parameter):
+    """"""
+
+    StorageTypeMetadata: ClassVar[dict[str, Any]] = dict(
+        StorageType=Parameter.StorageTypeValue,
+        TimeDependent=False,
+        TimeExpansion=TimeExpansion.List,
+        OverrideIfComposite=True,
+    )
diff --git a/src/hippopt/base/variable.py b/src/hippopt/base/variable.py
index d51b3655..7353bb04 100644
--- a/src/hippopt/base/variable.py
+++ b/src/hippopt/base/variable.py
@@ -23,6 +23,7 @@ class Variable(OptimizationObject):
         TimeDependent=True,
         TimeExpansion=TimeExpansion.List,
         VariableType=VariableType.continuous,
+        OverrideIfComposite=False,
     )
 
     @classmethod
@@ -38,3 +39,15 @@ def default_storage_metadata(
         cls_dict[cls.VariableTypeField] = variable_type
 
         return cls_dict
+
+
+@dataclasses.dataclass
+class OverridableVariable(Variable):
+    """"""
+
+    StorageTypeMetadata: ClassVar[dict[str, Any]] = dict(
+        StorageType=Variable.StorageTypeValue,
+        TimeDependent=True,
+        TimeExpansion=TimeExpansion.List,
+        OverrideIfComposite=True,
+    )
diff --git a/test/test_opti_generate_objects.py b/test/test_opti_generate_objects.py
index f51027eb..3dbcace2 100644
--- a/test/test_opti_generate_objects.py
+++ b/test/test_opti_generate_objects.py
@@ -7,6 +7,8 @@
     CompositeType,
     OptimizationObject,
     OptiSolver,
+    OverridableParameter,
+    OverridableVariable,
     Parameter,
     StorageType,
     Variable,
@@ -74,3 +76,107 @@ def test_generate_objects_list():
         assert opti_var.other_parameter.shape == (3, 1)
         assert opti_var.other == "untouched"
     assert solver.get_optimization_objects() is opti_var_list
+
+
+@dataclasses.dataclass
+class CustomOverridableVariable(OptimizationObject):
+    overridable: StorageType = default_storage_field(cls=OverridableVariable)
+    not_overridable: StorageType = default_storage_field(cls=Variable)
+
+    def __post_init__(self):
+        self.overridable = 0.0
+        self.not_overridable = 0.0
+
+
+@dataclasses.dataclass
+class CustomCompositeOverridableVariable(OptimizationObject):
+    composite: CompositeType[CustomOverridableVariable] = default_composite_field(
+        cls=Parameter, factory=CustomOverridableVariable
+    )
+
+
+def test_generate_variables_overridable():
+    test_var = CustomCompositeOverridableVariable()
+    solver = OptiSolver()
+    opti_var = solver.generate_optimization_objects(test_var)
+    assert isinstance(opti_var.composite.overridable, cs.MX)
+    assert opti_var.composite.overridable.shape == (1, 1)
+    assert (
+        solver.get_object_type(opti_var.composite.overridable)
+        == Parameter.StorageTypeValue
+    )
+    assert isinstance(opti_var.composite.not_overridable, cs.MX)
+    assert opti_var.composite.not_overridable.shape == (1, 1)
+    assert (
+        solver.get_object_type(opti_var.composite.not_overridable)
+        == Variable.StorageTypeValue
+    )
+
+
+@dataclasses.dataclass
+class CustomOverridableParameter(OptimizationObject):
+    overridable: StorageType = default_storage_field(cls=OverridableParameter)
+    not_overridable: StorageType = default_storage_field(cls=Parameter)
+
+    def __post_init__(self):
+        self.overridable = 0.0
+        self.not_overridable = 0.0
+
+
+@dataclasses.dataclass
+class CustomCompositeOverridableParameter(OptimizationObject):
+    composite: CompositeType[CustomOverridableParameter] = default_composite_field(
+        cls=Variable, factory=CustomOverridableParameter
+    )
+
+
+def test_generate_parameters_overridable():
+    test_var = CustomCompositeOverridableParameter()
+    solver = OptiSolver()
+    opti_var = solver.generate_optimization_objects(test_var)
+    assert isinstance(opti_var.composite.overridable, cs.MX)
+    assert opti_var.composite.overridable.shape == (1, 1)
+    assert (
+        solver.get_object_type(opti_var.composite.overridable)
+        == Variable.StorageTypeValue
+    )
+    assert isinstance(opti_var.composite.not_overridable, cs.MX)
+    assert opti_var.composite.not_overridable.shape == (1, 1)
+    assert (
+        solver.get_object_type(opti_var.composite.not_overridable)
+        == Parameter.StorageTypeValue
+    )
+
+
+@dataclasses.dataclass
+class CustomCustomOverridableVariableInner(OptimizationObject):
+    composite: CompositeType[CustomOverridableVariable] = default_composite_field(
+        cls=OverridableVariable, factory=CustomOverridableVariable
+    )
+
+
+@dataclasses.dataclass
+class CustomCustomOverridableVariableNested(OptimizationObject):
+    composite: CompositeType[
+        CustomCustomOverridableVariableInner
+    ] = default_composite_field(
+        cls=OverridableParameter, factory=CustomCustomOverridableVariableInner
+    )
+
+
+def test_generate_nested_overridable_class():
+    test_var = CustomCustomOverridableVariableNested()
+    solver = OptiSolver()
+    opti_var = solver.generate_optimization_objects(test_var)
+    assert isinstance(opti_var.composite.composite.overridable, cs.MX)
+    assert opti_var.composite.composite.overridable.shape == (1, 1)
+    assert (
+        solver.get_object_type(opti_var.composite.composite.overridable)
+        == Parameter.StorageTypeValue
+    )
+    assert isinstance(opti_var.composite.composite.not_overridable, cs.MX)
+    assert opti_var.composite.composite.not_overridable.shape == (1, 1)
+    assert (
+        solver.get_object_type(opti_var.composite.composite.not_overridable)
+        == Variable.StorageTypeValue
+    )

From 740c12d114b8b815d8fa1f7ec54eb51d5a4ac5b5 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 12 Sep 2023 17:09:23 +0200
Subject: [PATCH 33/57] Added workaround for
 https://github.com/ami-iit/hippopt/issues/8

---
 .github/workflows/ci_cd.yml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index 6dba1d4a..b8a583db 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -37,7 +37,9 @@ jobs:
       - name: Dependencies
         shell: bash -l {0}
         run: |
+          mamba install ipopt=3.14.12=*_0 # Workaround for https://github.com/ami-iit/hippopt/issues/8
           mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree
+          mamba list
 
       - name: Install
         shell: bash  -l {0}

From ee06bbb6f2457cebbea9d781fe26f51c07384078 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 13 Sep 2023 09:46:14 +0200
Subject: [PATCH 34/57] Use different workaround for python crash

---
 .github/workflows/ci_cd.yml | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index b8a583db..9d37048a 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -34,10 +34,15 @@ jobs:
           channels: conda-forge,robotology
           channel-priority: true
 
+      - name: Python Crash Workaround [Ubuntu]
+        if: matrix.os == 'ubuntu-22.04'
+        shell: bash -l {0}
+        run: |
+          mamba install libgomp=12.* # Workaround for https://github.com/ami-iit/hippopt/issues/8, see https://github.com/conda-forge/casadi-feedstock/issues/91#issuecomment-1717093459
+
       - name: Dependencies
         shell: bash -l {0}
         run: |
-          mamba install ipopt=3.14.12=*_0 # Workaround for https://github.com/ami-iit/hippopt/issues/8
           mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree
           mamba list
 

From ca3ab20645b2da9a608ce10b3ca66792d6f74c5f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 22 Sep 2023 09:49:38 +0200
Subject: [PATCH 35/57] Removed workaround for
 https://github.com/ami-iit/hippopt/issues/8

---
 .github/workflows/ci_cd.yml | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index 9d37048a..b0dea935 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -34,12 +34,6 @@ jobs:
           channels: conda-forge,robotology
           channel-priority: true
 
-      - name: Python Crash Workaround [Ubuntu]
-        if: matrix.os == 'ubuntu-22.04'
-        shell: bash -l {0}
-        run: |
-          mamba install libgomp=12.* # Workaround for https://github.com/ami-iit/hippopt/issues/8, see https://github.com/conda-forge/casadi-feedstock/issues/91#issuecomment-1717093459
-
       - name: Dependencies
         shell: bash -l {0}
         run: |

From 65bc457b541803af9fca600db6e0e425480ee0a3 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 10 Oct 2023 13:03:18 +0200
Subject: [PATCH 36/57] Allow to set guess also from Casadi.DM

---
 src/hippopt/base/opti_solver.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index fe99666a..5ebc9a97 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -378,12 +378,12 @@ def _set_initial_guess_internal(
                 ):
                     guess = np.array(guess)
 
-                if not isinstance(guess, np.ndarray):
+                if not isinstance(guess, np.ndarray) and not isinstance(guess, cs.DM):
                     raise ValueError(
                         "The guess for the field "
                         + base_name
                         + field.name
-                        + " is not an numpy array."
+                        + " is neither an numpy nor a DM array."
                     )
 
                 input_shape = (

From f33c056a83b3b2759cb77e0616d19714ef06d188 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 10 Oct 2023 15:48:52 +0200
Subject: [PATCH 37/57] Added message specifying which parameters have not been
 set

---
 src/hippopt/base/opti_solver.py | 37 ++++++++++++++++++++++++++++-----
 1 file changed, 32 insertions(+), 5 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 5ebc9a97..73aa5381 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -69,6 +69,8 @@ class OptiSolver(OptimizationSolver):
         default=None
     )
     _objects_type_map: dict[cs.MX, str] = dataclasses.field(default=None)
+    _free_parameters: list[str] = dataclasses.field(default=None)
+    _parameters_map: dict[cs.MX, str] = dataclasses.field(default=None)
 
     def __post_init__(
         self,
@@ -93,6 +95,8 @@ def __post_init__(
         self._cost_expressions = {}
         self._constraint_expressions = {}
         self._objects_type_map = {}
+        self._free_parameters = []
+        self._parameters_map = {}
 
     def _generate_opti_object(
         self, storage_type: str, name: str, value: StorageType
@@ -122,12 +126,17 @@ def _generate_opti_object(
         if storage_type is Parameter.StorageTypeValue:
             opti_object = self._solver.parameter(*value.shape)
             self._objects_type_map[opti_object] = Parameter.StorageTypeValue
+            self._free_parameters.append(name)
+            self._parameters_map[opti_object] = name
             return opti_object
 
         raise ValueError("Unsupported input storage type")
 
     def _generate_objects_from_instance(
-        self, input_structure: TOptimizationObject, parent_metadata: dict
+        self,
+        input_structure: TOptimizationObject,
+        parent_metadata: dict,
+        base_name: str,
     ) -> TOptimizationObject:
         output = copy.deepcopy(input_structure)
 
@@ -178,6 +187,7 @@ def _generate_objects_from_instance(
                         input_structure=composite_value,
                         fill_initial_guess=False,
                         _parent_metadata=new_parent_metadata,
+                        _base_name=base_name + field.name + ".",
                     ),
                 )
                 continue
@@ -206,7 +216,7 @@ def _generate_objects_from_instance(
                     output_value.append(
                         self._generate_opti_object(
                             storage_type=storage_type,
-                            name=field.name,
+                            name=base_name + field.name,
                             value=value,
                         )
                     )
@@ -220,7 +230,10 @@ def _generate_objects_from_instance(
         return output
 
     def _generate_objects_from_list(
-        self, input_structure: list[TOptimizationObject], parent_metadata: dict
+        self,
+        input_structure: list[TOptimizationObject],
+        parent_metadata: dict,
+        base_name: str,
     ) -> list[TOptimizationObject]:
         assert isinstance(input_structure, list)
 
@@ -230,6 +243,7 @@ def _generate_objects_from_list(
                 input_structure=output[i],
                 fill_initial_guess=False,
                 _parent_metadata=parent_metadata,
+                _base_name=base_name + "[" + str(i) + "].",
             )
 
         self._variables = output
@@ -296,6 +310,9 @@ def _set_opti_guess(self, variable: cs.MX, value: np.ndarray) -> None:
                 self._solver.set_initial(variable, value)
             case Parameter.StorageTypeValue:
                 self._solver.set_value(variable, value)
+                parameter_name = self._parameters_map[variable]
+                if parameter_name in self._free_parameters:
+                    self._free_parameters.remove(parameter_name)
 
         return
 
@@ -502,13 +519,19 @@ def generate_optimization_objects(
             kwargs["_parent_metadata"] if "_parent_metadata" in kwargs else None
         )
 
+        base_name = kwargs["_base_name"] if "_base_name" in kwargs else ""
+
         if isinstance(input_structure, OptimizationObject):
             output = self._generate_objects_from_instance(
-                input_structure=input_structure, parent_metadata=parent_metadata
+                input_structure=input_structure,
+                parent_metadata=parent_metadata,
+                base_name=base_name,
             )
         else:
             output = self._generate_objects_from_list(
-                input_structure=input_structure, parent_metadata=parent_metadata
+                input_structure=input_structure,
+                parent_metadata=parent_metadata,
+                base_name=base_name,
             )
 
         fill_initial_guess = (
@@ -568,6 +591,10 @@ def set_opti_options(
     def solve(self) -> None:
         self._cost = self._cost if self._cost is not None else cs.MX(0)
         self._solver.minimize(self._cost)
+        if len(self._free_parameters):
+            raise ValueError(
+                "The following parameters are not set: " + str(self._free_parameters)
+            )
         try:
             self._opti_solution = self._solver.solve()
         except Exception as err:  # noqa

From d2107647100b97cbb5e119e13a0c9d9e4fcb223d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 10 Oct 2023 17:50:35 +0200
Subject: [PATCH 38/57] Fixed setting of guesses for lists in aggregated
 variables.

---
 src/hippopt/base/opti_solver.py    |  7 ++++++-
 test/test_opti_generate_objects.py | 14 ++++++++++++++
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 73aa5381..92caa11c 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -424,7 +424,9 @@ def _set_initial_guess_internal(
 
             composite_variable_guess = initial_guess.__getattribute__(field.name)
 
-            if not isinstance(composite_variable_guess, OptimizationObject):
+            if not isinstance(
+                composite_variable_guess, OptimizationObject
+            ) and not isinstance(composite_variable_guess, list):
                 continue
 
             if not hasattr(corresponding_variable, field.name):
@@ -669,3 +671,6 @@ def get_object_type(self, obj: cs.MX) -> str:
         if obj not in self._objects_type_map:
             raise ValueError("The object is not an optimization object.")
         return self._objects_type_map[obj]
+
+    def get_free_parameters_names(self) -> list[str]:
+        return self._free_parameters
diff --git a/test/test_opti_generate_objects.py b/test/test_opti_generate_objects.py
index 3dbcace2..eefd0db8 100644
--- a/test/test_opti_generate_objects.py
+++ b/test/test_opti_generate_objects.py
@@ -34,12 +34,17 @@ class AggregateClass(OptimizationObject):
     aggregated: CompositeType[CustomVariable] = default_composite_field(
         factory=CustomVariable
     )
+    aggregated_list: CompositeType[list[CustomVariable]] = default_composite_field(
+        factory=list
+    )
     other_parameter: StorageType = default_storage_field(cls=Parameter)
     other: str = ""
 
     def __post_init__(self):
         self.other_parameter = np.ones(3)
         self.other = "untouched"
+        for _ in range(3):
+            self.aggregated_list.append(CustomVariable())
 
 
 def test_generate_objects():
@@ -54,8 +59,17 @@ def test_generate_objects():
     assert opti_var.other_parameter.shape == (3, 1)
     assert isinstance(opti_var.aggregated.scalar, cs.MX)
     assert opti_var.aggregated.scalar.shape == (1, 1)
+    assert isinstance(opti_var.aggregated_list, list)
+    for opti_var_list in opti_var.aggregated_list:
+        assert isinstance(opti_var_list.parameter, cs.MX)
+        assert opti_var_list.parameter.shape == (3, 1)
+        assert isinstance(opti_var_list.variable, cs.MX)
+        assert opti_var_list.variable.shape == (3, 1)
+        assert isinstance(opti_var_list.scalar, cs.MX)
+        assert opti_var_list.scalar.shape == (1, 1)
     assert opti_var.other == "untouched"
     assert solver.get_optimization_objects() is opti_var
+    assert len(solver.get_free_parameters_names()) == 0
 
 
 def test_generate_objects_list():

From f79df9eceb147d2b8a8a0e6ce9ca4690cb318846 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 10 Oct 2023 18:40:17 +0200
Subject: [PATCH 39/57] Catching exceptions when getting the solution from opti

This is in case some variable/parameter is not present in the cost/constraints
---
 src/hippopt/base/opti_solver.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 92caa11c..4d89bb1c 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -249,6 +249,12 @@ def _generate_objects_from_list(
         self._variables = output
         return output
 
+    def _get_opti_solution(self, variable: cs.MX) -> StorageType:
+        try:
+            return self._opti_solution.value(variable)
+        except Exception as err:  # noqa
+            return None
+
     def _generate_solution_output(
         self,
         variables: TOptimizationObject
@@ -279,9 +285,9 @@ def _generate_solution_output(
                 if isinstance(var, list):
                     output_val = []
                     for el in var:
-                        output_val.append(np.array(self._opti_solution.value(el)))
+                        output_val.append(np.array(self._get_opti_solution(el)))
                 else:
-                    output_val = np.array(self._opti_solution.value(var))
+                    output_val = np.array(self._get_opti_solution(var))
 
                 output.__setattr__(field.name, output_val)
                 continue

From 9e69233d165588e83e2f0f7755adc3c6c844bd0f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 10 Oct 2023 18:57:12 +0200
Subject: [PATCH 40/57] Added use of logging in opti_solver

---
 src/hippopt/base/opti_solver.py | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 4d89bb1c..74481261 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -1,5 +1,6 @@
 import copy
 import dataclasses
+import logging
 from typing import Any, ClassVar
 
 import casadi as cs
@@ -71,6 +72,8 @@ class OptiSolver(OptimizationSolver):
     _objects_type_map: dict[cs.MX, str] = dataclasses.field(default=None)
     _free_parameters: list[str] = dataclasses.field(default=None)
     _parameters_map: dict[cs.MX, str] = dataclasses.field(default=None)
+    _variables_map: dict[cs.MX, str] = dataclasses.field(default=None)
+    _logger: logging.Logger = dataclasses.field(default=None)
 
     def __post_init__(
         self,
@@ -97,6 +100,8 @@ def __post_init__(
         self._objects_type_map = {}
         self._free_parameters = []
         self._parameters_map = {}
+        self._variables_map = {}
+        self._logger = logging.getLogger("[hippopt::opti_solver]")
 
     def _generate_opti_object(
         self, storage_type: str, name: str, value: StorageType
@@ -119,11 +124,14 @@ def _generate_opti_object(
             value = value * np.ones((1, 1))
 
         if storage_type is Variable.StorageTypeValue:
+            self._logger.debug("Creating variable " + name)
             opti_object = self._solver.variable(*value.shape)
             self._objects_type_map[opti_object] = Variable.StorageTypeValue
+            self._variables_map[opti_object] = name
             return opti_object
 
         if storage_type is Parameter.StorageTypeValue:
+            self._logger.debug("Creating parameter " + name)
             opti_object = self._solver.parameter(*value.shape)
             self._objects_type_map[opti_object] = Parameter.StorageTypeValue
             self._free_parameters.append(name)
@@ -253,6 +261,12 @@ def _get_opti_solution(self, variable: cs.MX) -> StorageType:
         try:
             return self._opti_solution.value(variable)
         except Exception as err:  # noqa
+            self._logger.debug(
+                "Failed to get the solution for variable "
+                + self._variables_map[variable]
+                + ". Message: "
+                + str(err)
+            )
             return None
 
     def _generate_solution_output(
@@ -313,8 +327,16 @@ def _generate_solution_output(
     def _set_opti_guess(self, variable: cs.MX, value: np.ndarray) -> None:
         match self._objects_type_map[variable]:
             case Variable.StorageTypeValue:
+                self._logger.debug(
+                    "Setting initial value for variable "
+                    + self._variables_map[variable]
+                )
                 self._solver.set_initial(variable, value)
             case Parameter.StorageTypeValue:
+                self._logger.debug(
+                    "Setting initial value for parameter "
+                    + self._parameters_map[variable]
+                )
                 self._solver.set_value(variable, value)
                 parameter_name = self._parameters_map[variable]
                 if parameter_name in self._free_parameters:

From dc908ea9175b140e41ca0ac998611ac60719c982 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 13 Oct 2023 18:35:23 +0200
Subject: [PATCH 41/57] Using OptiSolver instead of opti_solver for logging

---
 src/hippopt/base/opti_solver.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 74481261..26e8b235 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -101,7 +101,7 @@ def __post_init__(
         self._free_parameters = []
         self._parameters_map = {}
         self._variables_map = {}
-        self._logger = logging.getLogger("[hippopt::opti_solver]")
+        self._logger = logging.getLogger("[hippopt::OptiSolver]")
 
     def _generate_opti_object(
         self, storage_type: str, name: str, value: StorageType

From cc55dd4b836bbb4186ad1a61223814d53c2eaf13 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 13 Oct 2023 18:37:29 +0200
Subject: [PATCH 42/57] Specified set of dependencies needed for visualizationr

---
 setup.cfg | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/setup.cfg b/setup.cfg
index f7a5785d..43765f76 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -64,11 +64,16 @@ robot_planning=
     adam-robotics
 turnkey_planners=
     idyntree
+visualization=
+    ffmpeg-python
+    idyntree
+    meshcat-python
 all =
     %(style)s
     %(testing)s
     %(robot_planning)s
     %(turnkey_planners)s
+    %(visualization)s
 
 [options.packages.find]
 where = src

From d682e1a51a2cd4e008411656ccb77e08db101d0d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 16 Oct 2023 15:46:55 +0200
Subject: [PATCH 43/57] Improved type hinting of the Output in Problem

---
 src/hippopt/base/problem.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/hippopt/base/problem.py b/src/hippopt/base/problem.py
index 83a66a15..be6a6653 100644
--- a/src/hippopt/base/problem.py
+++ b/src/hippopt/base/problem.py
@@ -159,7 +159,7 @@ def get_constraint_expressions(self) -> dict[str, cs.MX]:
     def solver(self) -> TGenericSolver:
         return self._solver
 
-    def solve(self) -> Output:
+    def solve(self) -> Output[TInputObjects]:
         self.solver().solve()
         self._output = Output(
             _cost_value=self.solver().get_cost_value(),
@@ -169,7 +169,7 @@ def solve(self) -> Output:
         )
         return self._output
 
-    def get_output(self) -> Output:
+    def get_output(self) -> Output[TInputObjects]:
         if self._output is None:
             raise ProblemNotSolvedException
 

From 7541c25a24e8cfe9f53d21ae5944a5b3e4911904 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 16 Oct 2023 18:08:37 +0200
Subject: [PATCH 44/57] Avoid setting guess when the input shape is null

---
 src/hippopt/base/opti_solver.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 26e8b235..46fbbef2 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -431,6 +431,9 @@ def _set_initial_guess_internal(
                         + " is neither an numpy nor a DM array."
                     )
 
+                if len(guess.shape) == 0:
+                    continue
+
                 input_shape = (
                     guess.shape if len(guess.shape) > 1 else (guess.shape[0], 1)
                 )

From 4eae76c2b58932bfc97fb959e12ce7c08f260f8d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 17 Oct 2023 17:58:18 +0200
Subject: [PATCH 45/57] Allow lists even when the input number is an integer

---
 src/hippopt/base/opti_solver.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 46fbbef2..9a827dab 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -419,7 +419,7 @@ def _set_initial_guess_internal(
                     guess = guess * np.ones((1, 1))
 
                 if isinstance(guess, list) and all(
-                    isinstance(elem, float) for elem in guess
+                    isinstance(elem, float) or isinstance(elem, int) for elem in guess
                 ):
                     guess = np.array(guess)
 

From addc7f7e523f274b723a42e10f6e1c677680d8aa Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 18 Oct 2023 15:39:16 +0200
Subject: [PATCH 46/57] Updated dependencies in CI

---
 .github/workflows/ci_cd.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index b0dea935..dd4be770 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -37,7 +37,8 @@ jobs:
       - name: Dependencies
         shell: bash -l {0}
         run: |
-          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree
+          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python 
+          mamba install metis=5.1.0 # Reminder for https://github.com/conda-forge/conda-forge-pinning-feedstock/pull/4857#issuecomment-1699470569
           mamba list
 
       - name: Install

From c3b7fb03e70ddb2dc2fb061743de5148d56fde25 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 30 Oct 2023 19:09:14 +0100
Subject: [PATCH 47/57] Added matplotlib dependencyy

---
 .github/workflows/ci_cd.yml | 2 +-
 setup.cfg                   | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index dd4be770..fb133516 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -37,7 +37,7 @@ jobs:
       - name: Dependencies
         shell: bash -l {0}
         run: |
-          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python 
+          mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib
           mamba install metis=5.1.0 # Reminder for https://github.com/conda-forge/conda-forge-pinning-feedstock/pull/4857#issuecomment-1699470569
           mamba list
 
diff --git a/setup.cfg b/setup.cfg
index 43765f76..9f5c1adf 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -68,6 +68,7 @@ visualization=
     ffmpeg-python
     idyntree
     meshcat-python
+    matplotlib
 all =
     %(style)s
     %(testing)s

From 857a6403f8371d49e7cfbf43e1967e390dea960b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 23 Nov 2023 18:26:44 +0100
Subject: [PATCH 48/57] Added possibility to transform an OptimizationObject to
 list and MX

---
 src/hippopt/base/optimization_object.py | 36 +++++++++++++++++++++++++
 test/test_opti_generate_objects.py      |  7 +++++
 2 files changed, 43 insertions(+)

diff --git a/src/hippopt/base/optimization_object.py b/src/hippopt/base/optimization_object.py
index 2ba31335..17cefd14 100644
--- a/src/hippopt/base/optimization_object.py
+++ b/src/hippopt/base/optimization_object.py
@@ -32,6 +32,42 @@ class OptimizationObject(abc.ABC):
         OverrideIfComposite=False,
     )
 
+    def to_list(self) -> list:
+        output_list = []
+        for field in dataclasses.fields(self):
+            composite_value = self.__getattribute__(field.name)
+
+            is_list = isinstance(composite_value, list)
+            list_of_optimization_objects = is_list and all(
+                isinstance(elem, OptimizationObject) or isinstance(elem, list)
+                for elem in composite_value
+            )
+            list_of_float = is_list and all(
+                isinstance(elem, float) for elem in composite_value
+            )
+            if list_of_float:
+                is_list = False
+
+            if list_of_optimization_objects:
+                for elem in composite_value:
+                    output_list += elem.to_list()
+                continue
+
+            if isinstance(composite_value, OptimizationObject):
+                output_list += composite_value.to_list()
+                continue
+
+            if OptimizationObject.StorageTypeField in field.metadata:
+                value_list = composite_value if is_list else [composite_value]
+                for value in value_list:
+                    output_list.append(value)
+                continue
+
+        return output_list
+
+    def to_mx(self) -> cs.MX:
+        return cs.vertcat(*self.to_list())
+
     @classmethod
     def default_storage_metadata(cls, **kwargs) -> dict:
         pass
diff --git a/test/test_opti_generate_objects.py b/test/test_opti_generate_objects.py
index eefd0db8..bd9fa3f3 100644
--- a/test/test_opti_generate_objects.py
+++ b/test/test_opti_generate_objects.py
@@ -49,6 +49,10 @@ def __post_init__(self):
 
 def test_generate_objects():
     test_var = AggregateClass()
+    test_var_as_list = test_var.to_list()
+    assert (
+        len(test_var_as_list) == 3 + 3 * 3 + 1
+    )  # 3 for aggregated, 3*3 for aggregated_list, 1 for other_parameter
     solver = OptiSolver()
     opti_var = solver.generate_optimization_objects(test_var)
     assert isinstance(opti_var.aggregated.parameter, cs.MX)
@@ -70,6 +74,9 @@ def test_generate_objects():
     assert opti_var.other == "untouched"
     assert solver.get_optimization_objects() is opti_var
     assert len(solver.get_free_parameters_names()) == 0
+    assert (len(opti_var.to_list())) == len(test_var_as_list)
+    expected_len = 7 + 3 * 7 + 3
+    assert opti_var.to_mx().shape == (expected_len, 1)
 
 
 def test_generate_objects_list():

From c592a3af380a5e551032b719db396e786ffda123 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 29 Nov 2023 16:44:05 +0100
Subject: [PATCH 49/57] Improved error message in Opti solver

---
 src/hippopt/base/opti_solver.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 9a827dab..19b5deeb 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -440,11 +440,10 @@ def _set_initial_guess_internal(
 
                 if corresponding_value.shape != input_shape:
                     raise ValueError(
-                        "The guess has the field "
-                        + base_name
-                        + field.name
-                        + " but its dimension does not match with the corresponding"
-                        + " optimization variable"
+                        f"The guess has the field {base_name}{field.name} "
+                        f"but its dimension ({input_shape}) does not match with the"
+                        f" corresponding optimization variable "
+                        f"({corresponding_value.shape})."
                     )
 
                 self._set_opti_guess(

From 7a6ea9c623abf90fa79090c7f2d9cb965105ed49 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Fri, 5 Jan 2024 17:12:35 +0100
Subject: [PATCH 50/57] Initial modifications to OptiSolver to have callbacks

---
 src/hippopt/base/opti_solver.py | 33 +++++++++++++++++++++------------
 1 file changed, 21 insertions(+), 12 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 19b5deeb..f2c8284c 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -55,7 +55,6 @@ class OptiSolver(OptimizationSolver):
     _cost_expressions: dict[str, cs.MX] = dataclasses.field(default=None)
     _constraint_expressions: dict[str, cs.MX] = dataclasses.field(default=None)
     _solver: cs.Opti = dataclasses.field(default=None)
-    _opti_solution: cs.OptiSol = dataclasses.field(default=None)
     _output_solution: TOptimizationObject | list[
         TOptimizationObject
     ] = dataclasses.field(default=None)
@@ -257,9 +256,13 @@ def _generate_objects_from_list(
         self._variables = output
         return output
 
-    def _get_opti_solution(self, variable: cs.MX) -> StorageType:
+    def _get_opti_solution(
+        self, variable: cs.MX, input_solution: cs.OptiSol | dict
+    ) -> StorageType:
         try:
-            return self._opti_solution.value(variable)
+            if isinstance(input_solution, dict):
+                return input_solution[variable]
+            return input_solution.value(variable)
         except Exception as err:  # noqa
             self._logger.debug(
                 "Failed to get the solution for variable "
@@ -274,12 +277,13 @@ def _generate_solution_output(
         variables: TOptimizationObject
         | list[TOptimizationObject]
         | list[list[TOptimizationObject]],
+        input_solution: cs.OptiSol | dict,
     ) -> TOptimizationObject | list[TOptimizationObject]:
         output = copy.deepcopy(variables)
 
         if isinstance(variables, list):
             for i in range(len(variables)):
-                output[i] = self._generate_solution_output(variables[i])
+                output[i] = self._generate_solution_output(variables[i], input_solution)
             return output
 
         for field in dataclasses.fields(variables):
@@ -299,9 +303,11 @@ def _generate_solution_output(
                 if isinstance(var, list):
                     output_val = []
                     for el in var:
-                        output_val.append(np.array(self._get_opti_solution(el)))
+                        output_val.append(
+                            np.array(self._get_opti_solution(el, input_solution))
+                        )
                 else:
-                    output_val = np.array(self._get_opti_solution(var))
+                    output_val = np.array(self._get_opti_solution(var, input_solution))
 
                 output.__setattr__(field.name, output_val)
                 continue
@@ -319,7 +325,8 @@ def _generate_solution_output(
                 or list_of_optimization_objects
             ):
                 output.__setattr__(
-                    field.name, self._generate_solution_output(composite_variable)
+                    field.name,
+                    self._generate_solution_output(composite_variable, input_solution),
                 )
 
         return output
@@ -628,19 +635,21 @@ def solve(self) -> None:
                 "The following parameters are not set: " + str(self._free_parameters)
             )
         try:
-            self._opti_solution = self._solver.solve()
+            opti_solution = self._solver.solve()
         except Exception as err:  # noqa
             raise OptiFailure(message=err)
 
-        self._output_cost = self._opti_solution.value(self._cost)
-        self._output_solution = self._generate_solution_output(self._variables)
+        self._output_cost = opti_solution.value(self._cost)
+        self._output_solution = self._generate_solution_output(
+            variables=self._variables, input_solution=opti_solution
+        )
         self._cost_values = {
-            name: float(self._opti_solution.value(self._cost_expressions[name]))
+            name: float(opti_solution.value(self._cost_expressions[name]))
             for name in self._cost_expressions
         }
         self._constraint_values = {
             name: np.array(
-                self._opti_solution.value(
+                opti_solution.value(
                     self._solver.dual(self._constraint_expressions[name])
                 )
             )

From f82c88c489fd2178f575c55dff4c9bf0261df08a Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Fri, 5 Jan 2024 17:13:14 +0100
Subject: [PATCH 51/57] Initial implementation of opti_callback

---
 src/hippopt/base/opti_callback.py | 324 ++++++++++++++++++++++++++++++
 1 file changed, 324 insertions(+)
 create mode 100644 src/hippopt/base/opti_callback.py

diff --git a/src/hippopt/base/opti_callback.py b/src/hippopt/base/opti_callback.py
new file mode 100644
index 00000000..940d320f
--- /dev/null
+++ b/src/hippopt/base/opti_callback.py
@@ -0,0 +1,324 @@
+import abc
+import logging
+from typing import final
+
+import casadi as cs
+
+
+class Callback(cs.OptiCallback, abc.ABC):
+    """Abstract class of an Opti callback."""
+
+    def __init__(self) -> None:
+        cs.OptiCallback.__init__(self)
+
+    @final
+    def __call__(self, i: int) -> None:
+        self.call(i)
+
+    @abc.abstractmethod
+    def call(self, i) -> None:
+        pass
+
+
+class StoppingCriterion(abc.ABC):
+    """"""
+
+    def __init__(self) -> None:
+        """"""
+        self.opti = None
+
+    @abc.abstractmethod
+    def satisfied(self) -> bool:
+        pass
+
+    @abc.abstractmethod
+    def update(self) -> None:
+        pass
+
+    @abc.abstractmethod
+    def reset(self) -> None:
+        pass
+
+    def __or__(
+        self, stopping_criterion: "StoppingCriterion"
+    ) -> "CombinedStoppingCriterion":
+        if not isinstance(stopping_criterion, StoppingCriterion):
+            raise TypeError(stopping_criterion)
+
+        return CombinedStoppingCriterion([self, stopping_criterion])
+
+    def set_opti(self, opti: cs.Opti) -> None:
+        """"""
+        self.opti = opti
+
+
+class BestCost(StoppingCriterion):
+    """"""
+
+    def __init__(self) -> None:
+        """"""
+
+        StoppingCriterion.__init__(self)
+
+        self.best_cost = None
+        self.reset()
+
+    @final
+    def reset(self) -> None:
+        """"""
+
+        self.best_cost = cs.inf
+
+    @final
+    def satisfied(self) -> bool:
+        """"""
+
+        return self._get_current_cost() < self.best_cost
+
+    @final
+    def update(self) -> None:
+        """"""
+
+        best_cost = self._get_current_cost()
+
+        _logger = logging.getLogger(f"[hippopt::{self.__class__.__name__}]")
+        _logger.debug(f"New best cost: {best_cost} (old: {self.best_cost})")
+
+        self.best_cost = self._get_current_cost()
+
+    def _get_current_cost(self) -> float:
+        """"""
+
+        return self.opti.debug.value(self.opti.f)
+
+
+class AcceptableCost(StoppingCriterion):
+    """"""
+
+    def __init__(self, acceptable_cost: float = cs.inf) -> None:
+        """"""
+
+        StoppingCriterion.__init__(self)
+
+        self.acceptable_cost = acceptable_cost
+
+        self.best_acceptable_cost = None
+        self.reset()
+
+    @final
+    def reset(self) -> None:
+        """"""
+
+        self.best_acceptable_cost = cs.inf
+
+    def satisfied(self) -> bool:
+        """"""
+
+        return self._get_current_cost() < self.acceptable_cost
+
+    def update(self) -> None:
+        """"""
+
+        current_cost = self._get_current_cost()
+
+        if current_cost < self.best_acceptable_cost:
+            _logger = logging.getLogger(f"[hippopt::{self.__class__.__name__}]")
+            _logger.debug(
+                f"[New acceptable cost: {current_cost}"
+                f" (old: {self.best_acceptable_cost})"
+            )
+
+            self.best_acceptable_cost = current_cost
+
+    def _get_current_cost(self) -> float:
+        """"""
+
+        return self.opti.debug.value(self.opti.f)
+
+
+class AcceptablePrimalInfeasibility(StoppingCriterion):
+    """"""
+
+    def __init__(self, acceptable_primal_infeasibility: float = cs.inf) -> None:
+        """"""
+
+        StoppingCriterion.__init__(self)
+
+        self.acceptable_primal_infeasibility = acceptable_primal_infeasibility
+
+        self.best_acceptable_primal_infeasibility = None
+        self.reset()
+
+    @final
+    def reset(self) -> None:
+        """"""
+
+        self.best_acceptable_primal_infeasibility = cs.inf
+
+    def satisfied(self) -> bool:
+        """"""
+
+        return (
+            self._get_current_primal_infeasibility()
+            < self.acceptable_primal_infeasibility
+        )
+
+    def update(self) -> None:
+        """"""
+
+        current_primal_infeasibility = self._get_current_primal_infeasibility()
+
+        if current_primal_infeasibility < self.best_acceptable_primal_infeasibility:
+            _logger = logging.getLogger(f"[hippopt::{self.__class__.__name__}]")
+            _logger.debug(
+                f"New acceptable primal infeasibility: "
+                f"{current_primal_infeasibility} "
+                f"(old: {self.best_acceptable_primal_infeasibility})"
+            )
+
+            self.best_acceptable_primal_infeasibility = current_primal_infeasibility
+
+    def _get_current_primal_infeasibility(self) -> float:
+        """"""
+
+        return self.opti.debug.stats()["iterations"]["inf_pr"][-1]
+
+
+class BestPrimalInfeasibility(StoppingCriterion):
+    """"""
+
+    def __init__(self) -> None:
+        """"""
+
+        StoppingCriterion.__init__(self)
+
+        self.best_primal_infeasibility = None
+        self.reset()
+
+    @final
+    def reset(self) -> None:
+        """"""
+
+        self.best_primal_infeasibility = cs.inf
+
+    def satisfied(self) -> bool:
+        """"""
+
+        return self._get_current_primal_infeasibility() < self.best_primal_infeasibility
+
+    def update(self) -> None:
+        """"""
+
+        best_primal_infeasibility = self._get_current_primal_infeasibility()
+
+        _logger = logging.getLogger(f"[hippopt::{self.__class__.__name__}]")
+        _logger.debug(
+            f"New best primal infeasibility: {best_primal_infeasibility}"
+            f" (old: {self.best_primal_infeasibility})"
+        )
+
+        self.best_primal_infeasibility = best_primal_infeasibility
+
+    def _get_current_primal_infeasibility(self) -> float:
+        """"""
+
+        return self.opti.debug.stats()["iterations"]["inf_pr"][-1]
+
+
+class CombinedStoppingCriterion(StoppingCriterion):
+    """"""
+
+    def __init__(self, stopping_criteria: list[StoppingCriterion]) -> None:
+        """"""
+
+        StoppingCriterion.__init__(self)
+        self.stopping_criteria = stopping_criteria
+
+    def __or__(
+        self, stopping_criterion: StoppingCriterion
+    ) -> "CombinedStoppingCriterion":
+        if isinstance(stopping_criterion, CombinedStoppingCriterion):
+            ret = CombinedStoppingCriterion(
+                stopping_criteria=self.stopping_criteria
+                + stopping_criterion.stopping_criteria
+            )
+
+        elif isinstance(stopping_criterion, StoppingCriterion):
+            ret = CombinedStoppingCriterion(
+                stopping_criteria=self.stopping_criteria + [stopping_criterion]
+            )
+
+        else:
+            raise TypeError(stopping_criterion)
+
+        return ret
+
+    @final
+    def reset(self) -> None:
+        """"""
+
+        _ = [
+            stopping_criterion.reset() for stopping_criterion in self.stopping_criteria
+        ]
+
+    @final
+    def satisfied(self) -> bool:
+        """"""
+
+        return all(
+            [
+                stopping_criterion.satisfied()
+                for stopping_criterion in self.stopping_criteria
+            ]
+        )
+
+    @final
+    def update(self) -> None:
+        """"""
+
+        for stopping_criterion in self.stopping_criteria:
+            stopping_criterion.update()
+
+    @final
+    def set_opti(self, opti: cs.Opti) -> None:
+        """"""
+
+        for stopping_criterion in self.stopping_criteria:
+            stopping_criterion.set_opti(opti)
+
+
+class SaveBestUnsolvedVariablesCallback(Callback):
+    """Class to save the best unsolved variables."""
+
+    def __init__(
+        self,
+        criterion: StoppingCriterion,
+        opti: cs.Opti,
+        optimization_objects: list[cs.MX],
+    ) -> None:
+        """"""
+
+        Callback.__init__(self)
+
+        self.criterion = criterion
+        self.opti = opti
+        self.criterion.set_opti(self.opti)
+        self.optimization_objects = optimization_objects
+
+        self.best_stats = None
+        self.best_variables = {}
+
+    def call(self, i: int) -> None:
+        """"""
+
+        if self.criterion.satisfied():
+            self.criterion.update()
+
+            _logger = logging.getLogger(f"[hippopt::{self.__class__.__name__}]")
+            _logger.info(f"[i={i}] New best intermediate variables")
+
+            self.best_stats = self.opti.debug.stats()
+            self.best_variables = {
+                optimization_object: self.opti.debug.value(optimization_object)
+                for optimization_object in self.optimization_objects
+            }

From d72494d7b36c3090a289fd4421dc4d607ffb80ba Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Mon, 8 Jan 2024 12:56:51 +0100
Subject: [PATCH 52/57] Using weakref in opti callback and save also the cost
 values and dual variables

---
 src/hippopt/base/opti_callback.py | 26 +++++++++++++++++++++++---
 1 file changed, 23 insertions(+), 3 deletions(-)

diff --git a/src/hippopt/base/opti_callback.py b/src/hippopt/base/opti_callback.py
index 940d320f..ff925a4d 100644
--- a/src/hippopt/base/opti_callback.py
+++ b/src/hippopt/base/opti_callback.py
@@ -1,5 +1,6 @@
 import abc
 import logging
+import weakref
 from typing import final
 
 import casadi as cs
@@ -49,7 +50,9 @@ def __or__(
 
     def set_opti(self, opti: cs.Opti) -> None:
         """"""
-        self.opti = opti
+        # In theory, the callback is included in opti,
+        # so the weakref is to avoid circular references
+        self.opti = weakref.proxy(opti)
 
 
 class BestCost(StoppingCriterion):
@@ -295,18 +298,27 @@ def __init__(
         criterion: StoppingCriterion,
         opti: cs.Opti,
         optimization_objects: list[cs.MX],
+        costs: list[cs.MX],
+        constraints: list[cs.MX],
     ) -> None:
         """"""
 
         Callback.__init__(self)
 
         self.criterion = criterion
-        self.opti = opti
-        self.criterion.set_opti(self.opti)
+        # In theory, the callback is included in opti,
+        # so the weakref is to avoid circular references
+        self.opti = weakref.proxy(opti)
+        self.criterion.set_opti(opti)
         self.optimization_objects = optimization_objects
+        self.cost = costs
+        self.constraints = constraints
 
         self.best_stats = None
         self.best_variables = {}
+        self.best_cost = None
+        self.best_cost_values = {}
+        self.best_constraint_multipliers = {}
 
     def call(self, i: int) -> None:
         """"""
@@ -318,7 +330,15 @@ def call(self, i: int) -> None:
             _logger.info(f"[i={i}] New best intermediate variables")
 
             self.best_stats = self.opti.debug.stats()
+            self.best_cost = self.opti.debug.value(self.opti.f)
             self.best_variables = {
                 optimization_object: self.opti.debug.value(optimization_object)
                 for optimization_object in self.optimization_objects
             }
+            self.best_cost_values = {
+                cost: self.opti.debug.value(cost) for cost in self.cost
+            }
+            self.best_constraint_multipliers = {
+                constraint: self.opti.debug.value(self.opti.dual(constraint))
+                for constraint in self.constraints
+            }

From 62ef421aa13effb441e4e9587302e35fb0deb082 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Mon, 8 Jan 2024 16:14:41 +0100
Subject: [PATCH 53/57] Allowed combining callback criteria using or/and
 Renamed Stopping to Callback

---
 src/hippopt/base/opti_callback.py | 106 ++++++++++++++++--------------
 1 file changed, 55 insertions(+), 51 deletions(-)

diff --git a/src/hippopt/base/opti_callback.py b/src/hippopt/base/opti_callback.py
index ff925a4d..ef16841f 100644
--- a/src/hippopt/base/opti_callback.py
+++ b/src/hippopt/base/opti_callback.py
@@ -21,7 +21,7 @@ def call(self, i) -> None:
         pass
 
 
-class StoppingCriterion(abc.ABC):
+class CallbackCriterion(abc.ABC):
     """"""
 
     def __init__(self) -> None:
@@ -41,12 +41,26 @@ def reset(self) -> None:
         pass
 
     def __or__(
-        self, stopping_criterion: "StoppingCriterion"
-    ) -> "CombinedStoppingCriterion":
-        if not isinstance(stopping_criterion, StoppingCriterion):
+        self, stopping_criterion: "CallbackCriterion"
+    ) -> "CombinedCallbackCriterion":
+        if not isinstance(stopping_criterion, CallbackCriterion):
             raise TypeError(stopping_criterion)
 
-        return CombinedStoppingCriterion([self, stopping_criterion])
+        return OrCombinedCallbackCriterion(lhs=self, rhs=stopping_criterion)
+
+    def __ror__(self, other):
+        return self.__or__(other)
+
+    def __and__(
+        self, stopping_criterion: "CallbackCriterion"
+    ) -> "CombinedCallbackCriterion":
+        if not isinstance(stopping_criterion, CallbackCriterion):
+            raise TypeError(stopping_criterion)
+
+        return AndCombinedCallbackCriterion(lhs=self, rhs=stopping_criterion)
+
+    def __rand__(self, other):
+        return self.__and__(other)
 
     def set_opti(self, opti: cs.Opti) -> None:
         """"""
@@ -55,13 +69,13 @@ def set_opti(self, opti: cs.Opti) -> None:
         self.opti = weakref.proxy(opti)
 
 
-class BestCost(StoppingCriterion):
+class BestCost(CallbackCriterion):
     """"""
 
     def __init__(self) -> None:
         """"""
 
-        StoppingCriterion.__init__(self)
+        CallbackCriterion.__init__(self)
 
         self.best_cost = None
         self.reset()
@@ -95,13 +109,13 @@ def _get_current_cost(self) -> float:
         return self.opti.debug.value(self.opti.f)
 
 
-class AcceptableCost(StoppingCriterion):
+class AcceptableCost(CallbackCriterion):
     """"""
 
     def __init__(self, acceptable_cost: float = cs.inf) -> None:
         """"""
 
-        StoppingCriterion.__init__(self)
+        CallbackCriterion.__init__(self)
 
         self.acceptable_cost = acceptable_cost
 
@@ -139,13 +153,13 @@ def _get_current_cost(self) -> float:
         return self.opti.debug.value(self.opti.f)
 
 
-class AcceptablePrimalInfeasibility(StoppingCriterion):
+class AcceptablePrimalInfeasibility(CallbackCriterion):
     """"""
 
     def __init__(self, acceptable_primal_infeasibility: float = cs.inf) -> None:
         """"""
 
-        StoppingCriterion.__init__(self)
+        CallbackCriterion.__init__(self)
 
         self.acceptable_primal_infeasibility = acceptable_primal_infeasibility
 
@@ -187,13 +201,13 @@ def _get_current_primal_infeasibility(self) -> float:
         return self.opti.debug.stats()["iterations"]["inf_pr"][-1]
 
 
-class BestPrimalInfeasibility(StoppingCriterion):
+class BestPrimalInfeasibility(CallbackCriterion):
     """"""
 
     def __init__(self) -> None:
         """"""
 
-        StoppingCriterion.__init__(self)
+        CallbackCriterion.__init__(self)
 
         self.best_primal_infeasibility = None
         self.reset()
@@ -228,66 +242,56 @@ def _get_current_primal_infeasibility(self) -> float:
         return self.opti.debug.stats()["iterations"]["inf_pr"][-1]
 
 
-class CombinedStoppingCriterion(StoppingCriterion):
+class CombinedCallbackCriterion(abc.ABC, CallbackCriterion):
     """"""
 
-    def __init__(self, stopping_criteria: list[StoppingCriterion]) -> None:
+    def __init__(self, lhs: CallbackCriterion, rhs: CallbackCriterion) -> None:
         """"""
 
-        StoppingCriterion.__init__(self)
-        self.stopping_criteria = stopping_criteria
+        CallbackCriterion.__init__(self)
+        self.lhs = lhs
+        self.rhs = rhs
 
-    def __or__(
-        self, stopping_criterion: StoppingCriterion
-    ) -> "CombinedStoppingCriterion":
-        if isinstance(stopping_criterion, CombinedStoppingCriterion):
-            ret = CombinedStoppingCriterion(
-                stopping_criteria=self.stopping_criteria
-                + stopping_criterion.stopping_criteria
-            )
+    @final
+    def reset(self) -> None:
+        """"""
 
-        elif isinstance(stopping_criterion, StoppingCriterion):
-            ret = CombinedStoppingCriterion(
-                stopping_criteria=self.stopping_criteria + [stopping_criterion]
-            )
+        self.lhs.reset()
+        self.rhs.reset()
 
-        else:
-            raise TypeError(stopping_criterion)
+    @final
+    def update(self) -> None:
+        """"""
 
-        return ret
+        self.lhs.update()
+        self.rhs.update()
 
     @final
-    def reset(self) -> None:
+    def set_opti(self, opti: cs.Opti) -> None:
         """"""
 
-        _ = [
-            stopping_criterion.reset() for stopping_criterion in self.stopping_criteria
-        ]
+        self.lhs.set_opti(opti)
+        self.rhs.set_opti(opti)
+
+
+class OrCombinedCallbackCriterion(CombinedCallbackCriterion):
+    """"""
 
     @final
     def satisfied(self) -> bool:
         """"""
 
-        return all(
-            [
-                stopping_criterion.satisfied()
-                for stopping_criterion in self.stopping_criteria
-            ]
-        )
+        return self.lhs.satisfied() or self.rhs.satisfied()
 
-    @final
-    def update(self) -> None:
-        """"""
 
-        for stopping_criterion in self.stopping_criteria:
-            stopping_criterion.update()
+class AndCombinedCallbackCriterion(CombinedCallbackCriterion):
+    """"""
 
     @final
-    def set_opti(self, opti: cs.Opti) -> None:
+    def satisfied(self) -> bool:
         """"""
 
-        for stopping_criterion in self.stopping_criteria:
-            stopping_criterion.set_opti(opti)
+        return self.lhs.satisfied() and self.rhs.satisfied()
 
 
 class SaveBestUnsolvedVariablesCallback(Callback):
@@ -295,7 +299,7 @@ class SaveBestUnsolvedVariablesCallback(Callback):
 
     def __init__(
         self,
-        criterion: StoppingCriterion,
+        criterion: CallbackCriterion,
         opti: cs.Opti,
         optimization_objects: list[cs.MX],
         costs: list[cs.MX],

From f50cec78fa7cb951a8ca80a427b3d2950c0c0618 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Mon, 8 Jan 2024 18:45:40 +0100
Subject: [PATCH 54/57] Added possibility to set callback in opti to have
 intermediate solutions.

---
 src/hippopt/__init__.py           |  3 +-
 src/hippopt/base/__init__.py      |  1 +
 src/hippopt/base/opti_callback.py | 25 ++++++++----
 src/hippopt/base/opti_solver.py   | 63 +++++++++++++++++++++++++++++--
 test/test_optimization_problem.py | 18 +++++++++
 5 files changed, 98 insertions(+), 12 deletions(-)

diff --git a/src/hippopt/__init__.py b/src/hippopt/__init__.py
index 8d11dbd0..2c93d583 100644
--- a/src/hippopt/__init__.py
+++ b/src/hippopt/__init__.py
@@ -1,4 +1,5 @@
-from . import base, integrators
+import hippopt.base.opti_callback as opti_callback
+
 from .base.dynamics import Dynamics, TypedDynamics, dot
 from .base.multiple_shooting_solver import MultipleShootingSolver
 from .base.opti_solver import OptiFailure, OptiSolver
diff --git a/src/hippopt/base/__init__.py b/src/hippopt/base/__init__.py
index 42cf0b72..b8c8935c 100644
--- a/src/hippopt/base/__init__.py
+++ b/src/hippopt/base/__init__.py
@@ -1,6 +1,7 @@
 from . import (
     dynamics,
     multiple_shooting_solver,
+    opti_callback,
     opti_solver,
     optimal_control_problem,
     optimal_control_solver,
diff --git a/src/hippopt/base/opti_callback.py b/src/hippopt/base/opti_callback.py
index ef16841f..0481760c 100644
--- a/src/hippopt/base/opti_callback.py
+++ b/src/hippopt/base/opti_callback.py
@@ -67,6 +67,7 @@ def set_opti(self, opti: cs.Opti) -> None:
         # In theory, the callback is included in opti,
         # so the weakref is to avoid circular references
         self.opti = weakref.proxy(opti)
+        self.reset()
 
 
 class BestCost(CallbackCriterion):
@@ -242,7 +243,7 @@ def _get_current_primal_infeasibility(self) -> float:
         return self.opti.debug.stats()["iterations"]["inf_pr"][-1]
 
 
-class CombinedCallbackCriterion(abc.ABC, CallbackCriterion):
+class CombinedCallbackCriterion(CallbackCriterion, abc.ABC):
     """"""
 
     def __init__(self, lhs: CallbackCriterion, rhs: CallbackCriterion) -> None:
@@ -318,11 +319,12 @@ def __init__(
         self.cost = costs
         self.constraints = constraints
 
-        self.best_stats = None
-        self.best_variables = {}
+        self.best_iteration = None
+        self.best_objects = {}
         self.best_cost = None
         self.best_cost_values = {}
         self.best_constraint_multipliers = {}
+        self.ignore_map = {obj: False for obj in self.optimization_objects}
 
     def call(self, i: int) -> None:
         """"""
@@ -333,12 +335,19 @@ def call(self, i: int) -> None:
             _logger = logging.getLogger(f"[hippopt::{self.__class__.__name__}]")
             _logger.info(f"[i={i}] New best intermediate variables")
 
-            self.best_stats = self.opti.debug.stats()
+            self.best_iteration = i
             self.best_cost = self.opti.debug.value(self.opti.f)
-            self.best_variables = {
-                optimization_object: self.opti.debug.value(optimization_object)
-                for optimization_object in self.optimization_objects
-            }
+            self.best_objects = {}
+            for optimization_object in self.optimization_objects:
+                if self.ignore_map[optimization_object]:
+                    continue
+                try:
+                    self.best_objects[optimization_object] = self.opti.debug.value(
+                        optimization_object
+                    )
+                except Exception as err:  # noqa
+                    self.ignore_map[optimization_object] = True
+
             self.best_cost_values = {
                 cost: self.opti.debug.value(cost) for cost in self.cost
             }
diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index f2c8284c..5d037237 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -6,6 +6,10 @@
 import casadi as cs
 import numpy as np
 
+from hippopt.base.opti_callback import (
+    CallbackCriterion,
+    SaveBestUnsolvedVariablesCallback,
+)
 from hippopt.base.optimization_object import (
     OptimizationObject,
     StorageType,
@@ -22,8 +26,15 @@
 
 
 class OptiFailure(Exception):
-    def __init__(self, message: Exception):
-        super().__init__("Opti failed to solve the problem. Message: " + str(message))
+    def __init__(self, message: Exception, callback_used: bool):
+        callback_info = ""
+        if callback_used:
+            callback_info = (
+                " and the callback did not manage to save an intermediate solution"
+            )
+        super().__init__(
+            f"Opti failed to solve the problem{callback_info}. Message: {str(message)}"
+        )
 
 
 class InitialGuessFailure(Exception):
@@ -50,6 +61,11 @@ class OptiSolver(OptimizationSolver):
     options_plugin: dataclasses.InitVar[dict[str, Any]] = dataclasses.field(
         default=None
     )
+    _callback_criterion: CallbackCriterion = dataclasses.field(default=None)
+    callback_criterion: dataclasses.InitVar[CallbackCriterion] = dataclasses.field(
+        default=None
+    )
+    _callback: SaveBestUnsolvedVariablesCallback = dataclasses.field(default=None)
 
     _cost: cs.MX = dataclasses.field(default=None)
     _cost_expressions: dict[str, cs.MX] = dataclasses.field(default=None)
@@ -80,6 +96,7 @@ def __post_init__(
         problem_type: str = "nlp",
         options_solver: dict[str, Any] = None,
         options_plugin: dict[str, Any] = None,
+        callback_criterion: CallbackCriterion = None,
     ):
         self._solver = cs.Opti(problem_type)
         self._inner_solver = (
@@ -94,6 +111,7 @@ def __post_init__(
         self._solver.solver(
             self._inner_solver, self._options_plugin, self._options_solver
         )
+        self._callback_criterion = callback_criterion
         self._cost_expressions = {}
         self._constraint_expressions = {}
         self._objects_type_map = {}
@@ -634,10 +652,49 @@ def solve(self) -> None:
             raise ValueError(
                 "The following parameters are not set: " + str(self._free_parameters)
             )
+        use_callback = self._callback_criterion is not None
+        if use_callback:
+            self._callback = SaveBestUnsolvedVariablesCallback(
+                criterion=self._callback_criterion,
+                opti=self._solver,
+                optimization_objects=list(self._objects_type_map.keys()),
+                costs=list(self._cost_expressions.values()),
+                constraints=list(self._constraint_expressions.values()),
+            )
+            self._solver.callback(self._callback)
         try:
             opti_solution = self._solver.solve()
         except Exception as err:  # noqa
-            raise OptiFailure(message=err)
+            if use_callback and self._callback.best_iteration is not None:
+                self._logger.warning(
+                    "Opti failed to solve the problem, but the callback managed to save"
+                    " an intermediate solution at "
+                    f"iteration {self._callback.best_iteration}."
+                )
+                self._output_cost = self._callback.best_cost
+                self._output_solution = self._generate_solution_output(
+                    variables=self._variables,
+                    input_solution=self._callback.best_objects,
+                )
+                self._cost_values = {
+                    name: float(
+                        self._callback.best_cost_values[self._cost_expressions[name]]
+                    )
+                    for name in self._cost_expressions
+                }
+                self._constraint_values = {
+                    name: np.array(
+                        (
+                            self._callback.best_constraint_multipliers[
+                                self._constraint_expressions[name]
+                            ]
+                        )
+                    )
+                    for name in self._constraint_expressions
+                }
+                return
+
+            raise OptiFailure(message=err, callback_used=use_callback)
 
         self._output_cost = opti_solution.value(self._cost)
         self._output_solution = self._generate_solution_output(
diff --git a/test/test_optimization_problem.py b/test/test_optimization_problem.py
index 3644ab9d..1c060020 100644
--- a/test/test_optimization_problem.py
+++ b/test/test_optimization_problem.py
@@ -9,10 +9,12 @@
     OptiFailure,
     OptimizationObject,
     OptimizationProblem,
+    OptiSolver,
     Parameter,
     StorageType,
     Variable,
     default_storage_field,
+    opti_callback,
 )
 
 
@@ -263,3 +265,19 @@ def test_opti_failure():
         print("Received error: ", err)
     else:
         assert False
+
+
+def test_opti_callback():
+    opti_solver = OptiSolver(
+        callback_criterion=opti_callback.BestCost()
+        | opti_callback.BestPrimalInfeasibility()
+    )
+    problem, variables = OptimizationProblem.create(
+        input_structure=SwitchVar(), optimization_solver=opti_solver
+    )
+
+    problem.add_constraint(variables.x <= 1)
+    problem.add_constraint(variables.x >= 0)
+    problem.add_constraint(variables.x**2 == 10)
+
+    problem.solve()

From fd830212043167c380a5557b9ab1ec116f60c2db Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Tue, 9 Jan 2024 14:38:27 +0100
Subject: [PATCH 55/57] Allow avoiding saving the cost values and constraint
 multipliers to save time in the callback

---
 src/hippopt/base/opti_solver.py | 58 ++++++++++++++++++++++++---------
 1 file changed, 42 insertions(+), 16 deletions(-)

diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 5d037237..955496e1 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -66,6 +66,12 @@ class OptiSolver(OptimizationSolver):
         default=None
     )
     _callback: SaveBestUnsolvedVariablesCallback = dataclasses.field(default=None)
+    _callback_save_costs: bool = dataclasses.field(default=True)
+    _callback_save_constraint_multipliers: bool = dataclasses.field(default=True)
+    callback_save_costs: dataclasses.InitVar[bool] = dataclasses.field(default=None)
+    callback_save_constraint_multipliers: dataclasses.InitVar[bool] = dataclasses.field(
+        default=None
+    )
 
     _cost: cs.MX = dataclasses.field(default=None)
     _cost_expressions: dict[str, cs.MX] = dataclasses.field(default=None)
@@ -97,6 +103,8 @@ def __post_init__(
         options_solver: dict[str, Any] = None,
         options_plugin: dict[str, Any] = None,
         callback_criterion: CallbackCriterion = None,
+        callback_save_costs: bool = True,
+        callback_save_constraint_multipliers: bool = True,
     ):
         self._solver = cs.Opti(problem_type)
         self._inner_solver = (
@@ -112,6 +120,10 @@ def __post_init__(
             self._inner_solver, self._options_plugin, self._options_solver
         )
         self._callback_criterion = callback_criterion
+        self._callback_save_costs = callback_save_costs
+        self._callback_save_constraint_multipliers = (
+            callback_save_constraint_multipliers
+        )
         self._cost_expressions = {}
         self._constraint_expressions = {}
         self._objects_type_map = {}
@@ -658,8 +670,12 @@ def solve(self) -> None:
                 criterion=self._callback_criterion,
                 opti=self._solver,
                 optimization_objects=list(self._objects_type_map.keys()),
-                costs=list(self._cost_expressions.values()),
-                constraints=list(self._constraint_expressions.values()),
+                costs=list(self._cost_expressions.values())
+                if self._callback_save_costs
+                else [],
+                constraints=list(self._constraint_expressions.values())
+                if self._callback_save_constraint_multipliers
+                else [],
             )
             self._solver.callback(self._callback)
         try:
@@ -676,22 +692,32 @@ def solve(self) -> None:
                     variables=self._variables,
                     input_solution=self._callback.best_objects,
                 )
-                self._cost_values = {
-                    name: float(
-                        self._callback.best_cost_values[self._cost_expressions[name]]
-                    )
-                    for name in self._cost_expressions
-                }
-                self._constraint_values = {
-                    name: np.array(
-                        (
-                            self._callback.best_constraint_multipliers[
-                                self._constraint_expressions[name]
+                self._cost_values = (
+                    {
+                        name: float(
+                            self._callback.best_cost_values[
+                                self._cost_expressions[name]
                             ]
                         )
-                    )
-                    for name in self._constraint_expressions
-                }
+                        for name in self._cost_expressions
+                    }
+                    if self._callback_save_costs
+                    else {}
+                )
+                self._constraint_values = (
+                    {
+                        name: np.array(
+                            (
+                                self._callback.best_constraint_multipliers[
+                                    self._constraint_expressions[name]
+                                ]
+                            )
+                        )
+                        for name in self._constraint_expressions
+                    }
+                    if self._callback_save_constraint_multipliers
+                    else {}
+                )
                 return
 
             raise OptiFailure(message=err, callback_used=use_callback)

From a4e4c716cb726a329f072924b0baf046a6ea326f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 10 Jan 2024 12:47:10 +0100
Subject: [PATCH 56/57] Saving parameters only once in the opti callback

---
 src/hippopt/base/opti_callback.py | 24 ++++++++++++++----------
 src/hippopt/base/opti_solver.py   | 11 ++++++++++-
 2 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/src/hippopt/base/opti_callback.py b/src/hippopt/base/opti_callback.py
index 0481760c..3cfc2576 100644
--- a/src/hippopt/base/opti_callback.py
+++ b/src/hippopt/base/opti_callback.py
@@ -302,7 +302,8 @@ def __init__(
         self,
         criterion: CallbackCriterion,
         opti: cs.Opti,
-        optimization_objects: list[cs.MX],
+        variables: list[cs.MX],
+        parameters: list[cs.MX],
         costs: list[cs.MX],
         constraints: list[cs.MX],
     ) -> None:
@@ -315,7 +316,8 @@ def __init__(
         # so the weakref is to avoid circular references
         self.opti = weakref.proxy(opti)
         self.criterion.set_opti(opti)
-        self.optimization_objects = optimization_objects
+        self.variables = variables
+        self.parameters = parameters
         self.cost = costs
         self.constraints = constraints
 
@@ -324,7 +326,7 @@ def __init__(
         self.best_cost = None
         self.best_cost_values = {}
         self.best_constraint_multipliers = {}
-        self.ignore_map = {obj: False for obj in self.optimization_objects}
+        self.ignore_map = {obj: False for obj in self.variables + self.parameters}
 
     def call(self, i: int) -> None:
         """"""
@@ -337,16 +339,18 @@ def call(self, i: int) -> None:
 
             self.best_iteration = i
             self.best_cost = self.opti.debug.value(self.opti.f)
-            self.best_objects = {}
-            for optimization_object in self.optimization_objects:
-                if self.ignore_map[optimization_object]:
+            for variable in self.variables:
+                if self.ignore_map[variable]:
                     continue
                 try:
-                    self.best_objects[optimization_object] = self.opti.debug.value(
-                        optimization_object
-                    )
+                    self.best_objects[variable] = self.opti.debug.value(variable)
                 except Exception as err:  # noqa
-                    self.ignore_map[optimization_object] = True
+                    self.ignore_map[variable] = True
+            for parameter in self.parameters:
+                if self.ignore_map[parameter]:
+                    continue
+                self.best_objects[parameter] = self.opti.debug.value(parameter)
+                self.ignore_map[parameter] = True  # Parameters are saved only once
 
             self.best_cost_values = {
                 cost: self.opti.debug.value(cost) for cost in self.cost
diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py
index 955496e1..715c974a 100644
--- a/src/hippopt/base/opti_solver.py
+++ b/src/hippopt/base/opti_solver.py
@@ -666,10 +666,19 @@ def solve(self) -> None:
             )
         use_callback = self._callback_criterion is not None
         if use_callback:
+            variables = []
+            parameters = []
+            for obj in self._objects_type_map:
+                if self._objects_type_map[obj] is Variable.StorageTypeValue:
+                    variables.append(obj)
+                elif self._objects_type_map[obj] is Parameter.StorageTypeValue:
+                    parameters.append(obj)
+
             self._callback = SaveBestUnsolvedVariablesCallback(
                 criterion=self._callback_criterion,
                 opti=self._solver,
-                optimization_objects=list(self._objects_type_map.keys()),
+                variables=variables,
+                parameters=parameters,
                 costs=list(self._cost_expressions.values())
                 if self._callback_save_costs
                 else [],

From 535ce016d1620e06bbc4eef52de66d5de7d9f990 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 15 Jan 2024 14:14:39 +0100
Subject: [PATCH 57/57] Removed pinning of metis

---
 .github/workflows/ci_cd.yml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml
index fb133516..d00f5683 100644
--- a/.github/workflows/ci_cd.yml
+++ b/.github/workflows/ci_cd.yml
@@ -38,7 +38,6 @@ jobs:
         shell: bash -l {0}
         run: |
           mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib
-          mamba install metis=5.1.0 # Reminder for https://github.com/conda-forge/conda-forge-pinning-feedstock/pull/4857#issuecomment-1699470569
           mamba list
 
       - name: Install