Added modifications to use a parametric model in the humanoid_kinodyn…

…amic planner

src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py

@@ -1,6 +1,7 @@
 import copy
 
 import adam.casadi
+import adam.parametric.casadi
 import casadi as cs
 import numpy as np
 
@@ -21,14 +22,26 @@ def __init__(self, settings: Settings) -> None:
         if not settings.is_valid():
             raise ValueError("Settings are not valid")
         self.settings = copy.deepcopy(settings)
-        self.kin_dyn_object = adam.casadi.KinDynComputations(
-            urdfstring=self.settings.robot_urdf,
-            joints_name_list=self.settings.joints_name_list,
-            root_link=self.settings.root_link,
-            gravity=self.settings.gravity,
-            f_opts=self.settings.casadi_function_options,
-        )
-        self.numeric_mass = self.kin_dyn_object.get_total_mass()
+        if self.settings.parametric_link_names is not None:
+            self.parametric = True
+            self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric(
+                urdfstring=self.settings.robot_urdf,
+                joints_name_list=self.settings.joints_name_list,
+                links_name_list=self.settings.parametric_link_names,
+                root_link=self.settings.root_link,
+                gravity=self.settings.gravity,
+                f_opts=self.settings.casadi_function_options,
+            )
+        else:
+            self.parametric = False
+            self.kin_dyn_object = adam.casadi.KinDynComputations(
+                urdfstring=self.settings.robot_urdf,
+                joints_name_list=self.settings.joints_name_list,
+                root_link=self.settings.root_link,
+                gravity=self.settings.gravity,
+                f_opts=self.settings.casadi_function_options,
+            )
+            self.numeric_mass = self.kin_dyn_object.get_total_mass()
 
         self.variables = Variables(
             settings=self.settings, kin_dyn_object=self.kin_dyn_object
@@ -858,27 +871,36 @@ def _add_periodicity_expression(self, all_contact_points):
         )
 
     def _apply_mass_regularization(self, input_var: Variables) -> Variables:
-        assert self.numeric_mass > 0
+        if self.parametric:
+            numeric_mass_fun = self.kin_dyn_object.get_total_mass()
+            numeric_mass = numeric_mass_fun(  # noqa It is a function, not a float
+                input_var.parametric_link_densities,  # WARNING the order might change
+                input_var.parametric_link_length_multipliers,
+            )
+        else:
+            numeric_mass = self.numeric_mass
+
+        assert numeric_mass > 0
         output = input_var
         if output.initial_state is not None:
             if (
                 output.initial_state.centroidal_momentum is not None
                 and len(output.initial_state.centroidal_momentum.shape) > 0
                 and output.initial_state.centroidal_momentum.shape[0] == 6
             ):
-                output.initial_state.centroidal_momentum /= self.numeric_mass
+                output.initial_state.centroidal_momentum /= numeric_mass
             for point in (
                 output.initial_state.contact_points.left
                 + output.initial_state.contact_points.right
             ):
-                point.f /= self.numeric_mass
+                point.f /= numeric_mass
 
         if output.final_state is not None:
             for point in (
                 output.final_state.contact_points.left
                 + output.final_state.contact_points.right
             ):
-                point.f /= self.numeric_mass
+                point.f /= numeric_mass
 
         if output.system is None:
             return output
@@ -889,34 +911,43 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables:
 
         for system in system_list:
             if system.centroidal_momentum is not None:
-                system.centroidal_momentum /= self.numeric_mass
+                system.centroidal_momentum /= numeric_mass
             for point in system.contact_points.left + system.contact_points.right:
-                point.f /= self.numeric_mass
+                point.f /= numeric_mass
 
         return output
 
     def _undo_mass_regularization(self, input_var: Variables) -> Variables:
-        assert self.numeric_mass > 0
+        if self.parametric:
+            numeric_mass_fun = self.kin_dyn_object.get_total_mass()
+            numeric_mass = numeric_mass_fun(  # noqa It is a function, not a float
+                input_var.parametric_link_densities,  # WARNING the order might change
+                input_var.parametric_link_length_multipliers,
+            )
+        else:
+            numeric_mass = self.numeric_mass
+
+        assert numeric_mass > 0
         output = input_var
         if output.initial_state is not None:
             if (
                 output.initial_state.centroidal_momentum is not None
                 and len(output.initial_state.centroidal_momentum.shape) > 0
                 and output.initial_state.centroidal_momentum.shape[0] == 6
             ):
-                output.initial_state.centroidal_momentum *= self.numeric_mass
+                output.initial_state.centroidal_momentum *= numeric_mass
             for point in (
                 output.initial_state.contact_points.left
                 + output.initial_state.contact_points.right
             ):
-                point.f *= self.numeric_mass
+                point.f *= numeric_mass
 
         if output.final_state is not None:
             for point in (
                 output.final_state.contact_points.left
                 + output.final_state.contact_points.right
             ):
-                point.f *= self.numeric_mass
+                point.f *= numeric_mass
 
         if output.system is None:
             return output
@@ -927,9 +958,9 @@ def _undo_mass_regularization(self, input_var: Variables) -> Variables:
 
         for system in system_list:
             if system.centroidal_momentum is not None:
-                system.centroidal_momentum *= self.numeric_mass
+                system.centroidal_momentum *= numeric_mass
             for point in system.contact_points.left + system.contact_points.right:
-                point.f *= self.numeric_mass
+                point.f *= numeric_mass
 
         return output
 

src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py

@@ -13,6 +13,10 @@
 class Settings:
     robot_urdf: str = dataclasses.field(default=None)
     joints_name_list: list[str] = dataclasses.field(default=None)
+    parametric_link_names: list[str] = dataclasses.field(default=None)
+    initial_densities: np.ndarray = dataclasses.field(
+        default=None
+    )  # Necessary because of https://github.com/ami-iit/adam/issues/70
     contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None)
     root_link: str = dataclasses.field(default=None)
     gravity: np.array = dataclasses.field(default=None)
@@ -153,6 +157,13 @@ def is_valid(self) -> bool:
         if self.joints_name_list is None:
             logger.error("joints_name_list is None")
             ok = False
+        if self.parametric_link_names is not None:
+            if len(self.parametric_link_names) != len(self.initial_densities):
+                logger.error(
+                    f"len(parametric_link_names)={len(self.parametric_link_names)} !="
+                    f" len(initial_densities)={len(self.initial_densities)}"
+                )
+                ok = False
         if self.contact_points is None:
             logger.error("contact_points is None")
             ok = False

src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py

@@ -1,7 +1,9 @@
+import copy
 import dataclasses
 from typing import TypeVar
 
 import adam.casadi
+import adam.parametric.casadi
 import numpy as np
 
 import hippopt as hp
@@ -259,6 +261,11 @@ class Variables(hp.OptimizationObject):
 
     mass: hp.StorageType = hp.default_storage_field(hp.Parameter)
 
+    parametric_link_length_multipliers: hp.StorageType = hp.default_storage_field(
+        hp.Parameter
+    )
+    parametric_link_densities: hp.StorageType = hp.default_storage_field(hp.Parameter)
+
     initial_state: hp.CompositeType[ExtendedHumanoidState] = hp.default_composite_field(
         cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False
     )
@@ -295,14 +302,18 @@ class Variables(hp.OptimizationObject):
     )
 
     settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None)
-    kin_dyn_object: dataclasses.InitVar[adam.casadi.KinDynComputations] = (
-        dataclasses.field(default=None)
-    )
+    kin_dyn_object: dataclasses.InitVar[
+        adam.casadi.KinDynComputations
+        | adam.parametric.casadi.KinDynComputationsParametric
+    ] = dataclasses.field(default=None)
 
     def __post_init__(
         self,
         settings: Settings,
-        kin_dyn_object: adam.casadi.KinDynComputations,
+        kin_dyn_object: (
+            adam.casadi.KinDynComputations
+            | adam.parametric.casadi.KinDynComputationsParametric
+        ),
     ) -> None:
         self.system = ExtendedHumanoid(
             contact_point_descriptors=settings.contact_points,
@@ -321,7 +332,27 @@ def __post_init__(
 
         self.dt = settings.time_step
         self.gravity = kin_dyn_object.g
-        self.mass = kin_dyn_object.get_total_mass()
+
+        self.parametric_link_length_multipliers = (
+            np.ones(len(settings.parametric_link_names))
+            if settings.parametric_link_names is not None
+            else np.array([])
+        )
+        self.parametric_link_densities = (
+            copy.deepcopy(settings.initial_densities)
+            if settings.initial_densities is not None
+            else np.array([])
+        )
+
+        if isinstance(
+            kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric
+        ):
+            total_mass_fun = kin_dyn_object.get_total_mass()
+            self.mass = total_mass_fun(  # noqa
+                self.parametric_link_densities, self.parametric_link_length_multipliers
+            )  # WARNING, the order might change
+        else:
+            self.mass = kin_dyn_object.get_total_mass()
 
         self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier
         self.dcc_gain = settings.dcc_gain