search

robotology / icub-models

Official URDF and SDF models of the iCub humanoid robot.
Creative Commons Attribution Share Alike 4.0 International
33 stars 34 forks source link

Unable to visualize the iCub model with pinocchio and meshcat #97

Closed GiulioRomualdi closed 3 years ago

GiulioRomualdi commented 3 years ago

Hi all, I'm trying to visualize icub (both iCubGazeboV2_5 and iCubGazeboV3) with the Pinocchio wrapper to the meshcat visualizer.

Following the tutorial provided by Pinocchio I was able to spawn Talos

image

However, when I tried with icub, the model is not well-rendered. Indeed for iCubGazeboV2_5 the root link and the chest are squashed together, while for iCubGazeboV3, the head is tilted.

iCubGazeboV2_5 iCubGazeboV3
image image

If you want to replicate the experiment, you can follow the instructions contained in this repository. In particular, you can refer to the latest commit in the main branch (https://github.com/GiulioRomualdi/study-icub-meshcat/tree/7de391589267b78d4390874c213a09800c21cdfe)

Since I'm not an expert user of meshcat/Pinocchio, do you have any idea what should be the problem here?

@traversaro

traversaro commented 3 years ago

Thanks @GiulioRomualdi for the detailed report.

Two comments:

traversaro commented 3 years ago

If you want to replicate the experiment, you can follow the instructions contained in this repository. In particular, you can refer to the latest commit in the main branch (https://github.com/GiulioRomualdi/study-icub-meshcat/tree/7de391589267b78d4390874c213a09800c21cdfe)

I tried, but the output for me is: 

(icub-meshcat) straversaro@IITICUBLAP102:~/study-icub-meshcat$ python visualize-icub.py  -h
usage: visualize-icub.py [-h] [-m MODEL]

Spawn an icub model.

optional arguments:
  -h, --help            show this help message and exit
  -m MODEL, --model MODEL
                        Name of the model. The default value is iCubGazeboV2_5
(icub-meshcat) straversaro@IITICUBLAP102:~/study-icub-meshcat$ python visualize-icub.py
Traceback (most recent call last):
  File "/home/straversaro/study-icub-meshcat/visualize-icub.py", line 26, in <module>
    model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
  File "/home/straversaro/mambaforge/envs/icub-meshcat/lib/python3.9/site-packages/pinocchio/shortcuts.py", line 45, in buildModelsFromUrdf
    geom_model = pin.buildGeomFromUrdf(model, filename, geometry_type, package_dirs)
ValueError: Mesh package://iCub/meshes/simmechanics/sim_sea_2-5_root_link_prt-binary.stl could not be found.
traversaro commented 3 years ago

For reference, my packages are:

(icub-meshcat) straversaro@IITICUBLAP102:~/pinocchio/examples$ conda list
# packages in environment at /home/straversaro/mambaforge/envs/icub-meshcat:
#
# Name                    Version                   Build  Channel
_libgcc_mutex             0.1                 conda_forge    conda-forge
_openmp_mutex             4.5                      1_llvm    conda-forge
assimp                    5.0.1                hedfc422_5    conda-forge
backcall                  0.2.0              pyh9f0ad1d_0    conda-forge
backports                 1.0                        py_2    conda-forge
backports.functools_lru_cache 1.6.4              pyhd8ed1ab_0    conda-forge
boost                     1.74.0           py39h5472131_3    conda-forge
boost-cpp                 1.74.0               h312852a_4    conda-forge
bzip2                     1.0.8                h7f98852_4    conda-forge
ca-certificates           2021.5.30            ha878542_0    conda-forge
certifi                   2021.5.30        py39hf3d152e_0    conda-forge
console_bridge            1.0.1                h4bd325d_0    conda-forge
decorator                 5.0.9              pyhd8ed1ab_0    conda-forge
eigen                     3.3.9                h4bd325d_1    conda-forge
eigenpy                   2.6.4            py39h5aed9d1_0    conda-forge
hpp-fcl                   1.7.4            py39hbcdfc36_0    conda-forge
icu                       68.1                 h58526e2_0    conda-forge
ipython                   7.25.0           py39hef51801_1    conda-forge
ipython_genutils          0.2.0                      py_1    conda-forge
jedi                      0.18.0           py39hf3d152e_2    conda-forge
ld_impl_linux-64          2.36.1               hea4e1c9_1    conda-forge
libblas                   3.9.0                9_openblas    conda-forge
libcblas                  3.9.0                9_openblas    conda-forge
libffi                    3.3                  h58526e2_2    conda-forge
libgcc-ng                 9.3.0               h2828fa1_19    conda-forge
libgfortran-ng            9.3.0               hff62375_19    conda-forge
libgfortran5              9.3.0               hff62375_19    conda-forge
liblapack                 3.9.0                9_openblas    conda-forge
libopenblas               0.3.15          pthreads_h8fe5266_1    conda-forge
libsodium                 1.0.18               h36c2ea0_1    conda-forge
libstdcxx-ng              9.3.0               h6de172a_19    conda-forge
llvm-openmp               12.0.1               h4bd325d_0    conda-forge
lz4-c                     1.9.3                h9c3ff4c_0    conda-forge
matplotlib-inline         0.1.2              pyhd8ed1ab_2    conda-forge
meshcat-python            0.1.1              pyhd8ed1ab_0    conda-forge
ncurses                   6.2                  h58526e2_4    conda-forge
numpy                     1.21.0           py39hdbf815f_0    conda-forge
octomap                   1.9.7                h4bd325d_0    conda-forge
openssl                   1.1.1k               h7f98852_0    conda-forge
parso                     0.8.2              pyhd8ed1ab_0    conda-forge
pexpect                   4.8.0              pyh9f0ad1d_2    conda-forge
pickleshare               0.7.5                   py_1003    conda-forge
pinocchio                 2.6.2            py39hbcdfc36_0    conda-forge
pip                       21.1.3             pyhd8ed1ab_0    conda-forge
prompt-toolkit            3.0.19             pyha770c72_0    conda-forge
ptyprocess                0.7.0              pyhd3deb0d_0    conda-forge
pygments                  2.9.0              pyhd8ed1ab_0    conda-forge
pyngrok                   5.0.5              pyhd8ed1ab_0    conda-forge
python                    3.9.6           h49503c6_1_cpython    conda-forge
python_abi                3.9                      2_cp39    conda-forge
pyyaml                    5.4.1            py39h3811e60_0    conda-forge
pyzmq                     22.1.0           py39h37b5a0c_0    conda-forge
readline                  8.1                  h46c0cb4_0    conda-forge
setuptools                49.6.0           py39hf3d152e_3    conda-forge
sqlite                    3.36.0               h9cd32fc_0    conda-forge
tinyxml                   2.6.2                h4bd325d_2    conda-forge
tk                        8.6.10               h21135ba_1    conda-forge
tornado                   6.1              py39h3811e60_1    conda-forge
traitlets                 5.0.5                      py_0    conda-forge
tzdata                    2021a                he74cb21_1    conda-forge
u-msgpack-python          2.7.1              pyh9f0ad1d_0    conda-forge
urdfdom                   2.3.5                h4bd325d_0    conda-forge
urdfdom_headers           1.0.5                h4bd325d_2    conda-forge
wcwidth                   0.2.5              pyh9f0ad1d_2    conda-forge
wheel                     0.36.2             pyhd3deb0d_0    conda-forge
xz                        5.2.5                h516909a_1    conda-forge
yaml                      0.2.5                h516909a_0    conda-forge
zeromq                    4.3.4                h9c3ff4c_0    conda-forge
zlib                      1.2.11            h516909a_1010    conda-forge
zstd                      1.5.0                ha95c52a_0    conda-forge
GiulioRomualdi commented 3 years ago

Hi @traversaro, I was running the application in an environment where ROS_PACKAGE_PATH was set to the superbuild folder. I updated the script and the installation procedure. Now also icub-models is installed with conda https://github.com/GiulioRomualdi/study-icub-meshcat/tree/83191c001d2e384ee7d50db7de97013481f5a1b2

traversaro commented 3 years ago

After iterating with @GiulioRomualdi and fixing on the fly some Windows problems, I was able to reproduce with this environment:

(icub-meshcat) C:\src\study-icub-meshcat>conda list
# packages in environment at C:\Users\STraversaro\AppData\Local\miniforge3\envs\icub-meshcat:
#
# Name                    Version                   Build  Channel
assimp                    5.0.1                hc2aa0de_5    conda-forge
backcall                  0.2.0              pyh9f0ad1d_0    conda-forge
backports                 1.0                        py_2    conda-forge
backports.functools_lru_cache 1.6.4              pyhd8ed1ab_0    conda-forge
boost                     1.74.0           py39hefe7e4c_3    conda-forge
boost-cpp                 1.74.0               h5b4e17d_4    conda-forge
bzip2                     1.0.8                h8ffe710_4    conda-forge
ca-certificates           2021.5.30            h5b45459_0    conda-forge
certifi                   2021.5.30        py39hcbf5309_0    conda-forge
colorama                  0.4.4              pyh9f0ad1d_0    conda-forge
console_bridge            1.0.1                h5362a0b_0    conda-forge
decorator                 5.0.9              pyhd8ed1ab_0    conda-forge
eigen                     3.3.9                h2d74725_1    conda-forge
eigenpy                   2.6.4            py39h3ce40e6_0    conda-forge
hpp-fcl                   1.7.4            py39h2e7c763_0    conda-forge
icub-models               1.20.0              h0e60522_16    robotology
intel-openmp              2021.3.0          h57928b3_3372    conda-forge
ipython                   7.25.0           py39h832f523_1    conda-forge
ipython_genutils          0.2.0                      py_1    conda-forge
jedi                      0.18.0           py39hcbf5309_2    conda-forge
libblas                   3.9.0                     9_mkl    conda-forge
libcblas                  3.9.0                     9_mkl    conda-forge
liblapack                 3.9.0                     9_mkl    conda-forge
libsodium                 1.0.18               h62dcd97_1    conda-forge
lz4-c                     1.9.3                h8ffe710_0    conda-forge
matplotlib-inline         0.1.2              pyhd8ed1ab_2    conda-forge
meshcat-python            0.1.1              pyhd8ed1ab_0    conda-forge
mkl                       2021.2.0           hb70f87d_389    conda-forge
numpy                     1.21.0           py39h6635163_0    conda-forge
octomap                   1.9.7                h5362a0b_0    conda-forge
openssl                   1.1.1k               h8ffe710_0    conda-forge
parso                     0.8.2              pyhd8ed1ab_0    conda-forge
pickleshare               0.7.5           py39hde42818_1002    conda-forge
pinocchio                 2.6.2            py39h2e7c763_0    conda-forge
pip                       21.1.3             pyhd8ed1ab_0    conda-forge
prompt-toolkit            3.0.19             pyha770c72_0    conda-forge
pygments                  2.9.0              pyhd8ed1ab_0    conda-forge
pyngrok                   5.0.5              pyhd8ed1ab_0    conda-forge
python                    3.9.6           h7840368_1_cpython    conda-forge
python_abi                3.9                      2_cp39    conda-forge
pyyaml                    5.4.1            py39hb82d6ee_0    conda-forge
pyzmq                     22.1.0           py39he46f08e_0    conda-forge
setuptools                49.6.0           py39hcbf5309_3    conda-forge
sqlite                    3.36.0               h8ffe710_0    conda-forge
tbb                       2021.3.0             h2d74725_0    conda-forge
tinyxml                   2.6.2                h2d74725_2    conda-forge
tornado                   6.1              py39hb82d6ee_1    conda-forge
traitlets                 5.0.5                      py_0    conda-forge
tzdata                    2021a                he74cb21_1    conda-forge
u-msgpack-python          2.7.1              pyh9f0ad1d_0    conda-forge
ucrt                      10.0.20348.0         h57928b3_0    conda-forge
urdfdom                   2.3.5                h2d74725_0    conda-forge
urdfdom_headers           1.0.5                h5362a0b_2    conda-forge
vc                        14.2                 hc4473a8_5    conda-forge
vs2015_runtime            14.29.30037          h902a5da_5    conda-forge
wcwidth                   0.2.5              pyh9f0ad1d_2    conda-forge
wheel                     0.36.2             pyhd3deb0d_0    conda-forge
wincertstore              0.2             py39hcbf5309_1006    conda-forge
xz                        5.2.5                h62dcd97_1    conda-forge
yaml                      0.2.5                he774522_0    conda-forge
zeromq                    4.3.4                h0e60522_0    conda-forge
zlib                      1.2.11            h62dcd97_1010    conda-forge
zstd                      1.5.0                h6255e5f_0    conda-forge

Both Talos and iCub 2.5 show the same problem as saw by @GiulioRomualdi :

talos

icub_something

If the issue was only happening on iCub models, I would have suspected some corner case/obscure point in URDF spec (note that iCub models are correctly loaded in Gazebo, iDynTree, RViz and PyBullet) that was handled different by Pinocchio's URDF parser, but the fact that also the Talos models has some problem seems to indicate some Pinocchio problem. @GiulioRomualdi could it make sense to report this issue (only for what regard the issue related to Talos) on Pinocchio issue tracker?

traversaro commented 3 years ago 
* Shot in the dark: which locale do you have set in the system where you did the tests? You can print it via the `locale` command. A common bug in XML parser is that they parse numbers with functions affected by the locale setting, even if according to the XML specs they should not. See

At least for my Windows-based tests, the locale should not be a problem:

PS C:\Users\STraversaro> Get-UICulture

LCID             Name             DisplayName
----             ----             -----------
1033             en-US            English (United States)

PS C:\Users\STraversaro> Get-Host

Name             : ConsoleHost
Version          : 5.1.19041.1023
InstanceId       : ce1db613-ecd3-4546-958f-5e72f5b3c137
UI               : System.Management.Automation.Internal.Host.InternalHostUserInterface
CurrentCulture   : en-US
CurrentUICulture : en-US
PrivateData      : Microsoft.PowerShell.ConsoleHost+ConsoleColorProxy
DebuggerEnabled  : True
IsRunspacePushed : False
Runspace         : System.Management.Automation.Runspaces.LocalRunspace
traversaro commented 3 years ago

To clarify the difference:

Real Talos Rendered Talos
real_talos talos
GiulioRomualdi commented 3 years ago

Shall we open an issue in pinocchio?

traversaro commented 3 years ago

Yes, I think it make sense, reporting only the Talos part of the problem, as the Talos model is shipped by Pinocchio itself.

GiulioRomualdi commented 3 years ago

ok! I open it

GiulioRomualdi commented 3 years ago

Related issue in pinocchio: https://github.com/stack-of-tasks/pinocchio/issues/1471

GiulioRomualdi commented 3 years ago

Following what @jcarpent. suggested, I switched to pinocchio 2.5.6. Now the Talos problem is solved. However it is not possible to spawn icub since the meshes are not scaled

image

traversaro commented 3 years ago

Following what @jcarpent. suggested, I switched to pinocchio 2.5.6. Now the Talos problem is solved. However it is not possible to spawn icub since the meshes are not scaled

Probabiling the scaling was a bug that was fixed in Pinocchio 2.6.0 ?

jcarpent commented 3 years ago

Could you provide the link to the icub model in use? I will check on my side for the scaling.

jcarpent commented 3 years ago

You can also check on your side by just replacing the content of the Python file in 2.6.2 with the content of stack-of-tasks/pinocchio#1471

GiulioRomualdi commented 3 years ago

Here I'm. Sorry I was jumping from this issue to stack-of-tasks/pinocchio#1471

Here the recap:

  1. I installed Pinocchio 2.6.2 and meshcat with conda
  2. I applied all the changes listed in this PR https://github.com/stack-of-tasks/pinocchio/pull/1472
  3. I run the script stored in https://github.com/GiulioRomualdi/study-icub-meshcat The meshes were not scaled as discussed in https://github.com/stack-of-tasks/pinocchio/issues/1471#issuecomment-881521581
  4. I added the following lines 
    q0 = pin.neutral(model)
viz.display(q0)

    after viz.loadViewerModel(). As a consequence the models are correctly loaded

iCubGazeboV2_5 iCubGazeboV3
image image
GiulioRomualdi commented 3 years ago

I think we can close this issue

traversaro commented 3 years ago

I think we can close this issue

For people in the future: the issue was fixed on the pinocchio side by https://github.com/stack-of-tasks/pinocchio/pull/1472, that is going to be released some time in the future (I guess). So the problem is still present for currently existing releases of pinocchio, including the binary packages available via conda, robotpkg or the ROS build farm.