Merge pull request #85 from ami-iit/fix-link-filtering

Fix link filtering

src/adam/model/__init__.py

@@ -1,4 +1,4 @@
-from .abc_factories import Joint, Link, ModelFactory
+from .abc_factories import Joint, Link, ModelFactory, Inertial, Pose
 from .model import Model
 from .std_factories.std_joint import StdJoint
 from .std_factories.std_link import StdLink

src/adam/model/abc_factories.py

@@ -88,8 +88,36 @@ class Inertial:
     """Inertial description"""
 
     mass: npt.ArrayLike
-    inertia = Inertia
-    origin = Pose
+    inertia: Inertia
+    origin: Pose
+
+    @staticmethod
+    def zero() -> "Inertial":
+        """Returns an Inertial object with zero mass and inertia"""
+        return Inertial(
+            mass=0.0,
+            inertia=Inertia(
+                ixx=0.0,
+                ixy=0.0,
+                ixz=0.0,
+                iyy=0.0,
+                iyz=0.0,
+                izz=0.0,
+            ),
+            origin=Pose(xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]),
+        )
+
+    def set_mass(self, mass: npt.ArrayLike) -> "Inertial":
+        """Set the mass of the inertial object"""
+        self.mass = mass
+
+    def set_inertia(self, inertia: Inertia) -> "Inertial":
+        """Set the inertia of the inertial object"""
+        self.inertia = inertia
+
+    def set_origin(self, origin: Pose) -> "Inertial":
+        """Set the origin of the inertial object"""
+        self.origin = origin
 
 
 @dataclasses.dataclass

src/adam/model/std_factories/std_link.py

@@ -2,7 +2,7 @@
 import urdf_parser_py.urdf
 
 from adam.core.spatial_math import SpatialMath
-from adam.model import Link
+from adam.model import Link, Inertial, Pose
 
 
 class StdLink(Link):
@@ -15,10 +15,15 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath):
         self.inertial = link.inertial
         self.collisions = link.collisions
 
+        # if the link has no inertial properties (a connecting frame), let's add them
+        if link.inertial is None:
+            link.inertial = Inertial.zero()
+
         # 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:
-            link.inertial.origin.xyz = [0, 0, 0]
-            link.inertial.origin.rpy = [0, 0, 0]
+            link.inertial.origin = Pose(xyz=[0, 0, 0], rpy=[0, 0, 0])
+
+        self.inertial = link.inertial
 
     def spatial_inertia(self) -> npt.ArrayLike:
         """

src/adam/model/std_factories/std_model.py

@@ -44,9 +44,8 @@ def __init__(self, path: str, math: SpatialMath):
         # to have a useless and noisy warning, let's remove before hands all the sensor elements,
         # that anyhow are not parser by urdf_parser_py or adam
         # See https://github.com/ami-iit/ADAM/issues/59
-        xml_file = open(path, "r")
-        xml_string = xml_file.read()
-        xml_file.close()
+        with open(path, "r") as xml_file:
+            xml_string = xml_file.read()
         xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string)
         self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string(
             xml_string_without_sensors_tags
@@ -64,17 +63,45 @@ def get_links(self) -> List[StdLink]:
         """
         Returns:
             List[StdLink]: build the list of the links
+
+        A link is considered a "real" link if
+        - it has an inertial
+        - it has children
+        - if it has no children and no inertial, it is at lest connected to the parent with a non fixed joint
         """
         return [
-            self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None
+            self.build_link(l)
+            for l in self.urdf_desc.links
+            if (
+                l.inertial is not None
+                or l.name in self.urdf_desc.child_map.keys()
+                or any(
+                    j.type != "fixed"
+                    for j in self.urdf_desc.joints
+                    if j.child == l.name
+                )
+            )
         ]
 
     def get_frames(self) -> List[StdLink]:
         """
         Returns:
             List[StdLink]: build the list of the links
+
+        A link is considered a "fake" link (frame) if
+        - it has no inertial
+        - it does not have children
+        - it is connected to the parent with a fixed joint
         """
-        return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None]
+        return [
+            self.build_link(l)
+            for l in self.urdf_desc.links
+            if l.inertial is None
+            and l.name not in self.urdf_desc.child_map.keys()
+            and all(
+                j.type == "fixed" for j in self.urdf_desc.joints if j.child == l.name
+            )
+        ]
 
     def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> StdJoint:
         """

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

@@ -50,10 +50,11 @@ def __init__(
             length_multiplier=self.length_multiplier
         )
         self.mass = self.compute_mass()
-        self.inertial = Inertial(self.mass)
-        self.inertial.mass = self.mass
-        self.inertial.inertia = self.compute_inertia_parametric()
-        self.inertial.origin = self.modify_origin()
+        inertia_parametric = self.compute_inertia_parametric()
+        origin = self.modify_origin()
+        self.inertial = Inertial(
+            mass=self.mass, inertia=inertia_parametric, origin=origin
+        )
         self.update_visuals()
 
     def get_principal_length(self):