Removed usages of link origin.

Using link.inertial.origin instead
ami-iit · Mar 7, 2024 · 603d997 · 603d997
1 parent af2d1c4
commit 603d997
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 14 deletions.
diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py
@@ -14,7 +14,6 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath):
         self.visuals = link.visuals
         self.inertial = link.inertial
         self.collisions = link.collisions
-        self.origin = link.origin
 
         # if the link has inertial properties, but the origin is None, let's add it
         if link.inertial is not None and link.inertial.origin is None:

diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py
@@ -45,7 +45,7 @@ def modify(self, parent_joint_offset: npt.ArrayLike):
 
         length = self.parent_parametric.get_principal_length_parametric()
         # Ack for avoiding depending on casadi
-        vo = self.parent_parametric.origin[2]
+        vo = self.parent_parametric.inertial.origin[2]
         xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         xyz_rpy[0] = self.joint.origin.xyz[0]
         xyz_rpy[1] = self.joint.origin.xyz[1]

diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py
@@ -62,11 +62,10 @@ def __init__(
         (self.volume, self.visual_data_new) = self.compute_volume()
         self.mass = self.compute_mass()
         self.I = self.compute_inertia_parametric()
-        self.origin = self.modify_origin()
         self.inertial = Inertial(self.mass)
         self.inertial.mass = self.mass
         self.inertial.inertia = self.I
-        self.inertial.origin = self.origin
+        self.inertial.origin = self.modify_origin()
         self.update_visuals()
 
     def get_principal_length(self):
@@ -272,10 +271,10 @@ def spatial_inertia(self) -> npt.ArrayLike:
         I = self.I
         mass = self.mass
         o = self.math.factory.zeros(3)
-        o[0] = self.origin[0]
-        o[1] = self.origin[1]
-        o[2] = self.origin[2]
-        rpy = self.origin[3:]
+        o[0] = self.inertial.origin[0]
+        o[1] = self.inertial.origin[1]
+        o[2] = self.inertial.origin[2]
+        rpy = self.inertial.origin[3:]
         return self.math.spatial_inertial_with_parameters(I, mass, o, rpy)
 
     def homogeneous(self) -> npt.ArrayLike:
@@ -285,10 +284,10 @@ def homogeneous(self) -> npt.ArrayLike:
         """
 
         o = self.math.factory.zeros(3)
-        o[0] = self.origin[0]
-        o[1] = self.origin[1]
-        o[2] = self.origin[2]
-        rpy = self.origin[3:]
+        o[0] = self.inertial.origin[0]
+        o[1] = self.inertial.origin[1]
+        o[2] = self.inertial.origin[2]
+        rpy = self.inertial.origin[3:]
         return self.math.H_from_Pos_RPY(
             o,
             rpy,
@@ -297,9 +296,9 @@ def homogeneous(self) -> npt.ArrayLike:
     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]
+            self.visuals[0].origin.xyz[2] = self.inertial.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]
+            self.visuals[0].origin.xyz[2] = self.inertial.origin[2]
         elif self.geometry_type == Geometry.SPHERE:
             self.visuals[0].geometry.radius = self.visual_data_new