Support fluid added mass

[chapulina]

🎉 New feature

• Part of Hydrodynamic added mass gz-sim#1462
• Requires
  • Add fluid added mass to inertial gz-math#459
  • Add fluid added mass to inertial sdformat#1077

Summary

Uses the APIs added as part of the Fluid Added Mass Proposal.

Ideally the added mass functionality would be part of an optional feature. I have to look a bit deeper to figure out how to take that out of the link construction.

Test it

I'm working on tests and a gz-sim PR

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

[scpeters]

Ideally the added mass functionality would be part of an optional feature. I have to look a bit deeper to figure out how to take that out of the link construction.

this is beyond the scope of this PR; we don't currently have granularity in our SDF construction features. I'll see if we have an issue about this yet

[scpeters]

I would comment here about how the order of variables differs between SDFormat / Gazebo and Dartsim / Featherstone, which is why the submatrices need to be shuffled.

[chapulina]

We're past feature freeze ❄️ . The plan is to not merge this PR into gz-physics6 until after the stable release (i.e. October).

[srmainwaring]

@scpeters I've been experimenting with this and think there may be a problem with the way the fluid added mass influences the gravity force in dart and this affects hydrostatic calculations.

If a body is placed at its equilibrium position (say a sphere with mass = 1/2 volume x water density with centre of buoyancy = centre of mass = centre of geometry placed with it's origin at the free surface), then the net force (hydrostatic + gravity) should be zero and the body should not accelerate.

What happens however is that the hydrostatic force is calculated in a plugin with the body mass, whereas the gravity force appears to be calculated in dart using the spatial tensor (which has been modified with the fluid added mass, so the total adjusted mass is > body mass). The forces are no longer balanced and the object accelerates downwards.

I'm still going through the details to understand how dart implements this, but the problem seems to be that gravity is added globally for the world in the physics engine, whereas the remainder of the forces are added externally via plugins.

One solution may be to have an option to switch gravity off in the core physics engine and add it back in via a system plugin that adds the gravity force for each link using the mass matrix mass?

Edit

Updated with example figures.

The figures below illustrate the issue. They both show two runs of a drop test for a large ellipsoid buoy with hydrostatic restoring forces and a hydrodynamic radiation damping + added mass (on/off). The upper plot in each case is the buoy pose z-component with and without added mass. In figure 1 where dart calculates gravity the equilibrium pose with added mass is lower than the result when added mass is disabled. In figure 2 the world gravity is disabled and the gravity force is calculated in the plugin using the body mass. Here both lines have the same equilibrium pose and the effect of added mass is visible through the lowered frequency and increased amplitude.

Figure 1. Radiation damping and added mass 5m drop test. Dart gravity enabled.

Figure 2. Radiation damping and added mass 5m drop test. Dart gravity disabled (gravity calculated in plugin)

Btw. this is using a fully populated added mass matrix from a BEM calculation which I'm passing via a custom plugin rather than via the inertial SDF element:

<added_mass>
  52732.7109003133 -0.0461102712478722  5.88390664387022 0.296417351644741 246367.266847758, -1.69953653780694
  -0.0678279438415122 52944.8240664938 -1.47610970294622 -246974.480351485 -0.376605346811632 34.7833617031108
  5.56653991122738 0.0361713254775427 197297.142256690 0.802697521049529 -87.4935912381782 -0.0504125898702509
  0.410840691063419 -247341.339091709 5.02714890072835 1214533.79620113 2.83888902153701 -166.779120688123
  246734.857503928 -0.346433003966073 -87.1795788996776 2.60143644204397 1215195.98272167 -2.36242713115735
  -1.30857894347732 34.6478012553392 0.0173325214982212 -165.977540011457 -1.09781377470450 108.258384753611
</added_mass>

Dart complains about the spatial tensor being asymmetric and having non-zero elements, but then carries on anyway 🙂

[JoanAguilar]

Hi @srmainwaring, thanks for your comment. We suspected that the current added mass implementation might have an effect on how DART computes the gravity force, your tests seem to confirm that. We concur that disabling gravity in DART and applying it via a plugin might be a good solution to the problem.

[srmainwaring]

Here's a suggestion for calculating the missing term in the spatial tensor when the mass block no longer comprises a 3x3 diagonal matrix with identical elements:

Edited - simplified calc

import numpy as np

fAddedMass = np.array([
52732.7109003133, -0.0461102712478722, 5.88390664387022, 0.296417351644741, 246367.266847758, -1.69953653780694,
-0.0678279438415122, 52944.8240664938, -1.47610970294622, -246974.480351485, -0.376605346811632, 34.7833617031108,
5.56653991122738, 0.0361713254775427, 197297.142256690, 0.802697521049529, -87.4935912381782, -0.0504125898702509,
0.410840691063419, -247341.339091709, 5.02714890072835, 1214533.79620113, 2.83888902153701, -166.779120688123,
246734.857503928, -0.346433003966073, -87.1795788996776, 2.60143644204397, 1215195.98272167, -2.36242713115735,
-1.30857894347732, 34.6478012553392, 0.0173325214982212, -165.977540011457, -1.09781377470450, 108.258384753611
]).reshape(6, 6)

# decomposition of the fluid added mass matrix
M    = fAddedMass[0:3, 0:3]          # mass block
I    = fAddedMass[3:6, 3:6]          # moi block
CM   = fAddedMass[3:6, 0:3]          # CoM-terms

# solve for C
C = np.matmul(CM, np.linalg.inv(M)) 
CMCT = np.matmul(C, np.matmul(M, C.transpose()))

# contributions to the spatial tensor
TL   = I + CMCT   
BL   = CM.transpose()
TR   = CM  
BR   = M

# assemble spatial tensor
S    = np.zeros([6, 6])
S[0:3, 0:3] = TL
S[3:6, 0:3] = BL
S[0:3, 3:6] = TR
S[3:6, 3:6] = BR

with np.printoptions(precision=0, suppress=True):
    print(f"fAddedMass:\n{fAddedMass}")
    print(f"C:\n{C}")
    print(f"M:\n{M}")
    print(f"I:\n{I}")
    print(f"CMCT:\n{CMCT}")
    print(f"S:\n{S}")

fAddedMass:
[[  52733.      -0.       6.       0.  246367.      -2.]
 [     -0.   52945.      -1. -246974.      -0.      35.]
 [      6.       0.  197297.       1.     -87.      -0.]
 [      0. -247341.       5. 1214534.       3.    -167.]
 [ 246735.      -0.     -87.       3. 1215196.      -2.]
 [     -1.      35.       0.    -166.      -1.     108.]]
C:
[[ 0. -5. -0.]
 [ 5. -0. -0.]
 [-0.  0.  0.]]
M:
[[ 52733.     -0.      6.]
 [    -0.  52945.     -1.]
 [     6.      0. 197297.]]
I:
[[1214534.       3.    -167.]
 [      3. 1215196.      -2.]
 [   -166.      -1.     108.]]
CMCT:
[[1155500.       3.    -162.]
 [      2. 1154465.      -6.]
 [   -162.      -6.       0.]]
S:
[[2370034.       5.    -329.       0. -247341.       5.]
 [      5. 2369661.      -8.  246735.      -0.     -87.]
 [   -328.      -7.     108.      -1.      35.       0.]
 [      0.  246735.      -1.   52733.      -0.       6.]
 [-247341.      -0.      35.      -0.   52945.      -1.]
 [      5.     -87.       0.       6.       0.  197297.]]

I've used the add mass tensor from the example above - if the calculation is correct then the additional term mMass*C*C.transpose() is the same order as I.

[JoanAguilar]

Revisiting this PR, the major thing that seems to be missing for this to merge is unit tests.

Beyond that, there are a couple of side discussions we probably want to keep track of:

1. I think we cleared out with @srmainwaring (in a separate discussion) that the implementation in this PR is correct, and that the specification of the added mass matrix in an SDF file is always done in the link frame; it is probably for our own benefit to document this somewhere.

2. Per @srmainwaring comment, the addition of added mass results in incorrect gravitational effects. @srmainwaring already proposed a solution in the previously mentioned comment:

One solution may be to have an option to switch gravity off in the core physics engine and add it back in via a system plugin that adds the gravity force for each link using the mass matrix mass?

[codecov]

Codecov Report

Merging #384 (e4dcbc0) into gz-physics6 (48acd7a) will increase coverage by 0.05%.
The diff coverage is 100.00%.

@@               Coverage Diff               @@
##           gz-physics6     #384      +/-   ##
===============================================
+ Coverage        75.73%   75.79%   +0.05%     
===============================================
  Files              140      140              
  Lines             7039     7056      +17     
===============================================
+ Hits              5331     5348      +17     
  Misses            1708     1708              

Impacted Files	Coverage Δ
dartsim/src/SDFFeatures.cc	64.07% <100.00%> (+1.33%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.