Added method to retrieve the original densities

src/adam/parametric/casadi/computations_parametric.py

@@ -1,6 +1,7 @@
 # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
+from typing import List
 
 import casadi as cs
 import numpy as np
@@ -9,7 +10,7 @@
 from adam.core import RBDAlgorithms
 from adam.core.constants import Representations
 from adam.model import Model
-from adam.parametric.model import URDFParametricModelFactory
+from adam.parametric.model import URDFParametricModelFactory, ParametricLink
 
 
 class KinDynComputationsParametric:
@@ -37,6 +38,7 @@ def __init__(
         n_param_links = len(links_name_list)
         self.densities = cs.SX.sym("densities", n_param_links)
         self.length_multiplier = cs.SX.sym("length_multiplier", n_param_links)
+        self.links_name_list = links_name_list
         factory = URDFParametricModelFactory(
             path=urdfstring,
             math=math,
@@ -258,3 +260,17 @@ def get_total_mass(self) -> cs.Function:
         return cs.Function(
             "m", [self.length_multiplier, self.densities], [m], self.f_opts
         )
+
+    def get_original_densities(self) -> List[float]:
+        """Returns the original densities of the links
+
+        Returns:
+            densities: The original densities
+        """
+        densities = []
+        model = self.rbdalgos.model
+        for name in self.links_name_list:
+            link = model.links[name]
+            assert isinstance(link, ParametricLink)
+            densities.append(link.original_density)
+        return densities

src/adam/parametric/jax/computations_parametric.py

@@ -2,6 +2,8 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
+from typing import List
+
 import jax.numpy as jnp
 import numpy as np
 from jax import grad, jit, vmap
@@ -10,7 +12,7 @@
 from adam.core.constants import Representations
 from adam.jax.jax_like import SpatialMath
 from adam.model import Model
-from adam.parametric.model import URDFParametricModelFactory
+from adam.parametric.model import URDFParametricModelFactory, ParametricLink
 
 
 class KinDynComputationsParametric:
@@ -446,3 +448,17 @@ def get_total_mass(
         self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.get_total_mass()
+
+    def get_original_densities(self) -> List[float]:
+        """Returns the original densities of the links
+
+        Returns:
+            densities: The original densities
+        """
+        densities = []
+        model = self.rbdalgos.model
+        for name in self.links_name_list:
+            link = model.links[name]
+            assert isinstance(link, ParametricLink)
+            densities.append(link.original_density)
+        return densities

src/adam/parametric/model/parametric_factories/parametric_link.py

@@ -43,8 +43,12 @@ def __init__(
         self.original_visual = copy.deepcopy(link.visual)
         self.visuals = [copy.deepcopy(link.visual)]
         self.geometry_type, self.visual_data = self.get_geometry(self.original_visual)
+        original_volume, _ = self.compute_volume(length_multiplier=1.0)
+        self.original_density = link.inertial.mass / original_volume
         self.link_offset = self.compute_offset()
-        (self.volume, self.visual_data_new) = self.compute_volume()
+        (self.volume, self.visual_data_new) = self.compute_volume(
+            length_multiplier=self.length_multiplier
+        )
         self.mass = self.compute_mass()
         self.inertial = Inertial(self.mass)
         self.inertial.mass = self.mass
@@ -139,7 +143,7 @@ def get_geometry(visual_obj):
             f"The visual type {visual_obj.geometry.__class__} is not supported"
         )
 
-    def compute_volume(self):
+    def compute_volume(self, length_multiplier):
         """
         Returns:
             (npt.ArrayLike, npt.ArrayLike): the volume and the dimension parametric
@@ -150,21 +154,20 @@ def compute_volume(self):
         if self.geometry_type == Geometry.BOX:
             visual_data_new[0] = self.visual_data.size[0]  # * self.length_multiplier[0]
             visual_data_new[1] = self.visual_data.size[1]  # * self.length_multiplier[1]
-            visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier
+            visual_data_new[2] = self.visual_data.size[2] * length_multiplier
             volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2]
         elif self.geometry_type == Geometry.CYLINDER:
-            visual_data_new[0] = self.visual_data.length * self.length_multiplier
+            visual_data_new[0] = self.visual_data.length * length_multiplier
             visual_data_new[1] = self.visual_data.radius  # * self.length_multiplier[1]
             volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0]
         elif self.geometry_type == Geometry.SPHERE:
-            visual_data_new = self.visual_data.radius * self.length_multiplier
+            visual_data_new = self.visual_data.radius * length_multiplier
             volume = 4 * math.pi * visual_data_new**3 / 3
         return volume, visual_data_new
 
-    """Function that computes the mass starting from the densities, the length multiplier and the link"""
-
     def compute_mass(self):
         """
+        Function that computes the mass starting from the densities, and the link volume
         Returns:
             (npt.ArrayLike): the link mass
         """

src/adam/parametric/numpy/computations_parametric.py

@@ -3,11 +3,12 @@
 # GNU Lesser General Public License v2.1 or any later version.
 
 import numpy as np
+from typing import List
 
 from adam.core.rbd_algorithms import RBDAlgorithms
 from adam.core.constants import Representations
 from adam.model import Model
-from adam.parametric.model import URDFParametricModelFactory
+from adam.parametric.model import URDFParametricModelFactory, ParametricLink
 from adam.numpy.numpy_like import SpatialMath
 
 
@@ -441,3 +442,17 @@ def get_total_mass(
         self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = model.NDoF
         return self.rbdalgos.get_total_mass()
+
+    def get_original_densities(self) -> List[float]:
+        """Returns the original densities of the links
+
+        Returns:
+            densities: The original densities
+        """
+        densities = []
+        model = self.rbdalgos.model
+        for name in self.links_name_list:
+            link = model.links[name]
+            assert isinstance(link, ParametricLink)
+            densities.append(link.original_density)
+        return densities

src/adam/parametric/pytorch/computations_parametric.py

@@ -4,11 +4,12 @@
 
 import numpy as np
 import torch
+from typing import List
 
 from adam.core.rbd_algorithms import RBDAlgorithms
 from adam.core.constants import Representations
-from adam.model import Model, URDFModelFactory
-from adam.parametric.model import URDFParametricModelFactory
+from adam.model import Model
+from adam.parametric.model import URDFParametricModelFactory, ParametricLink
 from adam.pytorch.torch_like import SpatialMath
 
 
@@ -445,3 +446,17 @@ def get_total_mass(
         self.rbdalgos.set_frame_velocity_representation(self.representation)
         self.NDoF = self.rbdalgos.NDoF
         return self.rbdalgos.get_total_mass()
+
+    def get_original_densities(self) -> List[float]:
+        """Returns the original densities of the links
+
+        Returns:
+            densities: The original densities
+        """
+        densities = []
+        model = self.rbdalgos.model
+        for name in self.links_name_list:
+            link = model.links[name]
+            assert isinstance(link, ParametricLink)
+            densities.append(link.original_density)
+        return densities