Merge pull request #100 from flferretti/flferretti-patch-1

Update URDF parsing logic and improve R/W operations on files
ami-iit · Oct 25, 2024 · f6dfb35 · f6dfb35
2 parents 3468054 + 79f6d67
commit f6dfb35
Show file tree

Hide file tree

Showing 6 changed files with 101 additions and 85 deletions.
diff --git a/examples/mpc-ik.ipynb b/examples/mpc-ik.ipynb
@@ -3,24 +3,24 @@
     {
       "cell_type": "markdown",
       "metadata": {
-        "id": "view-in-github",
-        "colab_type": "text"
+        "colab_type": "text",
+        "id": "view-in-github"
       },
       "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>"
       ]
     },
     {
       "cell_type": "markdown",
+      "metadata": {
+        "id": "MmkmgG_gGWVi"
+      },
       "source": [
         "## Install Mujoco, adam, and mediapy.\n",
         "\n",
         "Download also mujoco-menagerie for the panda model and the urdf needed from adam.\n",
         "Set some stuff for the visualization."
-      ],
-      "metadata": {
-        "id": "MmkmgG_gGWVi"
-      }
+      ]
     },
     {
       "cell_type": "code",
@@ -47,12 +47,12 @@
     },
     {
       "cell_type": "markdown",
-      "source": [
-        "## Import packages"
-      ],
       "metadata": {
         "id": "Dz4cHPy2Gtmq"
-      }
+      },
+      "source": [
+        "## Import packages"
+      ]
     },
     {
       "cell_type": "code",
@@ -66,20 +66,17 @@
         "import mediapy as media\n",
         "from adam.casadi import KinDynComputations\n",
         "import numpy as np\n",
-        "import casadi as cs\n",
-        "import distutils.util\n",
-        "import os\n",
-        "import subprocess"
+        "import casadi as cs"
       ]
     },
     {
       "cell_type": "markdown",
-      "source": [
-        "## Import the panda scene in mujoco"
-      ],
       "metadata": {
         "id": "2zw4FO-IGxdR"
-      }
+      },
+      "source": [
+        "## Import the panda scene in mujoco"
+      ]
     },
     {
       "cell_type": "code",
@@ -95,17 +92,17 @@
     },
     {
       "cell_type": "markdown",
+      "metadata": {
+        "id": "CZMO7PsmKUB6"
+      },
       "source": [
         "## Import urdf in adam\n",
         "\n",
         "Set the commanded joint list and impor the urdf in adam.\n",
         "\n",
         "For now I have to use a separate urdf for adam.\n",
         "An importer for a mujoco model could be an idea for the future!"
-      ],
-      "metadata": {
-        "id": "CZMO7PsmKUB6"
-      }
+      ]
     },
     {
       "cell_type": "code",
@@ -122,12 +119,12 @@
     },
     {
       "cell_type": "markdown",
-      "source": [
-        "## A wrapper interface with mujoco"
-      ],
       "metadata": {
         "id": "g5LX5kQAKwaM"
-      }
+      },
+      "source": [
+        "## A wrapper interface with mujoco"
+      ]
     },
     {
       "cell_type": "code",
@@ -173,15 +170,15 @@
     },
     {
       "cell_type": "markdown",
+      "metadata": {
+        "id": "wuY9hqdlD3Vo"
+      },
       "source": [
         "# Model Inverse Kinematics as an MPC\n",
         "\n",
         "An MPC is maybe not the best way to solve an IK problem.\n",
         "I just want to show how to use the casadi interface."
-      ],
-      "metadata": {
-        "id": "wuY9hqdlD3Vo"
-      }
+      ]
     },
     {
       "cell_type": "code",
@@ -239,6 +236,9 @@
     },
     {
       "cell_type": "markdown",
+      "metadata": {
+        "id": "Hf-Uq8PWFy6v"
+      },
       "source": [
         "# Simulation loop\n",
         "\n",
@@ -247,10 +247,7 @@
         "\n",
         "On the notebook it is a bit slow.\n",
         "To run it real time set OMP_NUM_THREADS=1 on your laptop!"
-      ],
-      "metadata": {
-        "id": "Hf-Uq8PWFy6v"
-      }
+      ]
     },
     {
       "cell_type": "code",
@@ -314,6 +311,10 @@
     }
   ],
   "metadata": {
+    "colab": {
+      "include_colab_link": true,
+      "provenance": []
+    },
     "kernelspec": {
       "display_name": "adam_env",
       "language": "python",
@@ -330,10 +331,6 @@
       "nbconvert_exporter": "python",
       "pygments_lexer": "ipython3",
       "version": "3.10.11"
-    },
-    "colab": {
-      "provenance": [],
-      "include_colab_link": true
     }
   },
   "nbformat": 4,

diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py
@@ -23,36 +23,43 @@ def urdf_remove_sensors_tags(xml_string):
     return modified_xml_string
 
 
-def get_xml_string(path: str):
-    isPath = False
+def get_xml_string(path: str | pathlib.Path):
+
+    isPath = isinstance(path, pathlib.Path)
     isUrdf = False
+
+    # Extract the maximum path length for the current OS
+    try:
+        from ctypes.wintypes import MAX_PATH
+    except ValueError:
+        MAX_PATH = os.pathconf("/", "PC_PATH_MAX")
+
     # Checking if it is a path or an urdf
-    if type(path) is not (pathlib.Path):
-        if os.path.exists(path):
+    if not isPath:
+        if len(path) <= MAX_PATH and os.path.exists(path):
             path = pathlib.Path(path)
             isPath = True
         else:
             root = ET.fromstring(path)
-            robot_el = None
+
             for elem in root.iter():
                 if elem.tag == "robot":
                     xml_string = path
                     isUrdf = True
-    elif path.exists():
-        isPath = True
+                    break
+
+    if isPath:
+        if not path.is_file():
+            raise FileExistsError(path)
+
+        with path.open() as xml_file:
+            xml_string = xml_file.read()
 
-    if not (isPath) and not (isUrdf):
+    if not isPath and not isUrdf:
         raise ValueError(
             f"Invalid urdf string: {path}. It is neither a path nor a urdf string"
         )
 
-    if isPath:
-        if not (path.exists()):
-            raise FileExistsError(path)
-        path = pathlib.Path(path)
-        with open(path, "r") as xml_file:
-            xml_string = xml_file.read()
-            xml_file.close()
     return xml_string
 
 

diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py
@@ -22,14 +22,17 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
-## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
-robot_file_read = open(model_path, "r")
-robot_urdf_string = robot_file_read.read()
-robot_urdf_string = robot_urdf_string.replace("<?xml", "")
-robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
-robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
-robot_file_write = open(model_path, "w")
-robot_file_write.write(robot_urdf_string)
+## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+with open(model_path, "r", encoding="utf-8") as robot_file:
+    robot_urdf_string = (
+        robot_file.read()
+        .replace("<?xml", "")
+        .replace("version='1.0'", "")
+        .replace("encoding='UTF-8'?>", "")
+    )
+
+with open(model_path, "w") as robot_file:
+    robot_file.write(robot_urdf_string)
 
 joints_name_list = [
     "torso_pitch",

diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py
@@ -27,14 +27,17 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
-## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
-robot_file_read = open(model_path, "r")
-robot_urdf_string = robot_file_read.read()
-robot_urdf_string = robot_urdf_string.replace("<?xml", "")
-robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
-robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
-robot_file_write = open(model_path, "w")
-robot_file_write.write(robot_urdf_string)
+## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+with open(model_path, "r", encoding="utf-8") as robot_file:
+    robot_urdf_string = (
+        robot_file.read()
+        .replace("<?xml", "")
+        .replace("version='1.0'", "")
+        .replace("encoding='UTF-8'?>", "")
+    )
+
+with open(model_path, "w") as robot_file:
+    robot_file.write(robot_urdf_string)
 
 joints_name_list = [
     "torso_pitch",

diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py
@@ -24,14 +24,17 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
-## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
-robot_file_read = open(model_path, "r")
-robot_urdf_string = robot_file_read.read()
-robot_urdf_string = robot_urdf_string.replace("<?xml", "")
-robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
-robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
-robot_file_write = open(model_path, "w")
-robot_file_write.write(robot_urdf_string)
+## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+with open(model_path, "r", encoding="utf-8") as robot_file:
+    robot_urdf_string = (
+        robot_file.read()
+        .replace("<?xml", "")
+        .replace("version='1.0'", "")
+        .replace("encoding='UTF-8'?>", "")
+    )
+
+with open(model_path, "w") as robot_file:
+    robot_file.write(robot_urdf_string)
 
 joints_name_list = [
     "torso_pitch",

diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py
@@ -27,14 +27,17 @@
 Repo.clone_from(git_url, temp_dir.name)
 model_path = temp_dir.name + "/models/stickBot/model.urdf"
 
-## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
-robot_file_read = open(model_path, "r")
-robot_urdf_string = robot_file_read.read()
-robot_urdf_string = robot_urdf_string.replace("<?xml", "")
-robot_urdf_string = robot_urdf_string.replace("version='1.0'", "")
-robot_urdf_string = robot_urdf_string.replace("encoding='UTF-8'?>", "")
-robot_file_write = open(model_path, "w")
-robot_file_write.write(robot_urdf_string)
+## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49
+with open(model_path, "r", encoding="utf-8") as robot_file:
+    robot_urdf_string = (
+        robot_file.read()
+        .replace("<?xml", "")
+        .replace("version='1.0'", "")
+        .replace("encoding='UTF-8'?>", "")
+    )
+
+with open(model_path, "w") as robot_file:
+    robot_file.write(robot_urdf_string)
 
 joints_name_list = [
     "torso_pitch",