iDynTree Visualizer-based drop in replacement for iCubGui?

[traversaro]

Given the latest development in the visualizer contained in iDynTree 3, it should be now relatively easy to develop a simple drop-in replacement for the iCubGui that gets the configuration of the robot and the external force from YARP ports, but gets the kinematic model and the meshes of the robot from the URDF models rather then the custom BVH models used in iCubGui (see https://github.com/robotology/icub-main/pull/531/files). This may be convenient for new and custom robots developed now (such as iRonCub) or in the future.

[S-Dafarra]

I totally agree. For a recent demo we used the iCubGui extensively, and in general it proved to be pretty useful to have a visualization of the external wrenches and to have an idea of what the robot is doing when you don't have a clear view of it.

I started doing something similar in https://github.com/S-Dafarra/dyn-visualizer. Initially, I was thinking to port it to walking-teleoperation, but if you think there could be another spot more visible, just let me know. I can also move it to iDynTree, but clearly it will need YARP as a dependency.

The features are still missing compared to iCubGui:

• visualization of forces
• the possibility to connect to ports rather than opening the control boards (i.e.after a reboot it is simply necessary to reconnect rather than launching it again)
• hands and eyes movements have not been tested
• visualization of face expressions

[traversaro]

I can also move it to iDynTree, but clearly it will need YARP as a dependency.

And also it probably make sense to have ICUB as a dependency, as the most complete description of external forces (that the iCubGui currently uses) is published on port via the skinDynLib library (conversions to pure iDynTree stuff going on in https://github.com/robotology/idyntree/blob/master/src/icub/include/iDynTree/skinDynLibConversions.h).

In general, after robotology/idyntree#807 iDynTree as a library does not depend at build time on YARP or ICUB (you just depend on YARP or ICUB if you want to use the specific compatibility headers, but in that case probably you are already depending on YARP and ICUB). In there in fact one of the open points was:

Command line tools that depend on both idyntree and YARP, such as yarprobostatepublisher or urdf2df. Ideally, this tools could be moved to a different repo (idyntree-yarp-tools?) but that can be done without breaking anything, while making this libraries header-only needs to be done before iDynTree 3 release as it will break ABI compatibility.

Probably we can start creating a idyntree-yarp-tools repo that can contain this tools that depend on both YARP (or also other dependencies, such as ICUB or Qt for GUIs) and iDynTree, and for this specific reason we don't want to have them either in YARP or iDynTree. The first thing to put in idyntree-yarp-tools could be indeed this iCubGui drop in replacement, and then we can slowly migrate other commands from idyntree to idyntree-yarp-tools. What do you think? If you agree, I can start creating the idyntree-yarp-tools repo.

The features are still missing compared to iCubGui:

visualization of forces
the possibility to connect to ports rather than opening the control boards (i.e.after a reboot it is simply necessary to reconnect rather than launching it again)

At least for use in whole-body control demos, I think that this two points are really the one of interest for the MVP. For the first point, it is probably a good idea to revive robotology/icub-main#462, better to do it now that we are early in the release cycle of the 2021.02/1.19 distro/icub release. If you are interested, I can revive robotology/icub-main#462 . For the possibility to connect ports rather then devices, it could make sense to evaluate if it is convenient to re-use the ReadOnlyRemoteControlBoard YARP device, that once we had in human-dynamics-estimation but apparently it is not there anymore, but we should still have somewhere.

hands and eyes movements have not been tested
visualization of face expressions

I think this are nice to haves, but users interested in this use case can add support for them later.

[S-Dafarra]

Probably we can start creating a idyntree-yarp-tools repo that can contain this tools that depend on both YARP (or also other dependencies, such as ICUB or Qt for GUIs) and iDynTree, and for this specific reason we don't want to have them either in YARP or iDynTree. The first thing to put in idyntree-yarp-tools could be indeed this iCubGui drop in replacement, and then we can slowly migrate other commands from idyntree to idyntree-yarp-tools. What do you think? If you agree, I can start creating the idyntree-yarp-tools repo.

Sounds good to me! For example, I found it annoying that it is not possible to move the joints in idyntree-model-view. It would be cool if we were able to move them via the yarp motorgui for example, or to replay the motion dumped via yarpdatadumper or similar tools for example.

For the first point, it is probably a good idea to revive robotology/icub-main#462, better to do it now that we are early in the release cycle of the 2021.02/1.19 distro/icub release. If you are interested, I can revive robotology/icub-main#462 .

Up to you. I am not completely aware of how the contacts are visualized in the iCubGUI at the moment. If it is just reading from ports, then I would be also ok in porting the same logic. On the other hand, if those improvements allow using fewer assumptions (i.e. reading from the port you get both the wrench and where it is located in the model, not just the numeric value), that would be pretty interesting. If instead, it is very dependent on the availability of skin, this would complicate the usability with iCub3 for example.

For the possibility to connect ports rather then devices, it could make sense to evaluate if it is convenient to re-use the ReadOnlyRemoteControlBoard YARP device, that once we had in human-dynamics-estimation but apparently it is not there anymore, but we should still have somewhere.

That would also be interesting. If it is already available, then it would be pretty comfortable. Otherwise, it is not much of a big deal to restart the application. It just gets a little annoying when you spend time in fixing the camera, the dimension of the window or its location in the screen.

[traversaro]

For the first point, it is probably a good idea to revive robotology/icub-main#462, better to do it now that we are early in the release cycle of the 2021.02/1.19 distro/icub release. If you are interested, I can revive robotology/icub-main#462 .

Up to you. I am not completely aware of how the contacts are visualized in the iCubGUI at the moment. If it is just reading from ports, then I would be also ok in porting the same logic. On the other hand, if those improvements allow using fewer assumptions (i.e. reading from the port you get both the wrench and where it is located in the model, not just the numeric value), that would be pretty interesting. If instead, it is very dependent on the availability of skin, this would complicate the usability with iCub3 for example.

Reading the forces from the /wholebodydynamics/contacts:o works both with and without the skin and gives you all the forces that are acting on the robot, their location and the link on which they are applied, all by reading from a single port. This is simply much more scalable and convenient then just reading from a fixed set of 6D vector ports.
The main reason why reading from the 6D vector ports is widespread is

1. we don't have a quick/convenient way to read from YARP ports that publish structured data in Simulink
2. A lot of application (walking) are really concerned just on forces on a specific link (foot)

However, for a visualizer I think that reading from a structured port all the forces acting on the robot is quite convenient, think for example on iCub3 to visualize the two forces on the parts of the sole.

[S-Dafarra]

For the first point, it is probably a good idea to revive robotology/icub-main#462, better to do it now that we are early in the release cycle of the 2021.02/1.19 distro/icub release. If you are interested, I can revive robotology/icub-main#462 .

Up to you. I am not completely aware of how the contacts are visualized in the iCubGUI at the moment. If it is just reading from ports, then I would be also ok in porting the same logic. On the other hand, if those improvements allow using fewer assumptions (i.e. reading from the port you get both the wrench and where it is located in the model, not just the numeric value), that would be pretty interesting. If instead, it is very dependent on the availability of skin, this would complicate the usability with iCub3 for example.

Reading the forces from the /wholebodydynamics/contacts:o works both with and without the skin and gives you all the forces that are acting on the robot, their location and the link on which they are applied, all by reading from a single port. This is simply much more scalable and convenient then just reading from a fixed set of 6D vector ports.
The main reason why reading from the 6D vector ports is widespread is

1. we don't have a quick/convenient way to read from YARP ports that publish structured data in Simulink
2. A lot of application (walking) are really concerned just on forces on a specific link (foot)

However, for a visualizer I think that reading from a structured port all the forces acting on the robot is quite convenient, think for example on iCub3 to visualize the two forces on the parts of the sole.

Totally agree. This would be available even without robotology/icub-main#462 right?

[traversaro]

Totally agree. This would be available even without robotology/icub-main#462 right?

Yes, the main problem is that skinDynLib identifies link and frames with numerical indices, while iDynTree an the rest of the URDF-using world uses strings. To bridge the gap, in wholebodydynamics we have the IDYNTREE_SKINDYNLIB_LINKS configuration group that provide the mappings between skinDyn numerical indices and URDF names, but that is duplicated in every conf file (see for yourself: https://github.com/robotology/robots-configuration/search?q=IDYNTREE_SKINDYNLIB_LINKS). While we could have a similar group in the idyntree-yarp-model-viewer , with robotology/icub-main#462 we would not need it anymore. So robotology/icub-main#462 is not an hard requirement to implement force visualization in idyntree-yarp-model-viewer , just a nice to have.

[lrapetti]

• visualization of forces
• the possibility to connect to ports rather than opening the control boards (i.e.after a reboot it is simply necessary to reconnect rather than launching it again)

Those points are partially addressed in the HumanStateVisualizer with robotology/human-dynamics-estimation#241 (ports are used just for the wrenches that are streamed by analogue server device, but I directly connect to the port in the main)

[S-Dafarra]

Just for discussion, it would be also interesting to understand how to plot the torques. In Gazebo they use a sort of curved arrow shape, while in the iCubGui there is an arrow of a different color. I could not find a built-in command to draw a curved arrow in irrlicht, while the different color option tends to be confusing 🤔

[S-Dafarra]

• visualization of forces
• the possibility to connect to ports rather than opening the control boards (i.e.after a reboot it is simply necessary to reconnect rather than launching it again)

Those points are partially addressed in the HumanStateVisualizer with robotology/human-dynamics-estimation#241 (ports are used just for the wrenches that are streamed by analogue server device, but I directly connect to the port in the main)

Indeed, the general structure of this module would match a lot with the HumanStateVisualizer. I guess that through configuration files it would be possible to use it in different ways, matching the functionalities also of https://github.com/S-Dafarra/dyn-visualizer (i.e. streaming the visualization also through a port) and those discussed in this issue (connect to the robot via ports, use of wbd contacts).

@lrapetti @traversaro what would you suggest? Should we keep the two applications separated or we try to have a single application with complex configuration files?

[traversaro]

@lrapetti @traversaro what would you suggest? Should we keep the two applications separated or we try to have a single application with complex configuration files?

My intuition is that it could make sense to have two separate applications, especially if in the future @lrapetti may need to customize HumanStateVisualizer for its own needs it may be better to have the freedom to do so. But this is just my intuition, either solution is fine for me.

[S-Dafarra]

@lrapetti @traversaro what would you suggest? Should we keep the two applications separated or we try to have a single application with complex configuration files?

My intuition is that it could make sense to have two separate applications, especially if in the future @lrapetti may need to customize HumanStateVisualizer for its own needs it may be better to have the freedom to do so. But this is just my intuition, either solution is fine for me.

I just noticed that, rightfully, HumanStateVisualizer depends on the IHumanState interface. This would add an additional dependency to the idyntree-yarp-tools that may not be needed for most applications.

[traversaro]

Yes, these are the kind of cross-dependency I was scared about. I think that if there are strong commonalities we can probably refactor them as C++ libraries that we add down in the stack (for example in iDynTree or something else) and that can be shared by both visualizers.

[lrapetti]

Yes, these are the kind of cross-dependency I was scared about. I think that if there are strong commonalities we can probably refactor them as C++ libraries that we add down in the stack (for example in iDynTree or something else) and that can be shared by both visualizers.

I agree on having the two main application separate since they can have different dependencies and data sources.
Besides common utilities library, we can think of a layer, such-as model-state-visualization, that allows to visualize the model plus associated wrenches, and eventually joint torques as coloured spheres (I was planning to add them to mimic the RViz human-dynamics-estimation visualization)

[traversaro]

I think we can move this issue to https://github.com/robotology/idyntree-yarp-tools, what do you think @S-Dafarra ?

[S-Dafarra]

I think we can move this issue to https://github.com/robotology/idyntree-yarp-tools, what do you think @S-Dafarra ?

Sounds good! I am planning to work on it soon, although I haven't started yet.

[traversaro]

Done!

[traversaro]

Related PRs:

• First visualization draft #4
• Visualizer configuration #5

@S-Dafarra my last doubt is on the name, could it make sense to call it idyntree-yarp-visualizer rather then idyntree-visualizer? I think this would help to avoid confusion w.r.t. other existing tools such as idyntree-model-view .

[S-Dafarra]

Related PRs:

• First visualization draft #4
• Visualizer configuration #5

@S-Dafarra my last doubt is on the name, could it make sense to call it idyntree-yarp-visualizer rather then idyntree-visualizer? I think this would help to avoid confusion w.r.t. other existing tools such as idyntree-model-view .

No problem for me!

Regarding the visualization of the hands, I tried to connect to the robot while using the iCubGazeboV2_5_visuomanip model, but I had an error because some of the hands' joint names do not coincide with the robot ones.

[traversaro]

Regarding the visualization of the hands, I tried to connect to the robot while using the iCubGazeboV2_5_visuomanip model, but I had an error because some of the hands' joint names do not coincide with the robot ones.

Good to know. This is a byproduct of robotology/icub-models#30 .

[lrapetti]

What is still missing:

visualization of external wrenches

Just to document it somewhere, I recently ended up in the following "issue":
I needed to visualize the wrench associated to a frame of the model, but currently there is no direct way to get the frame pose from a visualized model, but only links frame are exposed with getWorldLinkTransform . Do you think it makes sense to open an issue in iDynTree in order to add a getFrameTransform method, or do you have other strategies to achieve that?

[S-Dafarra]

What is still missing:
visualization of external wrenches

Just to document it somewhere, I recently ended up in the following "issue":
I needed to visualize the wrench associated to a frame of the model, but currently there is no direct way to get the frame pose from a visualized model, but only links frame are exposed with getWorldLinkTransform . Do you think it makes sense to open an issue in iDynTree in order to add a getFrameTransform method, or do you have other strategies to achieve that?

Yes, I think we can add it easily. The trick would be to retrieve from the model the frame parent link (https://robotology.github.io/idyntree/master/classiDynTree_1_1Model.html#a64e8453f19a7c6f4e9dd0d9063062854), and the corresponding relative transformation (https://robotology.github.io/idyntree/master/classiDynTree_1_1Model.html#ad3e8276d1ec7e275aadeb75c1add80b6). Then, it is just matter of chaining the two transformations together.
We can hide this complexity in the interface though.

[lrapetti]

Then, it is just matter of chaining the two transformations together.
We can hide this complexity in the interface though.

PR opened at robotology/idyntree#849

[lrapetti]

I think this issue can be closed since the idyntree-yarp-visualizer is now being used

[traversaro]

Yes, it is not strictly speaking a drop-in replacement for iCubGui, but I guess at this point that is ok.