Skip to content

Commit

Permalink
Linear potential wave-body model demo (#70)
Browse files Browse the repository at this point in the history 
* Wave Body: Add linear potential wave-body model

Ellipsoid Buoy: add model for an ellipsoid buoy

- Add test model for hydrodynamics models.

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

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

- Initial version of plugin with hydrostatic restoring forces.

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

Wave-Body: update linear wave-body interaction model

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

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

Wave-Body: update linear wave-body interaction model

- update comments and disable debug output

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

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

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

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

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)

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

Wave-Body: use loaded hydro data in calculations

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

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

Wave-Body: use loaded hydro data in calculations

- Clean up.

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

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)

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

Wave-Body: split update into contributions by force type

- Split out hydrostatic force calculation.

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

Wave-Body: add parameter for hydro data file

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

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

Wave-Body: document the hydrostatics calculation

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

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

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

Wave-Body: add radiation damping test case

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

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

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

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

Wave-Body: add force control parameters to ellipsoid example

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

Wave-Body: initial version of radiation added mass force

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

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

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.

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

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

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

Wave-Body: replace hardcoded radiation damping override with parameters

- Allow the radiation damping to be set in parameters

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

Wave-Body:

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

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

Fluid added mass: update added mass example

- Clean and symmetrise matrix

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

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

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

Fluid added mass: add flags for additional debug info

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

Fluid added mass: cleanup

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

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

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

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

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

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

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

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.

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

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.

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

Wave Body: update example model and add notes

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

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

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

Wave Body: configure buoy example to use all forces

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

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

Wave Body: refactor flags and publishers

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

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

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.

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

Wave Body: add geometry overrides

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

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

Wave Body Tests: add wave models for test cases

- Add regular wave models with different periods and amplitudes.

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

Wave Body: fix geometry overrides

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

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

Wave Body: use waves test model

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

Wave Body: revert waves.sdf to original

- Move ellipsoid test to new world file.

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

Wave Body: add world for ellipsoid test case

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

Wave Body Tests: add spheroid test case

- Add spheroid for added mass Test1a

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

Wave Body Tests: update spheroid test case

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

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

Wave Body: improve overrides - 1

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

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

Wave Body: improve overrides - 2

- Move HDF5 reader to separate function.

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

Wave Excitation: fix error in torque calculation

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

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

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.

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

Wave Body: improve overrides - 3

- Refactor duplicated code in array read functions.

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

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>

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

Wave Body: improve overrides - 5

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

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

Wave Body: improve overrides - 6

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

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

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.

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

* ROS: add a project to publish gazebo messages to ros2

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

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

- Add body_response_publisher
- Update launch file

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

ROS: update README

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

ROS: correct topic name for excitation force

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

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Nov 30, 2022
1 parent c7522e4 commit 1a0da15
Show file tree
Hide file tree
Showing 43 changed files with 25,477 additions and 0 deletions.
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 1a0da15

Please sign in to comment.