Add parametrization w.r.t Hardware Parameters

ci_env.yml

@@ -16,3 +16,4 @@ dependencies:
   - pytest-repeat
   - icub-models
   - idyntree
+  - gitpython 

setup.cfg

@@ -53,6 +53,7 @@ test =
         idyntree
         icub-models
         black
+        gitpython
 all =
         jax
         jaxlib

src/adam/casadi/casadi_like.py

@@ -21,9 +21,9 @@ class CasadiLike(ArrayLike):
     def __matmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """Overrides @ operator"""
         if type(other) in [CasadiLike, NumpyLike]:
-            return CasadiLike(self.array @ other.array)
+            return CasadiLike(cs.mtimes(self.array, other.array))
         else:
-            return CasadiLike(self.array @ other)
+            return CasadiLike(cs.mtimes(self.array, other))
 
     def __rmatmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike":
         """Overrides @ operator"""
@@ -196,6 +196,19 @@ def vertcat(*x) -> "CasadiLike":
         y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
         return CasadiLike(cs.vertcat(*y))
 
+    @staticmethod
+    def horzcat(*x) -> "CasadiLike":
+        """
+        Returns:
+            CasadiLike:  horizontal 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.horzcat(*y))
+
 
 if __name__ == "__main__":
     math = SpatialMath()

src/adam/core/spatial_math.py

@@ -117,6 +117,21 @@ def vertcat(x: npt.ArrayLike) -> npt.ArrayLike:
         """
         pass
 
+    @abc.abstractmethod
+    def horzcat(x: npt.ArrayLike) -> npt.ArrayLike:
+        """
+        Args:
+            x (npt.ArrayLike): elements
+
+        Returns:
+            npt.ArrayLike: horizontal concatenation of elements x
+        """
+        pass
+
+    @abc.abstractmethod
+    def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> npt.ArrayLike:
+        pass
+
     @abc.abstractmethod
     def sin(x: npt.ArrayLike) -> npt.ArrayLike:
         """
@@ -227,7 +242,9 @@ def H_revolute_joint(
         T = self.factory.eye(4)
         R = self.R_from_RPY(rpy) @ self.R_from_axis_angle(axis, q)
         T[:3, :3] = R
-        T[:3, 3] = xyz
+        T[0, 3] = xyz[0]
+        T[1, 3] = xyz[1]
+        T[2, 3] = xyz[2]
         return T
 
     def H_prismatic_joint(
@@ -264,7 +281,9 @@ def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLik
         """
         T = self.factory.eye(4)
         T[:3, :3] = self.R_from_RPY(rpy)
-        T[:3, 3] = xyz
+        T[0, 3] = xyz[0]
+        T[1, 3] = xyz[1]
+        T[2, 3] = xyz[2]
         return T
 
     def R_from_RPY(self, rpy: npt.ArrayLike) -> npt.ArrayLike:
@@ -381,6 +400,33 @@ def spatial_inertia(
         IO[:3, :3] = self.factory.eye(3) * mass
         return IO
 
+    def spatial_inertial_with_parameter(self, I, mass, c, rpy):
+        """
+        Args:
+            I (npt.ArrayLike): inertia values parametric
+            mass (npt.ArrayLike): mass value parametric
+            c (npt.ArrayLike): origin of the link parametric
+            rpy (npt.ArrayLike): orientation of the link from urdf
+
+        Returns:
+            npt.ArrayLike: the 6x6 inertia matrix parametric expressed at the origin of the link (with rotation)
+        """
+        IO = self.factory.zeros(6, 6)
+        Sc = self.skew(c)
+        R = self.factory.zeros(3, 3)
+        R_temp = self.R_from_RPY(rpy)
+        inertia_matrix = self.vertcat(
+            self.horzcat(I.ixx, I.ixy, I.ixz),
+            self.horzcat(I.iyx, I.iyy, I.iyz),
+            self.horzcat(I.ixz, I.iyz, I.izz),
+        )
+
+        IO[3:, 3:] = R_temp @ inertia_matrix @ R_temp.T + mass * Sc @ Sc.T
+        IO[3:, :3] = mass * Sc
+        IO[:3, 3:] = mass * Sc.T
+        IO[:3, :3] = self.factory.eye(3) * mass
+        return IO
+
     def spatial_skew(self, v: npt.ArrayLike) -> npt.ArrayLike:
         """
         Args:

src/adam/jax/jax_like.py

@@ -195,7 +195,19 @@ def vertcat(*x) -> "JaxLike":
             JaxLike: Vertical concatenation of elements
         """
         if isinstance(x[0], JaxLike):
-            v = jnp.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1)
+            v = jnp.vstack([x[i].array for i in range(len(x))])
         else:
-            v = jnp.vstack([x[i] for i in range(len(x))]).reshape(-1, 1)
+            v = jnp.vstack([x[i] for i in range(len(x))])
+        return JaxLike(v)
+
+    @staticmethod
+    def horzcat(*x) -> "JaxLike":
+        """
+        Returns:
+            JaxLike: Horizontal concatenation of elements
+        """
+        if isinstance(x[0], JaxLike):
+            v = jnp.hstack([x[i].array for i in range(len(x))])
+        else:
+            v = jnp.hstack([x[i] for i in range(len(x))])
         return JaxLike(v)

src/adam/numpy/numpy_like.py

@@ -183,9 +183,21 @@ def vertcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
             NumpyLike: vertical concatenation of x
         """
         if isinstance(x[0], NumpyLike):
-            v = np.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1)
+            v = np.vstack([x[i].array for i in range(len(x))])
         else:
-            v = np.vstack([x[i] for i in range(len(x))]).reshape(-1, 1)
+            v = np.vstack([x[i] for i in range(len(x))])
+        return NumpyLike(v)
+
+    @staticmethod
+    def horzcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike":
+        """
+        Returns:
+            NumpyLike: horrizontal concatenation of x
+        """
+        if isinstance(x[0], NumpyLike):
+            v = np.hstack([x[i].array for i in range(len(x))])
+        else:
+            v = np.hstack([x[i] for i in range(len(x))])
         return NumpyLike(v)
 
     @staticmethod

src/adam/parametric/__init__.py

@@ -0,0 +1,3 @@
+# 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.

src/adam/parametric/casadi/__init__.py

@@ -0,0 +1,4 @@
+# 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 .computations_parametric import KinDynComputationsParametric