ami-iit / adam

adam implements a collection of algorithms for calculating rigid-body dynamics in Jax, CasADi, PyTorch, and Numpy.
https://adam-docs.readthedocs.io/en/latest/
BSD 3-Clause "New" or "Revised" License
131 stars 20 forks source link

Failed to load stickBot model in the parametric KinDynComputations. #71

Closed S-Dafarra closed 7 months ago

S-Dafarra commented 8 months ago

If I try to load the model https://github.com/icub-tech-iit/ergocub-gazebo-simulations/blob/1179630a88541479df51ebb108a21865ea251302/models/stickBot/model.urdf in KinDynComputationsParametric I have the following issue:

Traceback (most recent call last):
  File "C:\Software\hippopt\src\hippopt\turnkey_planners\humanoid_kinodynamic\main_periodic_step_parametric.py", line 377, in <module>
    planner = walking_planner.Planner(settings=planner_settings)
              ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "C:\Software\hippopt\src\hippopt\turnkey_planners\humanoid_kinodynamic\planner.py", line 27, in __init__
    self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric(
                          ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "C:\Software\adam\src\adam\parametric\casadi\computations_parametric.py", line 40, in __init__
    factory = URDFParametricModelFactory(
              ^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "C:\Software\adam\src\adam\parametric\model\parametric_factories\parametric_model.py", line 32, in __init__
    self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path)
                     ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "C:\Software\mambaforge\envs\hippopt\Lib\site-packages\urdf_parser_py\xml_reflection\core.py", line 617, in from_xml_file
    return cls.from_xml_string(xml_string)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "C:\Software\mambaforge\envs\hippopt\Lib\site-packages\urdf_parser_py\xml_reflection\core.py", line 610, in from_xml_string
    node = etree.fromstring(xml_string)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "src/lxml/etree.pyx", line 3264, in lxml.etree.fromstring
  File "src/lxml/parser.pxi", line 1984, in lxml.etree._parseMemoryDocument
ValueError: Unicode strings with encoding declaration are not supported. Please use bytes input or XML fragments without declaration.

The normal KinDynComputations instead works fine.

cc @CarlottaSartore

CarlottaSartore commented 8 months ago

Probably related to https://github.com/ami-iit/adam/pull/49#issuecomment-1804137575

CarlottaSartore commented 8 months ago

This should work


 ## 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")
traversaro commented 8 months ago

@S-Dafarra how did you installed adam ?

S-Dafarra commented 8 months ago

@S-Dafarra how did you installed adam ?

conda

now I switched to source installation. I noticed that in the standard case, there is this snippet https://github.com/ami-iit/adam/blob/2f18e9e737beeb00a4687a37d10a4822d0e5959b/src/adam/model/std_factories/std_model.py#L11-L23

I have added it with the following diff

index 1a46094..cbcb3ed 100644
--- a/src/adam/parametric/model/parametric_factories/parametric_model.py
+++ b/src/adam/parametric/model/parametric_factories/parametric_model.py
@@ -2,9 +2,9 @@ import pathlib
 from typing import List

 import urdf_parser_py.urdf
-
 from adam.core.spatial_math import SpatialMath
 from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint
+from adam.model.std_factories.std_model import urdf_remove_sensors_tags
 from adam.parametric.model import ParmetricJoint, ParametricLink

@@ -29,7 +29,22 @@ class URDFParametricModelFactory(ModelFactory):
         if not path.exists():
             raise FileExistsError(path)
         self.links_name_list = links_name_list
-        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
         self.length_multiplier = length_multiplier
         self.densities = densities

and now the model is loaded correctly

flferretti commented 8 months ago

Have you tried using:

xml_file = open(path, "r", encoding="utf-8")

in https://github.com/ami-iit/adam/blob/2f18e9e737beeb00a4687a37d10a4822d0e5959b/src/adam/model/std_factories/std_model.py#L47-L47

Because using the system default implicitly can create problems on Windows.

CarlottaSartore commented 8 months ago

mmm, I think the error is "hidden" from removing the sensor tag, but it is resolved because now urf_parser_py is loading a file without the declaration.

Which version of urdf_parser_py you are using? Since it should be fixed https://github.com/ros/urdf_parser_py/pull/83, maybe I am biased and the issue is another one.

S-Dafarra commented 8 months ago

@S-Dafarra how did you installed adam ?

For reference, this is my current

mamba list ``` (hippopt) C:\Software\hippopt>mamba list # packages in environment at C:\Software\mambaforge\envs\hippopt: # # Name Version Build Channel adam-robotics 0.1.1.dev4+dirty pypi_0 pypi aom 3.8.1 h63175ca_0 conda-forge assimp 5.3.1 h81f0834_2 conda-forge asttokens 2.4.1 pyhd8ed1ab_0 conda-forge black 24.2.0 py311h1ea47a8_0 conda-forge blis 0.9.0 hcfcfb64_1 conda-forge brotli 1.1.0 hcfcfb64_1 conda-forge brotli-bin 1.1.0 hcfcfb64_1 conda-forge bzip2 1.0.8 hcfcfb64_5 conda-forge ca-certificates 2024.2.2 h56e8100_0 conda-forge cairo 1.18.0 h1fef639_0 conda-forge casadi 3.6.3 py311h4930016_5 conda-forge certifi 2024.2.2 pyhd8ed1ab_0 conda-forge click 8.1.7 win_pyh7428d3b_0 conda-forge cmake 3.28.3 hf0feee3_0 conda-forge colorama 0.4.6 pyhd8ed1ab_0 conda-forge contourpy 1.2.0 py311h005e61a_0 conda-forge cycler 0.12.1 pyhd8ed1ab_0 conda-forge dav1d 1.2.1 hcfcfb64_0 conda-forge decorator 5.1.1 pyhd8ed1ab_0 conda-forge eigen 3.4.0 h91493d7_0 conda-forge exceptiongroup 1.2.0 pyhd8ed1ab_2 conda-forge executing 2.0.1 pyhd8ed1ab_0 conda-forge expat 2.5.0 h63175ca_1 conda-forge ffmpeg 6.1.1 gpl_hb766fab_104 conda-forge ffmpeg-python 0.2.0 py_0 conda-forge font-ttf-dejavu-sans-mono 2.37 hab24e00_0 conda-forge font-ttf-inconsolata 3.000 h77eed37_0 conda-forge font-ttf-source-code-pro 2.038 h77eed37_0 conda-forge font-ttf-ubuntu 0.83 h77eed37_1 conda-forge fontconfig 2.14.2 hbde0cde_0 conda-forge fonts-conda-ecosystem 1 0 conda-forge fonts-conda-forge 1 0 conda-forge fonttools 4.49.0 py311ha68e1ae_0 conda-forge freetype 2.12.1 hdaf720e_2 conda-forge future 1.0.0 pyhd8ed1ab_0 conda-forge gettext 0.21.1 h5728263_0 conda-forge git 2.44.0 h57928b3_0 conda-forge git-filter-repo 2.38.0 pyhd8ed1ab_0 conda-forge glfw 3.4 hcfcfb64_0 conda-forge glib 2.78.4 h12be248_0 conda-forge glib-tools 2.78.4 h12be248_0 conda-forge graphite2 1.3.13 1000 conda-forge gst-plugins-base 1.22.9 h001b923_0 conda-forge gstreamer 1.22.9 hb4038d2_0 conda-forge harfbuzz 8.3.0 h7ab893a_0 conda-forge hippopt 0.1.dev373+dirty pypi_0 pypi icu 73.2 h63175ca_0 conda-forge idyntree 10.3.0 py311heed4631_0 conda-forge iniconfig 2.0.0 pyhd8ed1ab_0 conda-forge intel-openmp 2024.0.0 h57928b3_49841 conda-forge ipopt 3.14.14 h1709daf_1 conda-forge ipython 8.22.1 pyh7428d3b_0 conda-forge irrlicht 1.8.5 h65f4d7e_4 conda-forge isort 5.13.2 pyhd8ed1ab_0 conda-forge jedi 0.19.1 pyhd8ed1ab_0 conda-forge kiwisolver 1.4.5 py311h005e61a_1 conda-forge krb5 1.21.2 heb0366b_0 conda-forge lcms2 2.16 h67d730c_0 conda-forge lerc 4.0.0 h63175ca_0 conda-forge libblas 3.9.0 21_win64_blis conda-forge libboost 1.82.0 h65993cd_6 conda-forge libbrotlicommon 1.1.0 hcfcfb64_1 conda-forge libbrotlidec 1.1.0 hcfcfb64_1 conda-forge libbrotlienc 1.1.0 hcfcfb64_1 conda-forge libcblas 3.9.0 21_win64_blis conda-forge libclang 15.0.7 default_hde6756a_4 conda-forge libclang13 15.0.7 default_h85b4d89_4 conda-forge libcurl 8.5.0 hd5e4a3a_0 conda-forge libdeflate 1.19 hcfcfb64_0 conda-forge libexpat 2.5.0 h63175ca_1 conda-forge libffi 3.4.2 h8ffe710_5 conda-forge libflang 5.0.0 h6538335_20180525 conda-forge libglib 2.78.4 h16e383f_0 conda-forge libhwloc 2.9.3 default_haede6df_1009 conda-forge libiconv 1.17 hcfcfb64_2 conda-forge libjpeg-turbo 3.0.0 hcfcfb64_1 conda-forge liblapack 3.9.0 5_hd5c7e75_netlib conda-forge libogg 1.3.4 h8ffe710_1 conda-forge libopus 1.3.1 h8ffe710_1 conda-forge libosqp 0.6.3 h63175ca_0 conda-forge libpng 1.6.43 h19919ed_0 conda-forge libqdldl 0.1.5 h63175ca_1 conda-forge libsodium 1.0.18 h8d14728_1 conda-forge libsqlite 3.45.1 hcfcfb64_0 conda-forge libssh2 1.11.0 h7dfc565_0 conda-forge libtiff 4.6.0 h6e2ebb7_2 conda-forge libuv 1.47.0 hcfcfb64_0 conda-forge libvorbis 1.3.7 h0e60522_0 conda-forge libwebp-base 1.3.2 hcfcfb64_0 conda-forge libxcb 1.15 hcd874cb_0 conda-forge libxml2 2.12.5 hc3477c8_0 conda-forge libxslt 1.1.39 h3df6e99_0 conda-forge libzlib 1.2.13 hcfcfb64_5 conda-forge liecasadi 0.0.6 pyhd8ed1ab_0 conda-forge llvm-meta 5.0.0 0 conda-forge lxml 5.1.0 py311h064e5ff_0 conda-forge m2w64-gcc-libgfortran 5.3.0 6 conda-forge m2w64-gcc-libs 5.3.0 7 conda-forge m2w64-gcc-libs-core 5.3.0 7 conda-forge m2w64-gmp 6.1.0 2 conda-forge m2w64-libwinpthread-git 5.0.0.4634.697f757 2 conda-forge matplotlib 3.8.3 py311h1ea47a8_0 conda-forge matplotlib-base 3.8.3 py311h6e989c2_0 conda-forge matplotlib-inline 0.1.6 pyhd8ed1ab_0 conda-forge meshcat-python 0.3.2 pyhd8ed1ab_0 conda-forge meson 1.3.2 pyhd8ed1ab_0 conda-forge metis 5.1.0 h63175ca_1007 conda-forge mkl 2024.0.0 h66d3029_49658 conda-forge msys2-conda-epoch 20160418 1 conda-forge mumps-seq 5.6.2 h1f49738_4 conda-forge munkres 1.1.4 pyh9f0ad1d_0 conda-forge mypy_extensions 1.0.0 pyha770c72_0 conda-forge ninja 1.11.1 h91493d7_0 conda-forge numpy 1.26.4 py311h0b4df5a_0 conda-forge openh264 2.4.1 h63175ca_0 conda-forge openjpeg 2.5.0 h3d672ee_3 conda-forge openmp 5.0.0 vc14_1 conda-forge openssl 3.2.1 hcfcfb64_0 conda-forge osqp-eigen 0.8.1 h6d7489e_0 conda-forge packaging 23.2 pyhd8ed1ab_0 conda-forge parso 0.8.3 pyhd8ed1ab_0 conda-forge pathspec 0.12.1 pyhd8ed1ab_0 conda-forge pcre2 10.42 h17e33f8_0 conda-forge pickleshare 0.7.5 py_1003 conda-forge pillow 10.2.0 py311h4dd8a23_0 conda-forge pip 24.0 pyhd8ed1ab_0 conda-forge pixman 0.43.2 h63175ca_0 conda-forge platformdirs 4.2.0 pyhd8ed1ab_0 conda-forge pluggy 1.4.0 pyhd8ed1ab_0 conda-forge ply 3.11 py_1 conda-forge prettytable 3.10.0 pyhd8ed1ab_0 conda-forge prompt-toolkit 3.0.42 pyha770c72_0 conda-forge proxsuite 0.6.3 py311h1ea47a8_0 conda-forge pthread-stubs 0.4 hcd874cb_1001 conda-forge pthreads-win32 2.9.1 hfa6e2cd_3 conda-forge pure_eval 0.2.2 pyhd8ed1ab_0 conda-forge pygments 2.17.2 pyhd8ed1ab_0 conda-forge pyngrok 7.1.2 pyhd8ed1ab_0 conda-forge pyparsing 3.1.1 pyhd8ed1ab_0 conda-forge pyqt 5.15.9 py311h125bc19_5 conda-forge pyqt5-sip 12.12.2 py311h12c1d0e_5 conda-forge pytest 8.0.2 pyhd8ed1ab_0 conda-forge python 3.11.8 h2628c8c_0_cpython conda-forge python-dateutil 2.8.2 pyhd8ed1ab_0 conda-forge python_abi 3.11 4_cp311 conda-forge pyyaml 6.0.1 py311ha68e1ae_1 conda-forge pyzmq 25.1.2 py311h9250fbb_0 conda-forge qt-main 5.15.8 h9e85ed6_19 conda-forge resolve-robotics-uri-py 0.1.0 pyhd8ed1ab_0 conda-forge scipy 1.12.0 py311h0b4df5a_2 conda-forge sdl 1.2.68 h21dd15a_0 conda-forge sdl2 2.28.5 h63175ca_1 conda-forge setuptools 69.1.1 pyhd8ed1ab_0 conda-forge simde 0.7.6 h91493d7_0 conda-forge sip 6.7.12 py311h12c1d0e_0 conda-forge six 1.16.0 pyh6c4a22f_0 conda-forge stack_data 0.6.2 pyhd8ed1ab_0 conda-forge svt-av1 1.8.0 h63175ca_0 conda-forge tbb 2021.11.0 h91493d7_1 conda-forge tinyxml2 10.0.0 h63175ca_0 conda-forge tk 8.6.13 h5226925_1 conda-forge toml 0.10.2 pyhd8ed1ab_0 conda-forge tomli 2.0.1 pyhd8ed1ab_0 conda-forge tornado 6.4 py311ha68e1ae_0 conda-forge traitlets 5.14.1 pyhd8ed1ab_0 conda-forge typing_extensions 4.10.0 pyha770c72_0 conda-forge tzdata 2024a h0c530f3_0 conda-forge u-msgpack-python 2.8.0 pyhd8ed1ab_0 conda-forge ucrt 10.0.22621.0 h57928b3_0 conda-forge urdfdom-py 1.2.1 py311h1ea47a8_3 conda-forge vc 14.3 hcf57466_18 conda-forge vc14_runtime 14.38.33130 h82b7239_18 conda-forge vs2015_runtime 14.38.33130 hcb4865c_18 conda-forge wcwidth 0.2.13 pyhd8ed1ab_0 conda-forge wheel 0.42.0 pyhd8ed1ab_0 conda-forge x264 1!164.3095 h8ffe710_2 conda-forge x265 3.5 h2d74725_3 conda-forge xorg-libxau 1.0.11 hcd874cb_0 conda-forge xorg-libxdmcp 1.1.3 hcd874cb_0 conda-forge xz 5.2.6 h8d14728_0 conda-forge yaml 0.2.5 h8ffe710_2 conda-forge zeromq 4.3.5 h63175ca_0 conda-forge zlib 1.2.13 hcfcfb64_5 conda-forge zstd 1.5.5 h12be248_0 conda-forge ```
S-Dafarra commented 8 months ago

Relevant PR: https://github.com/ami-iit/adam/pull/72

traversaro commented 8 months ago

Since it should be fixed ros/urdf_parser_py#83, maybe I am biased and the issue is another one.

Actually the issue is still there as the fix still needs to be released.