Getting the original densities directly from adam

ami-iit · Mar 8, 2024 · a397437 · a397437
1 parent a296cfe
commit a397437
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 26 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py
@@ -58,16 +58,27 @@ def get_planner_settings() -> walking_settings.Settings:
         "torso_2",
         "chest",
     ]
-    settings.initial_densities = [
-        1661.6863265236248,
-        727.4313078156689,
-        600.8642717368293,
-        2134.3111071426842,
-        2129.295296396375,
-        1199.0762240824756,
-        893.1076351798705,
-        626.6027187152905,
-    ]
+    # settings.parametric_link_densities = [
+    #     1661.6863265236248,
+    #     727.4313078156689,
+    #     600.8642717368293,
+    #     2134.3111071426842,
+    #     2129.295296396375,
+    #     1199.0762240824756,
+    #     893.1076351798705,
+    #     626.6027187152905,
+    # ]
+    # settings.parametric_link_length_multipliers = [
+    #     1.0,
+    #     1.0,
+    #     1.0,
+    #     1.0,
+    #     1.0,
+    #     1.0,
+    #     1.0,
+    #     1.0,
+    # ]
+
     number_of_joints = len(settings.joints_name_list)
     idyntree_model_loader = idyntree.ModelLoader()
     idyntree_model_loader.loadReducedModelFromFile(

diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py
@@ -14,9 +14,6 @@ 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)
@@ -157,13 +154,6 @@ 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

diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py
@@ -1,4 +1,3 @@
-import copy
 import dataclasses
 from typing import TypeVar
 
@@ -338,15 +337,12 @@ def __post_init__(
             if settings.parametric_link_names is not None
             else 0.0
         )
-        self.parametric_link_densities = (
-            copy.deepcopy(settings.initial_densities)
-            if settings.initial_densities is not None
-            else 0.0
-        )
 
         if isinstance(
             kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric
         ):
+            self.parametric_link_densities = kin_dyn_object.get_original_densities()
+            print("parametric_link_densities", self.parametric_link_densities)
             total_mass_fun = kin_dyn_object.get_total_mass()
             self.mass = float(
                 total_mass_fun(
@@ -356,6 +352,7 @@ def __post_init__(
             )
         else:
             self.mass = kin_dyn_object.get_total_mass()
+            self.parametric_link_densities = 0.0
 
         self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier
         self.dcc_gain = settings.dcc_gain