Buoyancy plugin refactor

[caguero]

This pull requests consolidates the two former buoyancy plugins (buoyancy_gazebo_plugin and usv_gazebo_dynamics_plugin) into a single one (SimpleBuoyancy). I'm open to suggestions for other names.

SimpleBuoyancy is based on buoyancy_gazebo_plugin with a few improvements and bug fixes from the previous version. Now, it's possible to specify via SDF multiple buoyancy blocks with the same <link_name> but different offsets. This is useful for the WAM-V as we want to specify the geometries of the hulls as offsets from base_link.

As an example, we can now reuse the same buoyancy plugin for buoys and for the WAM-V.

How to test it?

Launch the simulation:

ros2 launch vrx_ros competition_local.launch.py ign_args:="-v 4 -r sydney_regatta.sdf"

Launch a WAM-V:

ros2 launch vrx_ign spawn.launch.py name:=wamv world:=sydney_regatta model:=wam-v x:=-532 y:=162 z:=0 R:=0 P:=0 Y:=1

You should see a reasonable floating WAM-V moving in sync with the waves. I also added to the scene a red buoy with the same plugin loaded. The floating behavior of the buoy should look OK as well.

Bonus

This is a video of two WAM-Vs running the two buoyancy plugins considered: the Surface (based on the former usv_gazebo_dynamics and the SimpleBuoyancy introduced in this pull request. It's very qualitative but the behavior is quite similar.

[M1chaelM]

This seems to run as expected and it looks "reasonable" to me, in that, roughly, it bobs and rolls slightly on the water in a believable way. I'm having a hard time telling whether the physics of the waves are in sync with the visual using the sea state of the sydney_regatta.sdf. This is a separate issue, but affects the ability to judge whether buoyancy behavior "looks" right.

[M1chaelM]

I tested with the instructions you gave, and also verified that the objects have reasonable behavior when tilted at an angle in the water, when dropped into the water from a few feet up, and when started in a submerged position. Tilting seems to work the best right now. Dropping and submerging both "work" in that the object moves toward the surface until it reaches a normal floating position, at which point it resumes expected behavior of rising and falling with the wave surface.

The initial movement toward the surface does look a little strange to me. Submerged objects float up very slowly, and dropped objects don't seem to dip as far below the surface as I would expect before coming back up. I'm a little surprised by this because Rumman's original demo led me to expect more of a dampening oscillation.

It could be that I have not set up a comparable demo. I'm not sure, so I think it would be good to get @bsb808 's opinion before merging.

[bsb808]

@caguero Could you include instructions to reproduce the scenario in the "Bonus" section? I'd like to run a few side-by-side test of the old and new approaches to see if there are significant differences in behavior.

[caguero]

@caguero Could you include instructions to reproduce the scenario in the "Bonus" section? I'd like to run a few side-by-side test of the old and new approaches to see if there are significant differences in behavior.

I created a branch for testing both algorithms. Here are the instructions:

• Checkout the branch and compile:

git checkout caguero/buoyancy_testing
IGNITION_VERSION=fortress colcon build --merge-install

• Launch the simulation:

ros2 launch vrx_ros competition_local.launch.py ign_args:="-v 4 -r sydney_regatta.sdf"

• Run the WAM-V with the SimpleBuoyancy plugin:

ros2 launch vrx_ign spawn.launch.py name:=wamv world:=sydney_regatta model:=wam-v x:=-532 y:=162 z:=0 R:=0 P:=0 Y:=1

• Run a second WAM-V with the Surface plugin:

ros2 launch vrx_ign spawn.launch.py name:=wamv_2 world:=sydney_regatta model:=wam-v_2 x:=-535 y:=165 z:=0 R:=0 P:=0 Y:=1

Click on each robot and verify in the Component inspector->System Plugin Info that the right plugins are running in each robot.

[bsb808]

@caguero - Thank you for the side-by-side branch.

I get the following error when using the caguero/buoyancy_testing branch.

Here is the scenario:

1. I successfully run the example using the caguero/wave_refactor branch.
2. Then I switch to the caguero/buoyancy_testing, rebuild, re-source the setup and then restart the simulation. Generates the following errors and crashes:

[ign gazebo-1] Error [parser.cc:729] Error finding file [/home/bsb/.ignition/fuel/fuel.gazebosim.org/openrobotics/models/sydney_regatta/tip].
[ign gazebo-1] [Err] [Server.cc:139] Error Code 1: [/sdf/world[@name="sydney_regatta"]/include[0]/uri:/home/bsb/vrx2_ws/install/share/vrx_ign/worlds/sydney_regatta.sdf:L343]: Msg: Unable to read file[/home/bsb/.ignition/fuel/fuel.gazebosim.org/openrobotics/models/sydney_regatta/tip]
[ign gazebo-1] [Err] [Server.cc:139] Error Code 9: [/sdf/world[@name="sydney_regatta"]:/home/bsb/vrx2_ws/install/share/vrx_ign/worlds/sydney_regatta.sdf:L4]: Msg: Error reading element <world>
[ign gazebo-1] [Err] [Server.cc:139] Error Code 9: Msg: Error reading element <sdf>
[ign gazebo-1] [Err] [Server.cc:139] Error Code 1: Msg: Unable to read file:/home/bsb/vrx2_ws/install/share/vrx_ign/worlds/sydney_regatta.sdf

[bsb808]

I'd recommend a test to check the consistency of behavior with the two buoyancy models. If we have the two plugins operating side-by-side it should be pretty straightforward.

1. Increase the wave field parameters to generate significant waves to exaggerate the vessel motion.
2. Run both VRXs (one with each plugin) side by side and monitor the motion (ideally with a plot, e.g., rqt_plot or plotjuggler)
3. Compare heave (displacement in z), roll and pitch.

If the motion of the two vessels is reasonably consistent and synced with the wave surface (the vessels don't fly above or sink below the surface) - then we have a nice warm fuzzy that the new plugin does the same as the old.

If they are drastically different, it isn't necessarily bad - both models are coarse approximations.

PS - The gif in the description gives a warm fuzzy w.r.t. the heave, but larger waves will induce roll and pitch and stress the methods a bit more.

[bsb808]

On naming.

I propose we change SimpleBuoyancy to PolyhedraBuoyancyDrag. I realize it is a long name, but I think it helps connect the intent of the plugin with the Catto reference. The code appears to be a faithful implementation of the method - see attached catto2006buoyancy.pdf

[caguero]

@caguero - Thank you for the side-by-side branch.

I get the following error when using the caguero/buoyancy_testing branch.

Here is the scenario:

1. I successfully run the example using the caguero/wave_refactor branch.
2. Then I switch to the caguero/buoyancy_testing, rebuild, re-source the setup and then restart the simulation. Generates the following errors and crashes:

[ign gazebo-1] Error [parser.cc:729] Error finding file [/home/bsb/.ignition/fuel/fuel.gazebosim.org/openrobotics/models/sydney_regatta/tip].
[ign gazebo-1] [Err] [Server.cc:139] Error Code 1: [/sdf/world[@name="sydney_regatta"]/include[0]/uri:/home/bsb/vrx2_ws/install/share/vrx_ign/worlds/sydney_regatta.sdf:L343]: Msg: Unable to read file[/home/bsb/.ignition/fuel/fuel.gazebosim.org/openrobotics/models/sydney_regatta/tip]
[ign gazebo-1] [Err] [Server.cc:139] Error Code 9: [/sdf/world[@name="sydney_regatta"]:/home/bsb/vrx2_ws/install/share/vrx_ign/worlds/sydney_regatta.sdf:L4]: Msg: Error reading element <world>
[ign gazebo-1] [Err] [Server.cc:139] Error Code 9: Msg: Error reading element <sdf>
[ign gazebo-1] [Err] [Server.cc:139] Error Code 1: Msg: Unable to read file:/home/bsb/vrx2_ws/install/share/vrx_ign/worlds/sydney_regatta.sdf

Brian, thanks for testing. I think this issue is related with not using the latest version of libignition-fuel-tools7. Could you run vcs pull on your fortress_ws workspace, recompile it, and then, recompile vrx? It should be fixed if you do that.

[caguero]

On naming.

I propose we change SimpleBuoyancy to PolyhedraBuoyancyDrag. I realize it is a long name, but I think it helps connect the intent of the plugin with the Catto reference. The code appears to be a faithful implementation of the method - see attached catto2006buoyancy.pdf

Thanks for the suggestion. Renamed in 19d536c.

[bsb808]

I was able to get the side-by-side WAMVs working in the same simulation, but ran into trouble in changing the wave parameters in the new system - see #507

[bsb808]

I rebuilt everything and was able to adjust the wave parameters to 10 s period and gain = 10.0.

There seems to be a stability problem with the Polyhedra buoyancy setup - see https://vimeo.com/745630912

[acxz]

As someone who has worked a lot on the hydrodynamics for the current master branch, if I may I'd like to offer some thoughts. I apologize if I'm incorrect in some of my statements as I haven't looked at the gazebosim branch much.

This pull requests consolidates the two former buoyancy plugins (buoyancy_gazebo_plugin and usv_gazebo_dynamics_plugin) into a single one (SimpleBuoyancy).

From a quick look at the gazebosim branch, there doesn't seem to be a buoyancy_gazebo_plugin or a usv_gazebo_dynamics_plugin, so I'm not sure if you are consolidating anything here. There does already seem to be a SimpleHydrodynamics that does not include any buoyancy code, which is why I guess this PR is needed to add a Buoyancy computation.

After typing, this out I realized that you are comparing with the master buoyancy_gazebo_plugin/usv_gazebo_dynamics_plugin. In that case, this PR makes sense, however I do think it is jumping a step. Instead of refactoring the buoyancy code while porting over to gazebosim, I believe the proper way should be to refactor the buoyancy code first and then port the logic over to gazebosim (which will be much cleaner and easier to port). This can also prevent additional bugs that may pop up.

With regards to that, #485 should be merged first (i know I'm biased since I wrote the PR, but I do honestly believe that refactoring first and then porting is the better software engineering way of doing this)

Also for wave dynamics in gazebosim, I believe we should use https://github.com/srmainwaring/asv_wave_sim. The work done in that repo has a higher fidelity than what we are doing in ours and I think that spending time to integrate asv_wave_sim is a much better use of time than rewriting our older, low fidelity hydro codes from gazebo classic for gazebosim.

I'm having a hard time telling whether the physics of the waves are in sync with the visual using the sea state of the sydney_regatta.sdf. This is a separate issue, but affects the ability to judge whether buoyancy behavior "looks" right.

Yep, for completeness's sake here is the related issue: #494 This behavior was also observed by @bsb808 in the master branch/gazebo classic version of this PR (#485).

[bsb808]

@acxz - Thank you for the contribution to the discussion. There are a few different aspects of here. I'll take a swing at trying to summarize.

In Gazebo Classic

The VRX simulation made use of two methods that involved buoyancy:

1. The usv_gazebo_dynamics_plugin implements a vessel-centric model described in our "Toward Maritime Robotic Simulation in Gazebo" paper.
   • Buoyancy is one term in this model. The buoyancy estimate is specific to cylindrical hulls (catamaran) and samples points along the hull when calculating a buoyancy forces.
   • Drag is another term in the model and is based on 6 Dof linear and quadratic terms at the vessel-scale, i.e., terms typical of naval architecture models/experimental identification.
2. The buoyancy_gazebo_plugin is an implementation of the "Exact Buoyancy for Polyhedra" by Erin Catto. This is used for buoyancy/drag on elements in the ocean world such as buoys, docks, etc.
   • Buoyancy is based on displaced volume of the polyhedra, assuming flat water surface, and results in an applied force and torque.
   • Drag is implemented as single ad hoc scalar terms for translation and rotation terms.

PR #485 for Gazebo Classic

This proposes using the BuoyancyPlugin from Gazebo upstream as a replacement for the buoyancy elements in usv_gazebo_dynamics_plugin.

Currently seems to be problems with the observed behavior in higher seastates.

While it would be ideal to accomplish this, not clear what functionality this would enable. This is tough to test completely, so introduces regression risk. Currently we are not actively developing the Gazebo Classic branch, only working on maintenance, so, since the current implementation seems to not have problems, we aren't planning to dedicate resources to this change.

Current Status in Gazebo Sim (gazebosim branch)

Much the same as was dong in Gazebo classic.

• usv_gazebo_dynamics_plugin ported as SimpleHydrodynamics (the 6dof drag, Coriolis, added mass, etc.) and Surface (catamaran specific buoyancy - with some generalization)

This PR

• Ports and renames the buoyancy_gazebo_plugin (Polyhedra buoyancy and drag) to PolyhdraBuoyancyDrag.
• Uses the Polyhedra buoyancy and drag plugin as a more generalized replacement for the specific model component from usv_gazebo_dynamics_plugin that is specific to catamaran vessel with cylindrical cross section hulls.

[bsb808]

Proposed way forward.

This PR has grown in scope and does a few different things. I would suggest that we de-scope a bit. Would it be reasonable to do this following:

In this PR we can get the naming updated, basically reproducing what was in Gazebo Classic with new names and implementations of the known working functionality and clarify the method for changing environmental conditions - see Issue #507

Then start a next PR that looks at using the PolyheraBuoyancyDrag method as a replacement for the specific to catamaran vessel with cylindrical cross section hulls.

[acxz]

Thanks so much for responding! I just to clarify some things.

PR #485 for Gazebo Classic

This proposes using the BuoyancyPlugin from Gazebo upstream as a replacement for the buoyancy elements in usv_gazebo_dynamics_plugin.

This is incorrect, #485 actually proposes to use buoyancy_gazebo_plugin.cc which is based on the methodology of Erin Catto in Exact Polyhedra for Buoyancy.

Currently seems to be problems with the observed behavior in higher seastates.

The problems observed there are the same problems as observed in this PR as well as the current master branch. They can be linked back to #494

While it would be ideal to accomplish this, not clear what functionality this would enable.

This will solve #484 and is a stepping stone for #487 See the comment chain under this comment for more rationale: #472 (comment)

This is tough to test completely, so introduces regression risk.

I agree, but since we are already using the same code for other objects in the repo, the code is somewhat battle-tested.

we aren't planning to dedicate resources to this change.

Understood. However, this means that another project has to exist for users that want to continue research on top of Gazebo Classic VRX. I would prefer if, at least until Gazebo Classic EOL, VRX remains a steward for such a project, rather than another member of the community such as myself. Is it not possible for another way, maybe to create another branch that is feature focused called develop instead of the maintenance focused master branch for Gazebo Classic?

Proposed way forward.

For buoyancy in particular (not buoyancy drag/damping), the panel method utilized here: https://github.com/srmainwaring/asv_wave_sim/blob/master/gz-waves/src/Physics.cc for gazebosim is better than the one we are using. In order to keep the maritime plugins in the Gazebo ecosystem from being splintered, @caguero and @srmainwaring should get together and have a meeting on how to best integrate high fidelity buoyancy and waves into GazeboSim/VRX. This architecture'ing should be done before further code is written by @caguero on the hydrodynamics/wavefield codes so that there is no duplicated or wasted effort. In that sense, I believe this PR is jumping the gun before proper consideration of existing (and more accurate) codebases regarding buoyancy/hydrostatic forces.

Note that moving over to asv_wave_sim can also solve the potential issue #494. Even at high sea states, @srmainwaring's code does not produce visual artifacts as the one we are seeing. See the Gazebo Maritime Meeting Post: https://community.gazebosim.org/t/community-meeting-maritime-jun-2022/1472#surface-waves-and-marine-vehicle-dynamics-2

I apologize if I'm being too forward and thanks for listening to my thoughts. As a user of maritime plugins in the Gazebo ecosystem, I just want something that can be generalized and has high fidelity, without having code duplication efforts in the ecosystem. The onset of GazeboSim is the perfect time to do this.

[bsb808]

@acxz Great comments. I appreciate the correction with respect to PR #485.

Merging the work on VRX and the asv_wave_sim project would be fantastic. We have an internal meeting next week and will review all of this.

[caguero]

@bsb808 , @acxz , @M1chaelM, thanks for the useful comments and review!

As discussed, I updated this PR (7b68d65) for using the Surface plugin for producing WAM-V buoyancy instead of removing it in favor of the PolyhedraBuoyancyDrag plugin.

If everybody agrees we can move forward with this approach that it's stable, unblock all the remaining PRs, and then, go back and revisit this aspect.

@acxz , we had a meeting with @srmainwaring to explore how we can combine efforts.

[bsb808]

Approved! Good to go on my end.

Any chance you could comment on - #507 ? I'd be interested to know a better way to change the wave parameters - ideally a single place to enter the values.

Also, I did a few manual tests as an ad hoc stability test: https://vimeo.com/747848884

[caguero]

Approved! Good to go on my end.

Any chance you could comment on - #507 ? I'd be interested to know a better way to change the wave parameters - ideally a single place to enter the values.

Also, I did a few manual tests as an ad hoc stability test: https://vimeo.com/747848884

Replied! For now there's no single place to enter the values but that will change in the future.

[acxz]

we had a meeting with srmainwaring to explore how we can combine efforts.

I am excited the see the results!