Merge branch 'ami-iit:main' into aba

.github/workflows/black.yml

@@ -1,6 +1,7 @@
 name: Black action
 
 on:
+  pull_request:
   push:
     branches:
       - main
@@ -9,11 +10,11 @@ jobs:
   lint:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
       - name: black
-        uses: lgeiger/black[email protected]
+        uses: psf/black@stable
         with:
-          args: .
+          options: "--check --verbose"
       - name: Check for modified files
         id: git-check
         run: echo ::set-output name=modified::$(if git diff-index --quiet HEAD --; then echo "false"; else echo "true"; fi)

README.md

@@ -1,20 +1,20 @@
-# ADAM
+# adam
 
-[![Adam](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml/badge.svg?branch=main)](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml)
+[![adam](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml/badge.svg?branch=main)](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml)
 [![](https://img.shields.io/badge/license-LGPL-19c2d8.svg)](https://github.com/ami-iit/ADAM/blob/main/LICENSE)
 
 **Automatic Differentiation for rigid-body-dynamics AlgorithMs**
 
-ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _base fixed representations_, also called _left trivialized_ representation (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using:
+**adam** implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _body fixed representations_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using:
 
 - [Jax](https://github.com/google/jax)
 - [CasADi](https://web.casadi.org/)
 - [PyTorch](https://github.com/pytorch/pytorch)
 - [NumPy](https://numpy.org/)
 
-ADAM employs the **automatic differentiation** capabilities of these frameworks to compute, if needed, gradients, Jacobian, Hessians of rigid-body dynamics quantities. This approach enables the design of optimal control and reinforcement learning strategies in robotics.
+**adam** employs the **automatic differentiation** capabilities of these frameworks to compute, if needed, gradients, Jacobian, Hessians of rigid-body dynamics quantities. This approach enables the design of optimal control and reinforcement learning strategies in robotics.
 
-ADAM is based on Roy Featherstone's Rigid Body Dynamics Algorithms.
+**adam** is based on Roy Featherstone's Rigid Body Dynamics Algorithms.
 
 ---
 
@@ -94,8 +94,8 @@ pip install adam-robotics[selected-interface]@git+https://github.com/ami-iit/ADA
 or clone the repo and install:
 
 ```bash
-git clone https://github.com/ami-iit/ADAM.git
-cd ADAM
+git clone https://github.com/ami-iit/adam.git
+cd adam
 pip install .[selected-interface]
 ```
 
@@ -142,13 +142,13 @@ Activate the environment, clone the repo and install the library:
 ```bash
 mamba activate adamenv
 git clone https://github.com/ami-iit/ADAM.git
-cd ADAM
+cd adam
 pip install --no-deps .
 ```
 
 ## 🚀 Usage
 
-The following are small snippets of the use of ADAM. More examples are arriving!
+The following are small snippets of the use of **adam**. More examples are arriving!
 Have also a look at te `tests` folder.
 
 ### Jax interface
@@ -246,7 +246,7 @@ print(M)
 
 ## 🦸‍♂️ Contributing
 
-**ADAM** is an open-source project. Contributions are very welcome!
+**adam** is an open-source project. Contributions are very welcome!
 
 Open an issue with your feature request or if you spot a bug. Then, you can also proceed with a Pull-requests! :rocket:
 

examples/mpc-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/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>"
       ]
     },
     {
@@ -232,7 +232,7 @@
         "opti.minimize(target_cost + velocities_cost)\n",
         "\n",
         "# set the solver\n",
-        "p_opts = {\"expand\": True}\n",
+        "p_opts = {\"expand\": True, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.sb': 'yes'}\n",
         "s_opts = {\"max_iter\": 100, \"print_level\": 0}\n",
         "opti.solver(\"ipopt\", p_opts, s_opts)\n"
       ]
@@ -260,6 +260,8 @@
       },
       "outputs": [],
       "source": [
+        "print(\"Simulation in progress. Wait a bit!\")\n",
+        "\n",
         "# start mujoco simulation along with control\n",
         "duration = 10  # (seconds)\n",
         "framerate = 60  # (Hz)\n",

src/adam/model/std_factories/std_model.py

@@ -1,12 +1,28 @@
 import pathlib
 from typing import List
+import xml.etree.ElementTree as ET
 
 import urdf_parser_py.urdf
 
 from adam.core.spatial_math import SpatialMath
 from adam.model import ModelFactory, StdJoint, StdLink
 
 
+def urdf_remove_sensors_tags(xml_string):
+    # Parse the XML string
+    root = ET.fromstring(xml_string)
+
+    # Find and remove all tags named "sensor" that are child of
+    # root node (i.e. robot)
+    for sensors_tag in root.findall("sensor"):
+        root.remove(sensors_tag)
+
+    # Convert the modified XML back to a string
+    modified_xml_string = ET.tostring(root)
+
+    return modified_xml_string
+
+
 class URDFModelFactory(ModelFactory):
     """This factory generates robot elements from urdf_parser_py
 
@@ -21,7 +37,20 @@ def __init__(self, path: str, math: SpatialMath):
         if not path.exists():
             raise FileExistsError(path)
 
-        self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path)
+        # Read URDF, but before passing it to urdf_parser_py get rid of all sensor tags
+        # sensor tags are valid elements of URDF (see ),
+        # but they are ignored by urdf_parser_py, that complains every time it sees one.
+        # As there is nothing to be fixed in the used models, and it is not useful
+        # 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()
+        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
+        )
         self.name = self.urdf_desc.name
 
     def get_joints(self) -> List[StdJoint]:

src/adam/parametric/pytorch/computations_parametric.py

@@ -23,7 +23,9 @@ def __init__(
         joints_name_list: list,
         links_name_list: list,
         root_link: str = "root_link",
-        gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]),
+        gravity: np.array = torch.tensor(
+            [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64
+        ),
     ) -> None:
         """
         Args:
@@ -119,7 +121,7 @@ def forward_kinematics(
         self,
         frame,
         base_transform: torch.Tensor,
-        s: torch.Tensor,
+        joint_positions: torch.Tensor,
         length_multiplier: torch.Tensor,
         densities: torch.Tensor,
     ) -> torch.Tensor:
@@ -128,7 +130,7 @@ def forward_kinematics(
         Args:
             frame (str): The frame to which the fk will be computed
             base_transform (torch.tensor): The homogenous transform from base to world frame
-            s (torch.tensor): The joints position
+            joint_positions (torch.tensor): The joints position
             length_multiplier (torch.tensor): The length multiplier of the parametrized links
             densities (torch.tensor): The densities of the parametrized links
 
@@ -149,7 +151,9 @@ def forward_kinematics(
         self.NDoF = self.rbdalgos.NDoF
         return (
             self.rbdalgos.forward_kinematics(
-                frame, torch.FloatTensor(base_transform), torch.FloatTensor(s)
+                frame,
+                base_transform,
+                joint_positions,
             )
         ).array
 
@@ -414,7 +418,7 @@ def gravity_term(
             base_positions,
             torch.zeros(6).reshape(6, 1),
             torch.zeros(self.NDoF),
-            torch.FloatTensor(self.g),
+            self.g,
         ).array.squeeze()
 
     def get_total_mass(

src/adam/pytorch/computations.py

@@ -21,7 +21,9 @@ def __init__(
         urdfstring: str,
         joints_name_list: list = None,
         root_link: str = "root_link",
-        gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]),
+        gravity: np.array = torch.tensor(
+            [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64
+        ),
     ) -> None:
         """
         Args:
@@ -92,8 +94,8 @@ def forward_kinematics(
         return (
             self.rbdalgos.forward_kinematics(
                 frame,
-                torch.FloatTensor(base_transform),
-                torch.FloatTensor(joint_position),
+                base_transform,
+                joint_position,
             )
         ).array
 
@@ -240,7 +242,7 @@ def gravity_term(
             joint_positions,
             torch.zeros(6).reshape(6, 1),
             torch.zeros(self.NDoF),
-            torch.FloatTensor(self.g),
+            self.g,
         ).array.squeeze()
 
     def get_total_mass(self) -> float:

src/adam/pytorch/torch_like.py

@@ -7,9 +7,9 @@
 
 import numpy.typing as ntp
 import torch
+import numpy as np
 
 from adam.core.spatial_math import ArrayLike, ArrayLikeFactory, SpatialMath
-from adam.numpy import NumpyLike
 
 
 @dataclass
@@ -18,12 +18,17 @@ class TorchLike(ArrayLike):
 
     array: torch.Tensor
 
+    def __post_init__(self):
+        """Converts array to double precision"""
+        if self.array.dtype != torch.float64:
+            self.array = self.array.double()
+
     def __setitem__(self, idx, value: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
         """Overrides set item operator"""
         if type(self) is type(value):
             self.array[idx] = value.array.reshape(self.array[idx].shape)
         else:
-            self.array[idx] = torch.tensor(value)
+            self.array[idx] = torch.tensor(value) if isinstance(value, float) else value
 
     def __getitem__(self, idx):
         """Overrides get item operator"""
@@ -42,24 +47,29 @@ def T(self) -> "TorchLike":
         Returns:
             TorchLike: transpose of array
         """
-        if len(self.array.shape) != 1:
-            return TorchLike(self.array.mT)
+        # check if self.array is a 0-D tensor
+
+        if len(self.array.shape) == 0:
+            return TorchLike(self.array)
         x = self.array
         return TorchLike(x.permute(*torch.arange(x.ndim - 1, -1, -1)))
 
     def __matmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
         """Overrides @ operator"""
+
         if type(self) is type(other):
-            return TorchLike(self.array @ other.array.float())
+            return TorchLike(self.array @ other.array)
+        if isinstance(other, torch.Tensor):
+            return TorchLike(self.array @ other)
         else:
-            return TorchLike(self.array @ torch.tensor(other).float())
+            return TorchLike(self.array @ torch.tensor(other))
 
     def __rmatmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
         """Overrides @ operator"""
         if type(self) is type(other):
             return TorchLike(other.array @ self.array)
         else:
-            return TorchLike(torch.tensor(other).float() @ self.array)
+            return TorchLike(torch.tensor(other) @ self.array)
 
     def __mul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
         """Overrides * operator"""
@@ -127,7 +137,7 @@ def zeros(*x: int) -> "TorchLike":
         Returns:
             TorchLike: zero matrix of dimension *x
         """
-        return TorchLike(torch.zeros(x).float())
+        return TorchLike(torch.zeros(x))
 
     @staticmethod
     def eye(x: int) -> "TorchLike":
@@ -138,15 +148,15 @@ def eye(x: int) -> "TorchLike":
         Returns:
             TorchLike: identity matrix of dimension x
         """
-        return TorchLike(torch.eye(x).float())
+        return TorchLike(torch.eye(x))
 
     @staticmethod
     def array(x: ntp.ArrayLike) -> "TorchLike":
         """
         Returns:
             TorchLike: vector wrapping x
         """
-        return TorchLike(torch.FloatTensor(x))
+        return TorchLike(torch.tensor(x))
 
 
 class SpatialMath(SpatialMath):
@@ -162,7 +172,8 @@ def sin(x: ntp.ArrayLike) -> "TorchLike":
         Returns:
             TorchLike: sin value of x
         """
-        x = torch.tensor(x)
+        if isinstance(x, float):
+            x = torch.tensor(x)
         return TorchLike(torch.sin(x))
 
     @staticmethod
@@ -174,7 +185,9 @@ def cos(x: ntp.ArrayLike) -> "TorchLike":
         Returns:
             TorchLike: cos value of x
         """
-        x = torch.tensor(x)
+        # transform to torch tensor, if not already
+        if isinstance(x, float):
+            x = torch.tensor(x)
         return TorchLike(torch.cos(x))
 
     @staticmethod
@@ -200,13 +213,11 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike":
         """
         if not isinstance(x, TorchLike):
             return TorchLike(
-                torch.FloatTensor(
-                    [[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]
-                )
+                torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
             )
         x = x.array
         return TorchLike(
-            torch.FloatTensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
+            torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
         )
 
     @staticmethod
@@ -232,7 +243,7 @@ def vertcat(*x: ntp.ArrayLike) -> "TorchLike":
         if isinstance(x[0], TorchLike):
             v = torch.vstack([x[i].array for i in range(len(x))])
         else:
-            v = torch.FloatTensor(x)
+            v = torch.tensor(x)
         return TorchLike(v)
 
     @staticmethod
@@ -244,7 +255,7 @@ def horzcat(*x: ntp.ArrayLike) -> "TorchLike":
         if isinstance(x[0], TorchLike):
             v = torch.hstack([x[i].array for i in range(len(x))])
         else:
-            v = torch.FloatTensor(x)
+            v = torch.tensor(x)
         return TorchLike(v)
 
     @staticmethod