Merge branch 'main' into loading_from_string

examples/ik.ipynb

@@ -7,7 +7,7 @@
         "colab_type": "text"
       },
       "source": [
-        "<a href=\"https://colab.research.google.com/github/ami-iit/ADAM/blob/ik-example/examples/ik.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
+        "<a href=\"https://colab.research.google.com/github/ami-iit/adam/blob/main/examples/ik.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
       ]
     },
     {
@@ -306,4 +306,4 @@
   },
   "nbformat": 4,
   "nbformat_minor": 0
-}
+}

examples/mpc-ik.ipynb

@@ -7,7 +7,7 @@
         "colab_type": "text"
       },
       "source": [
-        "<a href=\"https://colab.research.google.com/github/ami-iit/ADAM/blob/main/examples/mpc-ik.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
+        "<a href=\"https://colab.research.google.com/github/ami-iit/adam/blob/main/examples/mpc-ik.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
       ]
     },
     {
@@ -338,4 +338,4 @@
   },
   "nbformat": 4,
   "nbformat_minor": 0
-}
+}

src/adam/casadi/casadi_like.py

@@ -103,37 +103,39 @@ def T(self) -> "CasadiLike":
 
 
 class CasadiLikeFactory(ArrayLikeFactory):
-    @staticmethod
-    def zeros(*x: int) -> "CasadiLike":
+
+    def __init__(self, cs_type: Union[cs.SX, cs.DM]):
+        self.cs_type = cs_type
+
+    def zeros(self, *x: int) -> "CasadiLike":
         """
         Returns:
             CasadiLike: Matrix of zeros of dim *x
         """
-        return CasadiLike(cs.SX.zeros(*x))
+        return CasadiLike(self.cs_type.zeros(*x))
 
-    @staticmethod
-    def eye(x: int) -> "CasadiLike":
+    def eye(self, x: int) -> "CasadiLike":
         """
         Args:
             x (int): matrix dimension
 
         Returns:
             CasadiLike: Identity matrix
         """
-        return CasadiLike(cs.SX.eye(x))
+        return CasadiLike(self.cs_type.eye(x))
 
-    @staticmethod
-    def array(*x) -> "CasadiLike":
+    def array(self, *x) -> "CasadiLike":
         """
         Returns:
             CasadiLike: Vector wrapping *x
         """
-        return CasadiLike(cs.SX(*x))
+        return CasadiLike(self.cs_type(*x))
 
 
 class SpatialMath(SpatialMath):
-    def __init__(self):
-        super().__init__(CasadiLikeFactory)
+
+    def __init__(self, cs_type: Union[cs.SX, cs.DM]):
+        super().__init__(CasadiLikeFactory(cs_type))
 
     @staticmethod
     def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":

src/adam/casadi/computations.py

@@ -22,6 +22,7 @@ def __init__(
         urdfstring: str,
         joints_name_list: list = None,
         root_link: str = "root_link",
+        cs_type: Union[cs.SX, cs.DM] = cs.SX,
         gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]),
         f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast")),
     ) -> None:
@@ -31,7 +32,7 @@ def __init__(
             joints_name_list (list): list of the actuated joints
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
-        math = SpatialMath()
+        math = SpatialMath(cs_type)
         factory = URDFModelFactory(path=urdfstring, math=math)
         model = Model.build(factory=factory, joints_name_list=joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=math)

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

@@ -69,6 +69,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
+        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
@@ -86,17 +88,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/casadi/computations_parametric.py

@@ -1,7 +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
+from typing import List, Union
 
 import casadi as cs
 import numpy as np
@@ -24,6 +24,7 @@ def __init__(
         joints_name_list: list,
         links_name_list: list,
         root_link: str = "root_link",
+        cs_type: Union[cs.SX, cs.DM] = cs.SX,
         gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]),
         f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast")),
     ) -> None:
@@ -34,7 +35,7 @@ def __init__(
             links_name_list (list): list of the parametrized links
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
-        math = SpatialMath()
+        math = SpatialMath(cs_type)
         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)

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):