Updating the visual info after the parametric modifications

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

@@ -53,8 +53,9 @@ def __init__(
         self.name = link.name
         self.length_multiplier = length_multiplier
         self.densities = densities
-        self.visuals = link.visual
-        self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
+        self.original_visual = link.visual
+        self.visuals = [link.visual]
+        self.geometry_type, self.visual_data = self.get_geometry(self.original_visual)
         self.link = link
         self.link_offset = self.compute_offset()
         (self.volume, self.visual_data_new) = self.compute_volume()
@@ -65,10 +66,11 @@ def __init__(
         self.inertial.mass = self.mass
         self.inertial.inertia = self.I
         self.inertial.origin = self.origin
+        self.update_visuals()
 
     def get_principal_length(self):
         """Method computing the principal link length, i.e. the dimension in which the kinematic chain grows"""
-        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy]
         if self.geometry_type == Geometry.CYLINDER:
             if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0:
                 v_l = (
@@ -88,7 +90,7 @@ def get_principal_length(self):
 
     def get_principal_length_parametric(self):
         """Method computing the principal link length parametric, i.e. the dimension in which the kinematic chain grows"""
-        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy]
         if self.geometry_type == Geometry.CYLINDER:
             if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0:
                 v_l = (
@@ -111,7 +113,7 @@ def compute_offset(self):
         Returns:
             npt.ArrayLike: link offset
         """
-        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy]
         v_l = self.get_principal_length()
         v_o = xyz_rpy[2]
         if v_o < 0:
@@ -126,7 +128,7 @@ def compute_joint_offset(self, joint_i, parent_offset):
             npt.ArrayLike: the child joint offset
         """
         # Taking the principal direction i.e. the length
-        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy]
         v_l = self.get_principal_length()
         j_0 = joint_i.origin.xyz[2]
         v_o = xyz_rpy[2]
@@ -170,7 +172,6 @@ def compute_volume(self):
             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 = 0.0
             visual_data_new = self.visual_data.radius * self.length_multiplier
             volume = 4 * math.pi * visual_data_new**3 / 3
         return volume, visual_data_new
@@ -192,7 +193,7 @@ def modify_origin(self):
             (npt.ArrayLike): the link origin parametrized
         """
         origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy]
         v_o = xyz_rpy[2]
         length = self.get_principal_length_parametric()
         if v_o < 0:
@@ -221,7 +222,7 @@ def compute_inertia_parametric(self):
         Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia
         """
         I = I_parametric()
-        xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
+        xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy]
         if self.geometry_type == Geometry.BOX:
             I.ixx = (
                 self.mass
@@ -281,7 +282,7 @@ def spatial_inertia(self) -> npt.ArrayLike:
     def homogeneous(self) -> npt.ArrayLike:
         """
         Returns:
-            npt.ArrayLike: the homogeneus transform of the link
+            npt.ArrayLike: the homogeneous transform of the link
         """
 
         o = self.math.factory.zeros(3)
@@ -293,3 +294,13 @@ def homogeneous(self) -> npt.ArrayLike:
             o,
             rpy,
         )
+
+    def update_visuals(self):
+        if self.geometry_type == Geometry.BOX:
+            self.visuals[0].geometry.size = self.visual_data_new
+            self.visuals[0].origin.xyz[2] = self.origin[2]
+        elif self.geometry_type == Geometry.CYLINDER:
+            self.visuals[0].geometry.length = self.visual_data_new[0]
+            self.visuals[0].origin.xyz[2] = self.origin[2]
+        elif self.geometry_type == Geometry.SPHERE:
+            self.visuals[0].geometry.radius = self.visual_data_new