search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.92k stars 395 forks source link

Talos robot not correctly loaded in the meshcat-viewer example #1471

Closed GiulioRomualdi closed 3 years ago

GiulioRomualdi commented 3 years ago

Hi all, first of all, thank you for the effort to develop and maintain pinocchio.

I was trying to visualize the Talos robot following this example. However I noticed that the model is not correctly loaded in meshcat.

Indeed there seems that the right shoulder and the legs are not correctly attached to the robot body.

Real Talos Visualized Talos
talos talos

I installed meshcat and pinocchio with conda in a clean Ubuntu 20.04 distro.

For reference, my packages are:

conda list
# packages in environment at /home/gromualdi/miniforge3/envs/pinocchio-env:
#
# 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

Could you help me to solve this issue? Thank you in advance

cc @traversaro

jcarpent commented 3 years ago

Thanks @GiulioRomualdi for raising this issue and reporting for this bug. I think this is due to some slight but important changes in the way we parse frames in the URDF parser and which has been introduced in Pinocchio 2.6.0. You can use 2.5.6 in meantime, and I will provide a fix later this day.

GiulioRomualdi commented 3 years ago

Hi @jcarpent. Thank you! With pinocchio 2.5.6 the problem disappears

jcarpent commented 3 years ago

@GiulioRomualdi I've fixed the bug and in fact it was only related to Meshcat support of Pinocchio. Please cherry-pick this commit 24dbede to solve locally your issue.

I will provide a release soon to fix it at least on conda. Thanks again for the quick report.

jcarpent commented 3 years ago

@GiulioRomualdi Could you confirm that fixes your issue in robotology/icub-models#97?

GiulioRomualdi commented 3 years ago

@GiulioRomualdi I've fixed the bug and in fact it was only related to Meshcat support of Pinocchio. Please cherry-pick this commit 24dbede to solve locally your issue.

I will provide a release soon to fix it at least on conda. Thanks again for the quick report.

https://github.com/stack-of-tasks/pinocchio/commit/24dbede81e884dc0be2bb05ecd1d3f7171cba982 solves the problem with Talos and Pinocchio 2.6.2. However, when I try to load the iCub (robotology/icub-models#97) the meshes are not scaled

image

without 24dbede the meshes are scaled even if the robot is not correctly loaded

GiulioRomualdi commented 3 years ago

Adding the following lines after

    q0 = pin.neutral(model)
    viz.display(q0)

viz.loadViewerModel() here, the robot is scaled and in the neutral configuration.

image

Looking in the code seems the meshes are scaled only when display is called (e.g. https://github.com/stack-of-tasks/pinocchio/pull/1472/commits/50f49cc7fc3ddd110f4bd09d381b9afb081e7ffa). Is it wanted?

jcarpent commented 3 years ago

I've just update #1472 to also scale the geometries when loading the meshes. Yet, it seems that there is a bug within MeshCat to use the scaling information when changing the placement (via set_transform).

jcarpent commented 3 years ago

Small remark: according to https://github.com/rdeits/meshcat/blob/1ea52a649c0c8df800587b54d0910ef2868f8de5/src/index.js#L357-L361, it seems that Meshcat erases the previous scaling.

jcarpent commented 3 years ago

Solved by #1472

traversaro commented 3 years ago

Thanks a lot @jcarpent for the fix!

akhilsathuluri commented 2 years ago

Hello, I am trying to reproduce the exact scenario as above. The iCub model loads to the viewer by,

viz.initViewer(loadModel=True)

as shown below, image

However, when I modify the pose of the robot (or even in neutral) by setting certain angles and upon visualising it using,

q0 = pin.neutral(robot.model())
viz.display(q0)

the robot model completely disappears from the meshcat viewer. I suspect this has to do with the scaling too.

My setup: Ubuntu 20.04, pinocchio-2.6.0

jcarpent commented 2 years ago

Could you use a more recent version of Pinocchio?

akhilsathuluri commented 2 years ago

Thanks for a super quick reply. I had a bunch of dependency issues between tsid and pinocchio versions. But finally pinocchio=2.6.3 with tsid=1.6.1 fixed the issue.

traversaro commented 2 years ago

I think the problem experienced by @akhilsathuluri comes from the fact that there is no tsid build with a recent urdfdom release. This should be fixed in the short term by https://github.com/conda-forge/tsid-feedstock/pull/8, and https://github.com/conda-forge/conda-forge-pinning-feedstock/pull/2925 should avoid that this problem occurs again in the future.