Merge branch 'main' into aba

.github/workflows/conda-forge-tests.yml

@@ -25,11 +25,20 @@ jobs:
       shell: bash -l {0}
       id: week
       run: echo "week=$(date +%Y-%U)" >> "${GITHUB_OUTPUT}"
+
     - uses: mamba-org/setup-micromamba@v1
+      if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu')
       with:
         environment-file: ci_env.yml
         cache-environment-key: environment-${{ steps.week.outputs.week }}-${{ matrix.os }}
+
+    - uses: mamba-org/setup-micromamba@v1
+      if: contains(matrix.os, 'windows')
+      with:
+        environment-file: ci_env_win.yml
+        cache-environment-key: environment-${{ steps.week.outputs.week }}-${{ matrix.os }}
 
+
     - name: Print packages and environment
       shell: bash -l {0}
       run: |
@@ -46,8 +55,6 @@ jobs:
       shell: bash -l {0}
       if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu')
       run: |
-          # Add additional dependencies not available on Windows
-          micromamba install jax pytorch
           pytest
 
     - name: Test with pytest [Windows]

ci_env.yml

@@ -16,3 +16,6 @@ dependencies:
   - pytest-repeat
   - icub-models
   - idyntree
+  - gitpython 
+  - jax
+  - pytorch

ci_env_win.yml

@@ -0,0 +1,19 @@
+name: adamdev
+channels:
+  - conda-forge
+dependencies:
+  - python >=3.7
+  - numpy
+  - scipy
+  - casadi
+  - prettytable
+  - urdfdom-py
+  - pip
+  - wheel
+  - setuptools
+  - setuptools_scm
+  - pytest
+  - 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"""
@@ -203,6 +203,15 @@ def vertcat(*x) -> "CasadiLike":
         return CasadiLike(cs.vertcat(*y))
 
     @staticmethod
+    def horzcat(*x) -> "CasadiLike":
+        """
+        Returns:
+            CasadiLike:  horizontal concatenation of elements
+        """
+
+        y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x]
+        return CasadiLike(cs.horzcat(*y))
+
     def solve(A: "CasadiLike", b: "CasadiLike") -> "CasadiLike":
         """
         Args:
@@ -215,6 +224,7 @@ def solve(A: "CasadiLike", b: "CasadiLike") -> "CasadiLike":
         return CasadiLike(cs.solve(A.array, b.array))
 
 
+
 if __name__ == "__main__":
     math = SpatialMath()
     print(math.eye(3))

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:
         """
@@ -239,7 +254,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(
@@ -276,7 +293,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:
@@ -393,6 +412,33 @@ def spatial_inertia(
         IO[:3, :3] = self.factory.eye(3) * mass
         return IO
 
+    def spatial_inertial_with_parameters(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

@@ -201,9 +201,21 @@ 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)
 
     @staticmethod

src/adam/numpy/numpy_like.py

@@ -190,9 +190,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