Skip to content

Commit

Permalink
Wave-Body: Linear potential wave-body model
Browse files Browse the repository at this point in the history 
This is squashed history of the linear potential wave-body model.

1. Linear potential wave-body model demo (#70)

1.1. Wave Body: Add linear potential wave-body model

- Add model for an ellipsoid buoy
- Add test model for hydrodynamics models.

1.2. Wave-Body: add plugin for testing linear wave-body interaction models

- Initial version of plugin with hydrostatic restoring forces.

1.3. Wave-Body: update linear wave-body interaction model

- Add contributions to moments from hydrostatic equilibrium term (centre
of buoyancy).

1.4. Wave-Body: update linear wave-body interaction model

- Update comments and disable debug output

1.5. Wave-Body: update models for testing linear wave-body interaction model

- Set wave amplitudes to zero and compare the two hydrostatic models

1.6. Wave-Body: add hydro data file and load into plugin

- Add BEM hydro data .hdf5 file for ellipsoid (generated by capytaine / bemio)
- Add dependency on Eigen. HDF5 and HighFive to read data file (HighFive should be cloned into to workspace)

1.7. Wave-Body: use loaded hydro data in calculations

- Store loaded hydro data in a struct and replace hardcoded data.

1.8. Wave-Body: use loaded hydro data in calculations

- Clean up.

1.9. Wave-Body: load remaining hydro data

- Complete loading of all hydro data (except for state space model data)
- Load routines can be factored out and consolidated (2nd pass)

1.10. Wave-Body: split update into contributions by force type

- Split out hydrostatic force calculation.

1.11. Wave-Body: add parameter for hydro data file

- Add parameter <hydrodata> for specifying HDF5 file.
- Move model specific config under model folder.

1.12. Wave-Body: document the hydrostatics calculation

1.13. Wave-Body: use waterplane origin as reference point for displacements

1.14. Wave-Body: add radiation damping test case

- Update variable labelling for pose and vectors
- Add radiation damping test case example (hardcoded)

1.15. Wave-Body: add parameters to control which forces are calculated

- Add flags to control which force contributions are included
- Format hdf5 file reader
- Make hydrostatics forces switchable

1.16. Wave-Body: add force control parameters to ellipsoid example

1.17. Wave-Body: initial version of radiation added mass force

- Reorganise code to enable switching forces on / off
- Outline added mass calculation

1.18. Wave-Body: updated version of radiation added mass force

- Investigate a couple of methods - both approaches to capture acceleration in the force contribution are unstable.
- Alternative methods is to use the SetFluidAddedMass method of the inertial.
- This can be set on the component attached to the link entity in the plugin config, and this appears to propagate to the physics engine.

1.19. Wave-Body: replace hardcoded added mass override with parameters

- Allow the added mass and radiation damping to be set in parameters
- Remove added mass calculation from update and set inertial in config
- Add example parameters to demo world

1.20. Wave-Body: replace hardcoded radiation damping override with parameters

- Allow the radiation damping to be set in parameters

1.21. Wave-Body: add gravity calculation

- Add aliases for Vector6d and Matrix6d
- Add gravity calculation to use instead of global physics engine gravity
- Required when enabling added mass
- Update added mass calculation to use gazebosim/gz-physics#384

1.22. Fluid added mass: update added mass example

- Clean and symmetrise matrix

1.23. Fluid added mass: add code to symmetrise added mass (disabled)

1.24. Fluid added mass: add flags for additional debug info

1.25. Fluid added mass: cleanup

1.26. Wave Excitation: add parameters for constant coefficient overrides

- Add parameters for storing wave excitation force coefficients
- Add template specialisation to read Eigen::Vector6d
- Add example data to sdf model
- Remove stale code for added mass adjustments from class declaration

1.27. Wave Excitation: implement excitation force for regular waves

- Add overrides for wave period, height and phase
- Update documentation describing added mass calculation
- Implement constant coefficient excitation calculation for regular waves
- Update wave model to used trochoids with steepness = 0 (direction not implemented for sinusoids)
- Rename elements for wave excitation force components

1.28. Wave Excitation: correct buoyancy moment calculation

- Correct the buoyancy moment calculation for larger displacements.
- Add missing parameter read for the flag to enable / disable the excitation force

1.29. Wave Excitation: implement excitation Froude-Krylov and scattering force components

- Add example coefficients for excitation Froude-Krylove and scattering components.
- Read parameters in Config.
- Implement force calculations for new components.
- Initialise vectors and matrices to zero.

1.30. Wave Excitation: rename variables using Kane/monogram notation

- Rename pose, vectors using Kane/monogram notation described in the Drake docs.
- Fix gravity calculation.
- Factor out common link state updates.

1.31. Wave Body: update example model and add notes

1.32. Wave Body: add force publishers

- Publish force and torque if enabled
- Add parameters to enable force publishing
- Update launch script to enable ROS bridge for forces

1.33. Wave Body: configure buoy example to use all forces

- Set initial position to origin for linear potential model example
- Update document with references

1.34. Wave Body: refactor flags and publishers

- Group flags into structs to reduce clutter.
- Move debug flags into their own SDF element.

1.35. Wave Body: refactor hydro coefficient overrides

- Group coefficients into structs to reduce clutter.
- Reorganise hydro coefficient elements in SDF.
- Move waves and sim environment params into separate structs.
- Add override for hydrostatic linear restoring.

1.36. Wave Body: add geometry overrides

- Add section for geometry overrides.
- Rename the initial pose the body waterplane.

1.37. Wave Body Tests: add wave models for test cases

- Add regular wave models with different periods and amplitudes.

1.38. Wave Body: fix geometry overrides

- Fix issue with parsing SDF and ensure SDF example valid.

1.39. Wave Body: use waves test model

1.40. Wave Body: revert waves.sdf to original

- Move ellipsoid test to new world file.

1.41. Wave Body: add world for ellipsoid test case

1.42. Wave Body Tests: add spheroid test case

- Add spheroid for added mass Test1a

1.42. Wave Body Tests: update spheroid test case

- Add ROS launch file for tests.
- Update BEM coeffs from WEC-Sim example

1.43. Wave Body: improve overrides - 1

- Rename hydro coefficient class.
- Handle override for hydrostatic restoring.

1.44. Wave Body: improve overrides - 2

- Move HDF5 reader to separate function.

1.45. Wave Excitation: fix error in torque calculation

- Fix indexing error in torque lookup.
- Shorten spatial force names.

1.46. Wave Body Hydrostatics: update treatment of CoB offset from CoM

- Use the initial offset of the CoB from CoM in updates - tricky as CoB is dynamic but linear model appears to rely on this being at initial position.

1.47 Wave Body: improve overrides - 3

- Refactor duplicated code in array read functions.

1.48. Wave Body: improve overrides - 4

- Refactor SDF element names.
- Change <hydrodata> to <hdf5_file> as the element is a file name.
- Move <waves> up a level.
- Change <environment> to <simulation_parameters>
- Shorten <force_publishers> to <publishers>

1.49. Wave Body: improve overrides - 5

- Add notes on model and WEC-Sim data structures and modelling.

1.50. Wave Body: improve overrides - 6

- Name refactoring - preparation for introducing new structs for data that will be used in updates.

1.51. Wave Body: improve overrides - 7

- Add improved handling of overrides.
- Remove all override decision making from update loops.
- Add separate data structure to contain the hydro force coefficients used in the update loop.
- Document the load and override policy.

1.52. ROS: add a project to publish gazebo messages to ros2

1.53. ROS: add node to publish body response labelled using maritime conventions

- Add body_response_publisher
- Update launch file

1.54. ROS: update README

1.55. ROS: correct topic name for excitation force

Signed-off-by: Rhys Mainwaring <[email protected]>

2. Wave Body: improve overrides - 8 (#71)

- Remove hardcoded frequency index used to lookup hydro coeffs from hdf5 data.
- Coefficients from hdf5 are linearly interpolated and scaled given the frequency in the simulation params.

Signed-off-by: Rhys Mainwaring <[email protected]>

3. Wave Body: use model:// URI to specify BEM file location (#72)

- Use model://<model_path_to_config> URI syntax to locate BEM data files.

Signed-off-by: Rhys Mainwaring <[email protected]>

4. Wave Body: Update example regular wave models (#73)

4.1  Wave Body: rename regular wave example models to refer to wave height rather than amplitude

- Use wave height in linear wave-body model SDF and in the names of the example regular wave models.

4.2  Wave Body: add extra regular wave models to cover spectrum

- Add low and high frequency examples.

4.3. Wave Body: reset wave model used by examples

- Set examples to use regular_waves_6s_2m model - this matches the override hydro coefficients.

Signed-off-by: Rhys Mainwaring <[email protected]>

5. Wave Body: add checks when reading the hdf5 file (#74)

5.1. Wave Body: add checks when reading the hdf5 file

- Check that datasets are present before loading.
- Check that data is not empty before attempting to display.

5.2. Wave Body: update comments to clarify scaled means non-dimensioned

Signed-off-by: Rhys Mainwaring <[email protected]>

6. Wave Body: add publisher for added mass force (#75)

- Add publisher for estimated added mass force.

Signed-off-by: Rhys Mainwaring <[email protected]>

7. Wave Body MBARI: add MBARI buoy example for testing (#76)

- Add test model for the MBARI WEC buoy (float only)

Signed-off-by: Rhys Mainwaring <[email protected]>

8. Wave Body MBARI: add collision mesh and change default BEM file (#77)

8.1. Wave Body MBARI: add collision mesh for buoy

- Add simplified collision meshes for the buoy at different levels of refinement.
- Use the mesh with 316 faces in the model.
- Add BEM file generated using Capytaine and the 5548 face mesh.

8.2. Wave Body MBARI: change default the BEM data file

- The excitation data in the hdf5 file generated from WAMIT data has the wrong sign, use Capytaine / bemio generated file in the meanwhile.
- Simplify ramp function.

Signed-off-by: Rhys Mainwaring <[email protected]>

9. Wave Body MBARI: fix ramp function missing bracket (#78)

Signed-off-by: Rhys Mainwaring <[email protected]>

10. Wave Body: don't use auto with Eigen (#79)

Signed-off-by: Rhys Mainwaring <[email protected]>

11. Wave Body: update cmake dependencies

- Use FetchContent to retrieve dependency HighFive.
- Fix typo in documentation.
- Remove cpp style header for mlinterp

11.1 Wave Body: update ci workflow

- Add build flag for HighFive dependency

11.2. Wave Body: update waves bridge tests

- Remove breaking tests inherited from ROS project template.

11.3.Wave Body: post rebase fixes.

11.4. Wave-Body: fix config of regular_waves_3s_1m

11.5. Wave-Body: revert changes to regular_waves model

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Jan 26, 2023
1 parent c997d60 commit f3ea3ec
Show file tree
Hide file tree
Showing 71 changed files with 36,388 additions and 2 deletions.
2 changes: 1 addition & 1 deletion .github/scripts/macos_build.sh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,4 +13,4 @@ export CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:/usr/local/opt/qt@5
# Python scripts installed with: `pip3 install --user <package>`
export PATH=$PATH:$HOME/Library/Python/3.11/bin

colcon build --symlink-install --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBUILD_TESTING=ON -DCMAKE_CXX_STANDARD=17 -DCMAKE_MACOSX_RPATH=FALSE -DCMAKE_INSTALL_NAME_DIR=$(pwd)/install/lib
colcon build --symlink-install --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBUILD_TESTING=ON -DCMAKE_CXX_STANDARD=17 -DCMAKE_MACOSX_RPATH=FALSE -DCMAKE_INSTALL_NAME_DIR=$(pwd)/install/lib -DHIGHFIVE_USE_EIGEN=ON
2 changes: 1 addition & 1 deletion .github/workflows/ubuntu-jammy-ci.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ jobs:
- name: Build Wave Sim
run: |
colcon build --symlink-install --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_STANDARD=17 -DBUILD_TESTING=ON
colcon build --symlink-install --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_STANDARD=17 -DBUILD_TESTING=ON -DHIGHFIVE_USE_EIGEN=ON
- name: Test Wave Sim
run: |
Expand Down
185 changes: 185 additions & 0 deletions AddedMass.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,185 @@
# Gazebo / DART inertia conversion

An explanation of the Gazebo to DART inertial conversion and the different
conventions used by each system. The conclusion is that there is a missing
term when populating the DART spatial tensor with added mass because the
moments of inertia are taken about different axis.

## Drake labelling conventions

- https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
- https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__inertia.html

### Frame labels

- `W` - world frame.
- `B` - body frame.
- `Bcm` - body CoM frame.

### Point labels

- `Bo` - body origin.
- `Bcm` - body center of mass.

### Pose

- `X_WB` - the pose of the body frame `B` in the world frame `W`.
- `X_BBcm` - the pose of the body CoM frame `Bcm` in the body frame `B`.


### Inertia matrix

The inertia matrix, taken about a point `P`, expressed in frame `F`.

- `I_BBcm_Bcm` - inertia matrix of body `B` about the body center of mass `Bcm` in frame `Bcm`
- `I_BBcm_B` - inertia matrix of body `B` about the body center of mass `Bcm` in frame `B`
- `I_BBo_B` - inertia matrix of body `B` about the body origin `Bo` in frame `B`

### Position vectors

- `c_BoBcm_B` position vector from the body origin `Bo` to the body center of mass `Bcm` in the body frame `B`.


## Gazebo inertial object

The SDF documentation http://sdformat.org/spec?ver=1.9&elem=link#inertial_inertia for the `<inertia>` element states:


> Description: This link's moments of inertia ixx, iyy, izz and products of inertia ixy, ixz, iyz about Co
> (the link's center of mass) for the unit vectors Ĉx, Ĉy, Ĉᴢ fixed in the center-of-mass-frame C.
This means that the object `gz::math::MassMatrix3` contains:

- `m` - the scalar mass.
- `I_BBcm_Bcm` - inertia matrix of body `B` about the body center of mass `Bcm` in frame `Bcm`.

This is used to construct the object `gz::math::Inertial` which is the inertial taken about point `Bcm`
for body `B` in frame `F`. Since the pose from SDFFormat is `X_BBcm`, the object stores internally:

- `m`
- `I_BBcm_Bcm`
- `X_BBcm`

The functions return the following:

- `MassMatrix -> (m, I_BBcm_Bcm)`
- `Pose -> (X_BBcm)`
- `Moi -> (I_BBcm_B = X_BBcm.R * I_BBcm_Bcm * X_BBcm.R^T)`

The key point to note is that when accessing the the moment of inertia it is transformed
to the body frame `B` but is taken about a point located at the CoM `P = Bcm`.

## DART spatial tensor

The Gazebo function we are interested in is: `gz::physics::dartsim::SDFFeatures::ConstructSdfLink`
where `gz::math::Inertial` is translated into `dart::dynamics::Inertia`.

### 1. First examine the case when there is no fluid added mass:

```c++
const gz::math::Inertiald &sdfInertia = _sdfLink.Inertial();
bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass());

const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi());

bodyProperties.mInertia.setMoment(I_link);

const Eigen::Vector3d localCom =
math::eigen3::convert(sdfInertia.Pose().Pos());

bodyProperties.mInertia.setLocalCOM(localCom);
```
Break down what is going on:
- Set `m` - the scalar mass `m`.
- Set `I_BBcm_B` - the moment of inertia of the body `B`, taken about the body CoM `Bcm` in frame `B`.
- Set `c_BoBcm_B` - position vector from the body origin `Bo` to the body center of mass `Bcm` in the body frame `B`.
From these inputs DART, calculates the spatial tensor for rigid body rotations about the **body origin** `Bo`.
This is the spatial inertial tensor from Roy Featherstone, Rigid Body Dynamics Algorithms, §2.13, p33, Eq(2.63) Springer, 2008. Internally DART computes the spatial tensor to have the following elements:
- `TL = I_BBo_B = I_BBcm_B + m * cx * cx^T`
- `TR = m * cx`
- `BL = m * cx^T`
- `BR = m * Identity(3)`
where `cx` is the skew-symmetric operator created from `c_BoBcm_B`.
Compare this to the definition for the Gazebo body matrix returned by: `gz::math::Inertial::BodyMatrix`:
- `BR = I_BBcm_B
- `BL = m * cx`
- `TR = m * cx^T`
- `TL = m * Identity(3)`
which is for rigid body rotations about the **body center of mass** `Bm`.
Now in this case, the different convention for the axis of rotation does not matter because Gazebo does not use
the `BodyMatrix` function to set the DART spatial inertial tensor, and the change of axis is calculated internally
by DART. However this is not the case when added mass is considered.
### 2. With fluid added mass
In this case the DART spatial inertial tensor is first calculated as in case 1., then a spatial inertial tensor
for the fluid added mass is calculated and the sum of the two is set using
`dart::dynamics::Inertia::setSpatialTensor`:
```c++
bodyProperties.mInertia.setSpatialTensor(
bodyProperties.mInertia.getSpatialTensor() +
math::eigen3::convert(featherstoneMatrix)
```

So the important note here is that the featherstoneMatrix must be for rotations
about the body orgin `Bo` and with respect to the body frame `B`.


While not clearly documented in the Gazebo class, it is suggested by the function:

```c++
/// \brief Spatial mass matrix, which includes the body's inertia, as well
/// as the inertia of the fluid that is dislocated when the body moves.
/// The matrix is expressed in the object's frame F, not to be confused
/// with the center of mass frame Bi.
/// \return The spatial mass matrix.
/// \sa BodyMatrix
/// \sa FluidAddedMass
public: Matrix6<T> SpatialMatrix() const
{
return this->addedMass.has_value() ?
this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
}
```
that the Gazebo fluid added mass is expected to be computed in the body frame `B` for rotations about
the body center of mass `Bcm` (because the quantities are being summed, and this is consistent with
the other interfaces to this class that return moment of intertia etc.)
## Conclusion
The above suggests that there is a term missing in the current calculation of the featherstone matrix
in `gz::physics::dartsim::SDFFeatures::ConstructSdfLink` to account for the change of axis
from `Bcm` to `Bo`.
For fluid added mass the mass matrix is not diagonal with equal entries, so we can't calculate
`m * cx * cx^T` as for the inertial term. My suggestion is to use:
```
I_BBo_B = I_BBcm_B + cx * M * cx^T
```
and deduce `CMCT = cx * M * cx^T` using:
```c++
auto M = sdfInertia.FluidAddedMass().value().Submatrix(
math::Matrix6d::TOP_LEFT);
auto CM = sdfInertia.FluidAddedMass().value().Submatrix(
math::Matrix6d::BOTTOM_LEFT);
auto invM = M.Inverse();
auto C = CM * invM;
auto CMCT = CM * C.Transposed();
```
Loading

0 comments on commit f3ea3ec

Please sign in to comment.