Merge pull request #37 from ami-iit/small_refactor

Refactor

examples/iCub_example_casadi.ipynb

@@ -10,10 +10,11 @@
     "from adam.geometry import utils\n",
     "import numpy as np\n",
     "import casadi as cs\n",
-    "import gym_ignition_models"
+    "import icub_models"
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -26,7 +27,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "urdf_path = gym_ignition_models.get_model_file(\"iCubGazeboV2_5\")\n",
+    "urdf_path = icub_models.get_model_file(\"iCubGazeboV2_5\")\n",
     "# The joint list\n",
     "joints_name_list = [\n",
     "    'torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',\n",
@@ -41,6 +42,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -63,6 +65,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -118,6 +121,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -150,6 +154,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -176,6 +181,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -201,6 +207,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -233,6 +240,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -265,6 +273,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -295,6 +304,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -321,6 +331,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -347,6 +358,7 @@
    ]
   },
   {
+   "attachments": {},
    "cell_type": "markdown",
    "metadata": {},
    "source": [
@@ -392,7 +404,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.10"
+   "version": "3.10.11"
   },
   "metadata": {
    "interpreter": {

setup.cfg

@@ -51,8 +51,9 @@ test =
         torch
         pytest
         idyntree
-        gym-ignition-models
+        icub-models
         black
+        gym-ignition-models
 all =
         jax
         jaxlib

src/adam/casadi/__init__.py

@@ -3,3 +3,4 @@
 # GNU Lesser General Public License v2.1 or any later version.
 
 from .computations import KinDynComputations
+from .casadi_like import CasadiLike

src/adam/casadi/casadi_like.py

@@ -8,23 +8,26 @@
 import casadi as cs
 import numpy.typing as npt
 
-from adam.core.spatial_math import ArrayLike
+from adam.core.spatial_math import ArrayLike, ArrayLikeFactory, SpatialMath
+from adam.numpy import NumpyLike
 
 
 @dataclass
 class CasadiLike(ArrayLike):
+    """Wrapper class for Casadi types"""
+
     array: Union[cs.SX, cs.DM]
 
     def __matmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """Overrides @ operator"""
-        if type(self) is type(other):
+        if type(other) in [CasadiLike, NumpyLike]:
             return CasadiLike(self.array @ other.array)
         else:
             return CasadiLike(self.array @ other)
 
     def __rmatmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """Overrides @ operator"""
-        if type(self) is type(other):
+        if type(other) in [CasadiLike, NumpyLike]:
             return CasadiLike(other.array @ self.array)
         else:
             return CasadiLike(other @ self.array)
@@ -98,6 +101,8 @@ def T(self) -> "CasadiLike":
         """
         return CasadiLike(self.array.T)
 
+
+class CasadiLikeFactory(ArrayLikeFactory):
     @staticmethod
     def zeros(*x: int) -> "CasadiLike":
         """
@@ -106,19 +111,6 @@ def zeros(*x: int) -> "CasadiLike":
         """
         return CasadiLike(cs.SX.zeros(*x))
 
-    @staticmethod
-    def vertcat(*x) -> "CasadiLike":
-        """
-        Returns:
-            CasadiLike:  vertical concatenation of elements
-        """
-        # here the logic is a bit convoluted: x is a tuple containing CasadiLike
-        # cs.vertcat accepts *args. A list of cs types is created extracting the value
-        # from the CasadiLike stored in the tuple x.
-        # Then the list is unpacked with the * operator.
-        y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
-        return CasadiLike(cs.vertcat(*y))
-
     @staticmethod
     def eye(x: int) -> "CasadiLike":
         """
@@ -130,6 +122,19 @@ def eye(x: int) -> "CasadiLike":
         """
         return CasadiLike(cs.SX.eye(x))
 
+    @staticmethod
+    def array(*x) -> "CasadiLike":
+        """
+        Returns:
+            CasadiLike: Vector wrapping *x
+        """
+        return CasadiLike(cs.DM(*x))
+
+
+class SpatialMath(SpatialMath):
+    def __init__(self):
+        super().__init__(CasadiLikeFactory)
+
     @staticmethod
     def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """
@@ -144,14 +149,6 @@ def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         else:
             return CasadiLike(cs.skew(x))
 
-    @staticmethod
-    def array(*x) -> "CasadiLike":
-        """
-        Returns:
-            CasadiLike: Vector wrapping *x
-        """
-        return CasadiLike(cs.DM(*x))
-
     @staticmethod
     def sin(x: npt.ArrayLike) -> "CasadiLike":
         """
@@ -185,3 +182,21 @@ def outer(x: npt.ArrayLike, y: npt.ArrayLike) -> "CasadiLike":
             CasadiLike: outer product between x and y
         """
         return CasadiLike(cs.np.outer(x, y))
+
+    @staticmethod
+    def vertcat(*x) -> "CasadiLike":
+        """
+        Returns:
+            CasadiLike:  vertical concatenation of elements
+        """
+        # here the logic is a bit convoluted: x is a tuple containing CasadiLike
+        # cs.vertcat accepts *args. A list of cs types is created extracting the value
+        # from the CasadiLike stored in the tuple x.
+        # Then the list is unpacked with the * operator.
+        y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
+        return CasadiLike(cs.vertcat(*y))
+
+
+if __name__ == "__main__":
+    math = SpatialMath()
+    print(math.eye(3))

src/adam/casadi/computations.py

@@ -5,11 +5,12 @@
 import casadi as cs
 import numpy as np
 
-from adam.casadi.casadi_like import CasadiLike
-from adam.core.rbd_algorithms import RBDAlgorithms
+from adam.casadi.casadi_like import SpatialMath
+from adam.core import RBDAlgorithms
+from adam.model import Model, URDFModelFactory
 
 
-class KinDynComputations(RBDAlgorithms, CasadiLike):
+class KinDynComputations:
     """This is a small class that retrieves robot quantities represented in a symbolic fashion using CasADi
     in mixed representation, for Floating Base systems - as humanoid robots.
     """
@@ -28,12 +29,12 @@ def __init__(
             joints_name_list (list): list of the actuated joints
             root_link (str, optional): the first link. Defaults to 'root_link'.
         """
-        super().__init__(
-            urdfstring=urdfstring,
-            joints_name_list=joints_name_list,
-            root_link=root_link,
-            gravity=gravity,
-        )
+        math = SpatialMath()
+        factory = URDFModelFactory(path=urdfstring, math=math)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = self.rbdalgos.NDoF
+        self.g = gravity
         self.f_opts = f_opts
 
     def mass_matrix_fun(self) -> cs.Function:
@@ -44,7 +45,7 @@ def mass_matrix_fun(self) -> cs.Function:
         """
         T_b = cs.SX.sym("T_b", 4, 4)
         s = cs.SX.sym("s", self.NDoF)
-        [M, _] = super().crba(T_b, s)
+        [M, _] = self.rbdalgos.crba(T_b, s)
         return cs.Function("M", [T_b, s], [M.array], self.f_opts)
 
     def centroidal_momentum_matrix_fun(self) -> cs.Function:
@@ -55,7 +56,7 @@ def centroidal_momentum_matrix_fun(self) -> cs.Function:
         """
         T_b = cs.SX.sym("T_b", 4, 4)
         s = cs.SX.sym("s", self.NDoF)
-        [_, Jcm] = super().crba(T_b, s)
+        [_, Jcm] = self.rbdalgos.crba(T_b, s)
         return cs.Function("Jcm", [T_b, s], [Jcm.array], self.f_opts)
 
     def forward_kinematics_fun(self, frame: str) -> cs.Function:
@@ -69,7 +70,7 @@ def forward_kinematics_fun(self, frame: str) -> cs.Function:
         """
         s = cs.SX.sym("s", self.NDoF)
         T_b = cs.SX.sym("T_b", 4, 4)
-        T_fk = super().forward_kinematics(frame, T_b, s)
+        T_fk = self.rbdalgos.forward_kinematics(frame, T_b, s)
         return cs.Function("T_fk", [T_b, s], [T_fk.array], self.f_opts)
 
     def jacobian_fun(self, frame: str) -> cs.Function:
@@ -83,7 +84,7 @@ def jacobian_fun(self, frame: str) -> cs.Function:
         """
         s = cs.SX.sym("s", self.NDoF)
         T_b = cs.SX.sym("T_b", 4, 4)
-        J_tot = super().jacobian(frame, T_b, s)
+        J_tot = self.rbdalgos.jacobian(frame, T_b, s)
         return cs.Function("J_tot", [T_b, s], [J_tot.array], self.f_opts)
 
     def relative_jacobian_fun(self, frame: str) -> cs.Function:
@@ -96,7 +97,7 @@ def relative_jacobian_fun(self, frame: str) -> cs.Function:
             J (casADi function): The Jacobian between the root and the frame
         """
         s = cs.SX.sym("s", self.NDoF)
-        J = super().relative_jacobian(frame, s)
+        J = self.rbdalgos.relative_jacobian(frame, s)
         return cs.Function("J", [s], [J.array], self.f_opts)
 
     def CoM_position_fun(self) -> cs.Function:
@@ -107,7 +108,7 @@ def CoM_position_fun(self) -> cs.Function:
         """
         s = cs.SX.sym("s", self.NDoF)
         T_b = cs.SX.sym("T_b", 4, 4)
-        com_pos = super().CoM_position(T_b, s)
+        com_pos = self.rbdalgos.CoM_position(T_b, s)
         return cs.Function("CoM_pos", [T_b, s], [com_pos.array], self.f_opts)
 
     def bias_force_fun(self) -> cs.Function:
@@ -121,7 +122,7 @@ def bias_force_fun(self) -> cs.Function:
         s = cs.SX.sym("s", self.NDoF)
         v_b = cs.SX.sym("v_b", 6)
         s_dot = cs.SX.sym("s_dot", self.NDoF)
-        h = super().rnea(T_b, s, v_b, s_dot, self.g)
+        h = self.rbdalgos.rnea(T_b, s, v_b, s_dot, self.g)
         return cs.Function("h", [T_b, s, v_b, s_dot], [h.array], self.f_opts)
 
     def coriolis_term_fun(self) -> cs.Function:
@@ -136,7 +137,7 @@ def coriolis_term_fun(self) -> cs.Function:
         v_b = cs.SX.sym("v_b", 6)
         q_dot = cs.SX.sym("q_dot", self.NDoF)
         # set in the bias force computation the gravity term to zero
-        C = super().rnea(T_b, q, v_b, q_dot, np.zeros(6))
+        C = self.rbdalgos.rnea(T_b, q, v_b, q_dot, np.zeros(6))
         return cs.Function("C", [T_b, q, v_b, q_dot], [C.array], self.f_opts)
 
     def gravity_term_fun(self) -> cs.Function:
@@ -149,5 +150,25 @@ def gravity_term_fun(self) -> cs.Function:
         T_b = cs.SX.sym("T_b", 4, 4)
         q = cs.SX.sym("q", self.NDoF)
         # set in the bias force computation the velocity to zero
-        G = super().rnea(T_b, q, np.zeros(6), np.zeros(self.NDoF), self.g)
+        G = self.rbdalgos.rnea(T_b, q, np.zeros(6), np.zeros(self.NDoF), self.g)
         return cs.Function("G", [T_b, q], [G.array], self.f_opts)
+
+    def forward_kinematics(self, frame, T_b, s) -> cs.Function:
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+
+        Returns:
+            T_fk (casADi function): The fk represented as Homogenous transformation matrix
+        """
+
+        return self.rbdalgos.forward_kinematics(frame, T_b, s)
+
+    def get_total_mass(self) -> float:
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        return self.rbdalgos.get_total_mass()

src/adam/core/__init__.py

@@ -2,4 +2,5 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-from . import rbd_algorithms, spatial_math, urdf_tree
+from .rbd_algorithms import RBDAlgorithms
+from .spatial_math import SpatialMath