search

gazebosim / gz-sim

Open source robotics simulator. The latest version of Gazebo.
https://gazebosim.org
Apache License 2.0
729 stars 272 forks source link

Unbounded velocity growth observed with hydrodynamics plugin #1923

Open laughlinbarker opened 1 year ago

laughlinbarker commented 1 year ago

Environment

There appears to be a bug, which I believe to be related to the hydrodynamics plugin, which results in a model world velocity run-away. I've experienced this with my personal model, and it is reproducible in the AUV example provided by `[auv_controls.sdf](https://github.com/gazebosim/gz-sim/blob/gz-sim7/examples/worlds/auv_controls.sdf#L139)`

Description

In my own simulation, the problem is not exhibited when the added mass terms are 0, or significantly smaller than the vehicle mass. I have adjusted the sign of the added mass terms in my own model (non-example LRAUV system) and this does not change the outcome.

I haven't had time yet to check the implementation of Fossen's Eqns in HydrodynamicsPlugins.cc.

I believe M_A (added mass matrix) should be positive definite, symmetric. Yet the terms that exist in [auv_controls.sdf](https://github.com/gazebosim/gz-sim/blob/206def391d6ba23259ce8596e7256c2d705a9e53/examples/worlds/auv_controls.sdf#L144) result in a decidedly negative definite matrix.

Steps to reproduce

  1. Launch the auv sim: gz sim auv_controls.sdf -v 4
  2. Run the prop momentarily: gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust -m gz.msgs.Double -p 'data: -31'
  3. Stop the prop: gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust -m gz.msgs.Double -p 'data: -0'

Output

You can see a video of the problem manifesting here: https://youtube.com/shorts/zExnMM8K-L8 Notice the world velocity of the AUV growing unbounded even once the prop has been commanded to stop.

ogre2.log

laughlinbarker commented 1 year ago

Of note: when commanding the prop in reverse (gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust -m gz.msgs.Double -p 'data: 31'), the vehicle backs up as expected. Upon stopping the prop, the vehicle decelerates (towards zero velocity), but the sim still crashes after a few seconds.

azeey commented 1 year ago

There was a behavior change introduced recently which has been documented in https://github.com/gazebosim/gz-sim/pull/1888. Can you see if that helps?

laughlinbarker commented 1 year ago

Thanks @azeey. Appropriate naming of those parameters appears (in my test run of N=1) to prevent the runaway condition.

Parameters used:

<plugin
        filename="gz-sim-hydrodynamics-system"
        name="gz::sim::systems::Hydrodynamics">
        <link_name>base_link</link_name>
        <xDotU>-4.876161</xDotU>
        <yDotV>-126.324739</yDotV>
        <zDotW>-126.324739</zDotW>
        <kDotP>0</kDotP>
        <mDotQ>-33.46</mDotQ>
        <nDotR>-33.46</nDotR>

        <xUabsU>-6.2282</xUabsU>
        <yVabsV>-601.27</yVabsV>
        <zWabsW>-601.27</zWabsW>
        <kPabsP>-0.1916</kPabsP>
        <mQabsQ>-632.69</mQabsQ>
        <nRabsR>-632.69</nRabsR>

        <xU>0</xU>
        <yV>0</yV>
        <zW>0</zW>
        <kP>0</kP>
        <mQ>0</mQ>
        <nR>0</nR>
      </plugin>

The mis-naming of the second-order (and in the case of the demo, the only non-zero) damping terms, appears to be at the heart of the issue.

Seems like appropriate course of action is:

laughlinbarker commented 1 year ago

Submitted minimal PR as described above, don't have time at the moment to investigate other options I enumerated.

azeey commented 1 year ago

/cc @arjo129

arjo129 commented 1 year ago

This was fixed in https://github.com/gazebosim/gz-sim/pull/1888. The said fix has not yet been forward ported to Garden. I like the idea of checking the values and warning users in the event values lead to unbounded velocity growth.

mateusmenezes95 commented 1 year ago

We have experienced the same problem in our project simulation, even using the correct naming for the coefficients of the Damping matrix. After deep investigation, I discovered that numerical computation instability can exist if the condition $MA \prec M{RB}$ is unsatisfied, where $MA$ is the Added Mass matrix, $M{RB}$ is the system inertia matrix, and $\prec$ is the element wise less than comparison. The authors of the uuv_simulator already reported this error in section II.D of their paper UUV Simulator: A Gazebo-based package for underwater intervention and multi-robot simulation. Based on the uuv_simulation, @joaobrittoneto have also reported this instability in his master thesis Parameters Identification of Dynamic Model and Simulation of Autonomous Underwater Vehicles, section 4.2.

I tested this criterion using an adapted world file used in the hydrodynamics plugins unit tests in the ign gazebo 6.14.0. To reproduce the error, first, simulate with the hydrodynamics.sdf containing the SDF snippet below using the command ign gazebo hydrodynamics.sdf. This first run shall execute without problems. Now, change the value of zDotU (line 108) to -26.0 kg and rerun the simulation. Using this mass value forces the criterion to be unsatisfied because the sphere's mass is 24 kg, and the left superior block of the Added Mass matrix $M{A{11}} \in R^{3 \times 3}$ equals $mI_{3 \times 3}$; thus -zDotU > m and $MA \prec M{RB}$ is unsatisfied. In my execution, the simulation crashed after some seconds. Even with zDotU -25.0 kg, my simulation crashed.

Thus, if someone could reproduce the crash and this criterion must be satisfied, as the criterion is a simple element-wise comparison, my first suggestion is to put a warning or even a fatal error informing the user about the problem.

hydrodynamics.sdf (change the extension to .sdf): hydrodynamics.txt

arjo129 commented 1 year ago

Hi @mateusmenezes95 yes that is correct, as added mass is expressed in terms of an added unit instead of a percentage in the fossen equations instablilities could arise when Ma < Mrb. I could add a check to the hydrodynamics plugin. This a is a great suggestion.

mateusmenezes95 commented 1 year ago

Very nice @arjo129. Thanks for the fast reply 😄! Another suggestion may be to use the compensation method (named Method 1) suggested by @joaobrittoneto in his thesis that guarantees stability even when the criterion is unsatisfied. However, as reported in the results, this method may degrade the simulation result when there are body rotations. An option is to turn this compensation on/off by a parameter.

arjo129 commented 1 year ago

I don't have time to add the compensation method but PRs are welcome!

mateusmenezes95 commented 1 year ago

Sure! I will try to submit a pull request with the suggestions.

mateusmenezes95 commented 5 months ago

I think that https://github.com/gazebosim/gz-sim/pull/1592 solves this issue for Gazebo Harmonic. What do you think @azeey?

joaobrittoneto commented 4 months ago

I think that https://github.com/gazebosim/gz-sim/pull/1592 solves this issue for Gazebo Harmonic. What do you think @azeey?

I agree with @mateusmenezes95. The fluid_added_mass present in the inertial (which leverages DART's ability to handle 6x6 spatial inertia tensors) should be favored. It leads to a simpler and more robust solution. The only downturn is the reliance on DART itself (it's not supported by other physics engines).

The added mass effect of the hydrodynamics plugin, IMHO, could be deprecated. The current implementation, as known, is brittle. Doing a[k] = -Mrb^-1 * Ma * a[k-1] leads to instability when Ma > Mrb. Trying to use the compensated terms, as mentioned in the references here, leads to a convoluted solution, vide a custom gazebo_underwater plugin I helped to create where those terms are used.

arjo129 commented 4 months ago

I agree with you too. We should deprecate the added mass added by the hydrodynamics plugin and rely ont the physics engine based one.

On Tue, Jul 23, 2024, 9:28 PM João Britto @.***> wrote:

I think that #1592 https://github.com/gazebosim/gz-sim/pull/1592 solves this issue for Gazebo Harmonic. What do you think @azeey https://github.com/azeey?

I agree with @mateusmenezes95 https://github.com/mateusmenezes95. The fluid_added_mass http://sdformat.org/tutorials?tut=added_mass_proposal&cat=pose_semantics_docs present in the inertial (which leverages DART's https://github.com/dartsim/dart/blob/main/dart/dynamics/Inertia.hpp#L123 ability to handle 6x6 spatial inertia tensors) should be favored. It leads to a simpler and more robust solution. The only downturn is the reliance on DART itself (it's not supported by other physics engines).

The added mass effect of the hydrodynamics plugin, IMHO, could be deprecated. The current implementation, as known, is brittle. Doing a[k] = -Mrb^-1 Ma a[k-1] https://github.com/gazebosim/gz-sim/blob/206def391d6ba23259ce8596e7256c2d705a9e53/src/systems/hydrodynamics/Hydrodynamics.cc#L492 leads to instability when Ma > Mrb. Trying to use the compensated terms, as mentioned in the references here https://github.com/gazebosim/gz-sim/issues/1923#issuecomment-1661282262, leads to a convoluted solution, vide a custom gazebo_underwater plugin https://github.com/rock-gazebo/simulation-gazebo_underwater/blob/master/src/GazeboUnderwater.cpp I helped to create where those terms are used.

— Reply to this email directly, view it on GitHub https://github.com/gazebosim/gz-sim/issues/1923#issuecomment-2245830107, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAEEMQH7FFRSP3Y27GAXPOTZN2HC3AVCNFSM6AAAAAAVVWXWP6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDENBVHAZTAMJQG4 . You are receiving this because you were mentioned.Message ID: @.***>