diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py index 0dd4bf9..0426e31 100644 --- a/src/adam/model/__init__.py +++ b/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 diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 853dbb8..4720588 100644 --- a/src/adam/model/abc_factories.py +++ b/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 diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 9d90829..7754747 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/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: """ diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index ffeff71..5f8f4fd 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/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: """ diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index c9ae02b..1346f3f 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/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):