Fix external wrench plugin visualization on Gazebo 11

[yeshasvitirupachuri]

During some tests I noticed that the visualization of the cylinder when using the external wrench plugin is odd as below

I tried with yarp and gazeboyarpplugins in devel branch. The usual behavior is the cylinder appears at the link at which we apply the external wrench

@paolo-viceconte notice the same behavior using master branches of yarp and gazeboyarpplugins

@MiladShafiee @prashanthr05 @GiulioRomualdi Did you notice this problem while using external wrench plugin ?

CC @traversaro

[yeshasvitirupachuri]

I am using Gazebo 11.0.0

@paolo-viceconte please report the Gazebo version you are using

[yeshasvitirupachuri]

I did some debugging and double checked if the visual message pose is set correctly, and it is. So, may be there is something wrong going in the rendering.

[paolo-viceconte]

@paolo-viceconte please report the Gazebo version you are using

Same for me, Gazebo 11.0.0

[MiladShafiee]

I also remember sometimes I had this behavior and I used the Gazebo 9.11.0

[traversaro]

I had some issues with Gazebo 11 and SDF models recently (see gazebosim/gazebo-classic#2728 and gazebosim/gazebo-classic#2739), so first of all I would try to check if this issue is affecting only Gazebo 11 or also Gazebo 9/10.

Furthermore, it would be great if when reporting an issue such as this you could also provide these details that are useful:

• Precise commits sha of YARP and gazebo-yarp-plugins, as master or devel are branches that change depending on the time, you can get this sha by running git log in the YARP and gazebo-yarp-plugins repos
• Which model of iCub are you using? Are you dropping it in the simulation through the GUI or via a world? If a world, which world? As you can read in URDF included in SDF 1.6 is silently corrupted in Gazebo 11 when spawned from the GUI gazebosim/gazebo-classic#2728 and URDF model included in SDF 1.6 model included in SDF 1.6 world is silently corrupted in Gazebo 11 gazebosim/gazebo-classic#2739, there are a lot of problems related to SDF 1.6 models, so a good first try may be to just bump all the involved models to SDF 1.7 , to see if the problem is related.

Which exact command are you sending to the plugin? I am particularly interested to understand if this happens on all the links, and also on pure forces (in all the screenshot I see that you are sending external forces with both forces and torques). Related to that, if you are able to reproduce the problem in a simple model with just one link (as the one in https://github.com/robotology/gazebo-yarp-plugins/tree/master/tutorial/model/camera) it would probably be much simpler to identify the issue.

Note that the iRonCub team (@gabrielenava @HosameldinMohamed @Giulero @FabioBergonti @vpunithreddy) have a plugin in a private repo that is also rendering cylinders, and as far as I understand that one is working correctly on Gazebo 11, so you may want to check with them why that one is working.

I also remember sometimes I had this behavior and I used the Gazebo 9.11.0

Thanks @MiladShafiee , if you could double check to be sure it would be great. That "sometimes" is a bit difficult to process when you want to understand what is going wrong, especially because it seems from @Yeshasvitvs and @paolo-viceconte that the issue is deterministic (i.e. happening in all their tests). Furthermore, it would be also great if you could report this problems as soon as you experienced them, to catch (and solve, if there is someone interested in working on them) them early and avoid that they impact the work of other people in the team.

I tried with yarp and gazeboyarpplugins in devel branch.
...
@paolo-viceconte notice the same behavior using master branches of yarp and gazeboyarpplugins

Not directly related to this issue, but noticed that since robotology/community#365 YARP is not using anymore the master/devel workflow. To know the branches of YARP and gazebo-yarp-plugins that are tested to be compatible with one another, please check https://github.com/robotology/robotology-superbuild/blob/master/cmake/ProjectsTagsStable.cmake for stable branches (for the repos not present in the file it is assumed "master") while https://github.com/robotology/robotology-superbuild/blob/master/cmake/ProjectsTagsUnstable.cmake for the development branches.

[MiladShafiee]

if you could double check to be sure it would be great

For the test that I'm doing it now, it seems there is no problem(Gazebo 9.11.0 and branch feature/externalwrench-smoothing and also with the master branch of origin, YARP version 3.3.1 master, icub-model master):

Furthermore, it would be also great if you could report this problems as soon as you experienced them, to catch (and solve, if there is someone interested in working on them) them early and avoid that they impact the work of other people in the team.

I think multiple months ago during primary tests I had this problem and because I was struggling with the algorithm I didn't pay attention about that, sorry for that.

[yeshasvitirupachuri]

Precise commits sha of YARP and gazebo-yarp-plugins

I tested with

• yarp devel branch at robotology/yarp@5c3406b
• GazeboYarpPlugins devel branch at 54c5022

Which model of iCub are you using?

Using icub-models master branch at robotology/icub-models@371cc30

Are you dropping it in the simulation through the GUI or via a world?

Dropping the model through GUI

[paolo-viceconte]

Precise commits sha of YARP and gazebo-yarp-plugins

I tested on the setup of the Development docker image by @diegoferigo:

• yarp yarp-3.3 branch at a33cf54b1 commit
• GazeboYarpPlugins master branch at 12d962e commit

Which model of iCub are you using?

icub-models master branch at 44d0c7e commit, iCubGazeboV2_5

Are you dropping it in the simulation through the GUI or via a world?

I am dropping the model via GUI as well

[traversaro]

Thanks for the details! Indeed, from your input it seems that there is some kind of regression in the ~/visual topic of the model in Gazebo 11. As mentioned before, if you are able to reproduce the problem on a simpler model (even better using a standalone executable) the best next step is open a issue upstream at https://github.com/osrf/gazebo .

[traversaro]

As you can read in osrf/gazebo#2728 and osrf/gazebo#2739, there are a lot of problems related to SDF 1.6 models, so a good first try may be to just bump all the involved models to SDF 1.7 , to see if the problem is related.

@Yeshasvitvs @paolo-viceconte did you checked if updating the model to SDF 1.7 solved any of your issues? For what regards gazebosim/gazebo-classic#2728 there is a PR with a proposed fix in gazebosim/gazebo-classic#2734, so that could be something else to check out.

[yeshasvitirupachuri]

did you checked if updating the model to SDF 1.7

@traversaro I tried by changing the sdf version in model.config files and the behavior is the same.

Also, I noticed a small typo in external wrench plugin rpc reply messages at https://github.com/robotology/gazebo-yarp-plugins/blob/master/plugins/externalwrench/src/ApplyExternalWrench.cc#L212. The message should be

link **not** found in the model

[traversaro]

Hi @Yeshasvitvs, thanks to the related debug of @HosameldinMohamed, it is possible that this behavior is due to some changes from ignition-math4 to ignition-math6 . In another plugin what created problems for us was the change in behaviour of operator*(Pose3d, Pose3d), see #415 and gazebosim/gz-math#60 . At a first glance it does not seem that we are using operator*(Pose3d, Pose3d) in this plugin, but I wonder if something is using it inside Gazebo and it is creating this problem.

[yeshasvitirupachuri]

At a first glance it does not seem that we are using operator*(Pose3d, Pose3d) in this plugin, but I wonder if something is using it inside Gazebo and it is creating this problem.

I had a quick look at the code and the pose of the visual is handled is https://github.com/robotology/gazebo-yarp-plugins/blob/master/plugins/externalwrench/src/ExternalWrench.cc#L277-L281

for reference, in gazebo documentation I could not find the method mutable_pose(), and the reasons are as pointed in

• https://answers.gazebosim.org/question/23873/how-to-change-sdf-parameter-static-from-gui-plugin/?answer=23876#post-id-23876
• https://answers.gazebosim.org//question/16295/where-i-can-find-contact_size-function/?answer=16298#post-id-16298

and the function call msgs::set(msgs::Pose * _p, const ignition::math::Pose3d & _v ) http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/group__gazebo__msgs.html#gab1f035de1770f32d018054eb2e22cb8d

simply sets the position and orientation https://github.com/osrf/gazebo/blob/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/gazebo/msgs/msgs.cc#L157