Merge pull request #9 from ami-iit/humanoidPlanning_base

Modifications and improvements to the base functionalities

.github/workflows/ci_cd.yml

@@ -31,11 +31,14 @@ 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 idyntree meshcat-python ffmpeg-python matplotlib
+          mamba list
 
       - name: Install
         shell: bash  -l {0}

.gitignore

@@ -130,3 +130,6 @@ dmypy.json
 
 # Pycharm files
 .idea/
+
+# VSCode files
+.vscode/

setup.cfg

@@ -59,9 +59,22 @@ style =
     isort
 testing=
     pytest
+robot_planning=
+    liecasadi
+    adam-robotics
+turnkey_planners=
+    idyntree
+visualization=
+    ffmpeg-python
+    idyntree
+    meshcat-python
+    matplotlib
 all =
     %(style)s
     %(testing)s
+    %(robot_planning)s
+    %(turnkey_planners)s
+    %(visualization)s
 
 [options.packages.find]
 where = src

src/hippopt/__init__.py

@@ -1,26 +1,30 @@
-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
-from .base.optimal_control_problem import OptimalControlProblem
+from .base.optimal_control_problem import (
+    OptimalControlProblem,
+    OptimalControlProblemInstance,
+)
 from .base.optimization_object import (
+    CompositeType,
     OptimizationObject,
     StorageType,
     TimeExpansion,
     TOptimizationObject,
+    default_composite_field,
     default_storage_field,
     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
+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 .integrators.forward_euler import ForwardEuler
-from .integrators.implicit_trapezoid import ImplicitTrapezoid
+from .base.variable import OverridableVariable, TVariable, Variable

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,

src/hippopt/base/dynamics.py

@@ -10,26 +10,45 @@
 
 @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] | cs.MX] = 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] | cs.MX, 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.
-        :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
         """
-        self._f = 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
 
@@ -48,10 +67,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,43 +88,95 @@ 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()
 
 
 @dataclasses.dataclass
 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. If one output
-          needs to be mapped to a nested item, use "." as separator, e.g. "a.b"
-        :param t_label: The label of the time variable. Default "t"
+          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.
+          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 equal(self, f: cs.Function, names_map: dict[str, str] = None) -> TDynamics:
+        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] | 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):
             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)
 
     def __eq__(
-        self, other: cs.Function | tuple[cs.Function, dict[str, str]]
+        self,
+        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[cs.MX, 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)
+            or isinstance(other, cs.MX)
+        )
         return self.equal(f=other)
 
     def state_variables(self) -> list[str]:
@@ -106,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)