Skip to content

Commit

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

* 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]>

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]>

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

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]>

Update example regular wave models (#73)

* 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.

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

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

- Add low and high frequency examples.

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

* 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]>

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

* 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.

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

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

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

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

- Add publisher for estimated added mass force.

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

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]>

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

* 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.

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

* 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]>

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

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

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

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

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

Merge (#80) into demo/linear-wave-body  (#81)

* Waves: enable wave direction in the sinusoid wave model (#80)

* Waves: enable wave direction in the sinusoid wave model

- Allow wave direction to be set for regular waves.
- Add test for a wave direction not parallel to the x-axis

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

* Waves: rename monochromatic_waves model

- Rename to regular waves.

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

* Waves: update regular wave model descriptions

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

Merge branch 'master' into demo/linear-wave-body (#83)

* Waves: enable wave direction in the sinusoid wave model (#80)

* Waves: enable wave direction in the sinusoid wave model

- Allow wave direction to be set for regular waves.
- Add test for a wave direction not parallel to the x-axis

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

* Waves: rename monochromatic_waves model

- Rename to regular waves.

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

* Waves simulation optimisation - part 1 (#82)

* Waves: add classes for spreading functions

- Add classes for directional spreading functions.
- Add vectorised calculations using Eigen.

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

* Waves: update classes for spreading functions

- Update classes for directional spreading functions - use Eigen::MatrixXd rather than VectorXd.
- Update tests including broadcasting to arrays.

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

* Waves: add classes for omni-directional spectrum functions

- Add classes for omni-directional wave spectra.
- Add test case for vectorised spectrum calculation using Eigen.
- Add regression test and check limits (k = 0, u19 = 0)

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

* Waves: update classes for omni-directional spectrum functions

- Implement ECVK omni-directional spectra.
- Add test case for vectorised spectrum calculation using Eigen.
- Add regression test and check limits (k = 0, u10 = 0)

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

* Waves: add regression tests for spectra.

- Add regression test against spectrum calculation used in  FFT2.
- Make spectrum functions in FFT2 static.
- Add ECKV spectrum tests.

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

* Waves: fix spreading function test.

- Fix segfault caused by missing arg.

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

* Waves: remove unused wave simulation classes

- Remove FFTW and OpenCL simulation classes.

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

* Waves: reset test values after physical constant updates.

- Update buoyancy regression values after fluid density and gravity value update.

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

* Waves: fix failing static test in gz_waves_bridge

- Disable flake8 and fix docstring.

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

* Waves: replace std::vector with Eigen in WaveSimulation

- Migration from std::vector to Eigen::VectorXf. Not efficient as involved heap allocation each update.
- Replace std::vector for interface calls in OceanTile.
- Remove unused code from WaveSpectrum.

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

* Waves: fixes post cherry-pick

- Revert interface change in WaveSimulation.

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

* Waves: replace std::vector with Eigen in WaveSimulation

- Remove interface using std::vector<double> from WaveSimulation classes.
- Update waves plugin classes to use new interfaces.
- Update test

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

* Waves: replace std::vector with Eigen in WaveSimulation

- Remove internal interface using std::vector<double> from WaveSimulation classes.
- Update tests

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

* Waves: replace std::vector with Eigen in FFT WaveSimulation

- Remove internal interface using std::vector<double> from WaveSimulation classes.

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

* Waves: correct typos in derivative calculations for sinusoid and
trochoid waves

- Ensure Eigen vector workspace is initialised in WavesVisual.

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

* Waves: visual lighting workaround

- Tangent space calculation not correct - environment mapping looks incorrect unless dhdx <---> dhdx?

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

* Waves: update sinusoid wave interface

- Allow x and y to be set independently in wave grid.
- Streamline class declarations.
- Update tests.
- Add vectorise option for sinusoid waves.

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

* Waves: remove unused functions.

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

* Waves: add tests for vectorised sinusoid wave sim

- Cross check vectorised code against non-vectorised version.

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

* Waves: remove unused include from WaveSimulationSinusoid

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

* Waves: refactor WaveSimulation method names.

- Rename ComputeHeights to ComputeElevation.

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

* Waves: refactor update indentation formatting.

- Add vectorisation method to FFT wave simulation.

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

* Waves: refactor update member variable names in FFT wave simulation

- Standardise caps convention for naming member variables (using underscores).
- Update interface to allow different nx and ny for grid.

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

* Waves: temp workaround for wave derivatives used in lighting tangent space

- Workaround mixed conventions for matrix-ij vs matrix-xy storage in tangent space calcs.

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

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

* Waves simulation optimisation - part 2 (#84)

* Waves: update FFT wave simulation tests for non-square grids (nx != ny)

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

* Waves: apply google coding standards

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

* Waves: apply google coding standards

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

* Waves: apply google coding standards

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

* Waves: apply google coding standards

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

* Waves: apply google coding standards

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

* Waves: apply google coding standards

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

* Waves: update FFT test namesWaves

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

* Waves: preparation for vectorising FFT wave simulation update

- Add placeholders for vectorised versions of functions
- Replace hardcoded gravity constants with params.

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

* Waves: preparation for vectorising FFT wave simulation update

- Add cross-check tests for vectorised versions of base and current amplitude calculations.

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

* Waves: progress vectorising FFT wave simulation update

- Change capitalisation of Cos2sSpreadingFunction.
- Update comments in WaveSpectrum.
- WaveSimulationFFT
 	- Consolidate initialisation code.
	- Update variable names (nyquist)
	- Delay allocation of storage for Eigen member variables.
	- Update tests to use nearest for cross checks on vectorised version.
	- Remove use of auto for Eigen references (use Eigen::Ref instead)
	- Factor out wavenumber initialisation.
	- Partial vectorisation.

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

* Waves: vectorise base amplitude calc for FFT wave simulation

- Leave initialisation of IID random normals un-vectorised for testing against original versions.

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

* Waves: add unit test for Eigen and FFTW

- Add test skeleton.

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

* Waves: add unit test for Eigen and FFTW

- Add examples using different storage choices.

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

* Waves: add unit test for Eigen and FFTW

- Add examples for row vs column major storage.

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

* Waves: add Eigen alignment macro

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

* Waves: use Eigen types for FFTW storage - reduce copies

- Use Eigen types instead of fftw_complex* for FFT workspace.

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

* Waves: remove unused code from FFWT wave simulation

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

* Waves: use row-major Eigen matrix storage for FFT wave simulation

- Intermediate step moving from Eigen::VectorXcd to Eigen::MatrixXcdRowMajor.
- FFTW requires row-major ordering, Eigen is column major by default.

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

* Waves: use row-major Eigen matrix storage for FFT wave simulation

- Change to matrix indexing in FFT wave storage (from a single column).

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

* Waves: update output mapping from row to col major storage in FFT wave simulation

- Replace loops with vector calls using reshaped to flatten.

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

* Set up CI using GitHub Actions (#85)

- CI: remove travis integration
- Add codeowners and gazebo ci.yml
- Minimal version - checkout only.
- Change checkout actions/checkout@v3
- Add CI workflows for ubuntu and macos
- Rename and split macOS into separate workflow.
- Install Gazebo Garden.
- Test caching
- Add brew install script.
- Add build script.
- Update macos-ci
- Add test step.
- Add GitHub Actions status badge, remove travis badges.
- Update ci install scripts
- Update the brew installation script to force relinking on all dependencies of the installed packages.
- Fix relink step.
- Update ubunti-ci workflow.
- Add install, build and test steps.
- Rename macos scripts.
- Update ubunti-ci workflow.
- Install additional build dependencies.
- Update ubunti-ci workflow.
- Fix colcon build command line.
- Move macOS to colcon build.
- Update workflow for ubuntu 20.04
- Ensure cgal etc are linked as well as deps.
- Split Ubuntu jobs to separate workflow
- Update macos workflow - add build tools
- Fix EigenFFTW breaking on Ubuntu.
- Update macos workflow - reenable build and test
- Fix timer in CGAL test and disable triangulation tests failing on Ubuntu.
- Fix errors in WaveSimulationFFT2 tests and set tolerances for variance comparisons.
- Check colcon install on macOS workflow
- Disable trochoid wave test
- Investigate why colcon not found in macOS CI workflow
- Add workflow to check build tools are installed.
- Use pip3
- Use full path to colcon
- Add pip install checks
- Update path to Python scripts
- Update path to Python scripts
- Pip install colcon and vcstool with --user flag

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

* Waves simulation optimisation - part 3 (#87)

* Waves: optimise current amplitude calculation for FFT wave simulation

- Investigate optimisation options for the current amplitude calculation.
- vectorised assignment to fft worlspace is not faster than a single loop.
- pre-calculate the fourier amplitude coefficients (reduce index lookup) - marginal gain?

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

* Waves: label indices in ocean tile to distinguish between vertex indices and wave indices

- Vertex array index: v_idx_cm: v - vertex, cm - column major
- Wave array index: w_idx_cm: v - vertex, cm - column major
- Wave array index: w_idx_rm: v - vertex, rm - row major

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

* CI: only run macOS workflow on PR

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

* Waves: rename FFT wave simulation class

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

* Waves: duplicate FFT wave simulation class to factor out reference version

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

* Waves: duplicate FFT wave simulation class to factor out reference version

- Complete split of reference implementation into separate class.
- Remove unused code from the separated FFT wave simulation classes.

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

* Waves: remove commented code

- Remove code referring to reference versions of the spectrum calculations.

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

* Waves: add performance checks for spectrum and FFT amplitude calculations

- Initial performance tests. Contain debugging and timing info.
- Eigen component-wise array versions are not performant...

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

* Waves: replace slow array versions of spectrum calcs.

- Eigen component-wise array versions are not performant - replace with calls using std::transform on unary / binary versions.

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

* Waves: refactor wave simulation and spectrum test names

- Functions are not truly vectorised - use Eigen array type to label tests instead.

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

* Waves: satisfy Eigen check that range iterators in STL algorithms are from the same expression.

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

* Waves simulation optimisation - part 4 (#88)

* Waves: restructure wave spectrum performance test for instrumenting

- Move initialisation into test fixture

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

* Waves: remove c-wise amplitude update

- Remove inefficient component wise amplitude update and associated data.

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

* Waves: remove c-wise cos / sin update in current amplitude calc

- Remove inefficient component wise amplitude update.

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

* Waves: restructure FFT wave amplitude performance test for instrumenting

- Move initialisation into test fixture.

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

* Waves: update FFT wave simulation

- Consolidate storage (shape depends whether loop is flattened or not).
-  Make test fixture class names consistent.
- Remove array allocation in current amplitude update

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

* Waves: use previously calculated wave numbers

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

* Waves: remove two loops from FFT base amplitude calculation

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

* Waves: remove component-wise array amplitude calculations

- Abandon using c-wise amplitude calculations as the loop based update has better performance.
- Remove unnecessary storage from the FFT class to reduce memory footprint.

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

* Waves: remove WaveSimulationFFTRef from public interface

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

* Waves: add performance checks for elevation and displacement calculations

- Add performance check for main update loop - elevations, displacements and derivatives.

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

* Waves simulation optimisation - part 5 (#89)

* Waves: update FFTW check

- Modify check to transform hermitian amplitudes back to real values.

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

* Waves: create FFTW plans before initialising arrays

- FFTW may overwrite data in arrays while planning. Allocate, plan, initialise.

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

* Waves: use complex to real version of FFTW

- Change outputs to real arrays.
- Change size of input complex arrays to nx * ny/2+1
- Output in Compute* functions is now real.
- Add example in the Eigen FFTW test using a c2r plan.

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

* Waves: use complex to real version of FFTW

- Flattened index must be calculated as loops do not cover full range.

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

* Waves: use complex to real version of FFTW

- Disable hermitian tests for optimised FFT wave sim (arrays truncated using symmetry)

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

* Waves: temp disable tests broken because of switch to real DFT

The following tests require further review - restructuring
- ParsevalsIdentityTimeZero
- ParsevalsIdentityTimeNonZero
- Displacement

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

* Waves: additional Hermitian tests

- Add placeholder tests to check derivative and displacement amplitudes are Hermitian.

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

* Waves: fix Hermitian symmetry for derivatives (#90)

- Correct Nyquist term in derivative amplitudes (must be zero).
- Enable tests to check derivative and displacement amplitudes are Hermitian.

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

* CI: clean build warnings (#91)

* CI: enable all warnings and enforce as errors

- Clean build of all warnings.
- Disable unused variables from tests (primarily present for printing info).
- Comment unused parameters in function sigs.
- Fix int type comparison warnings in loops.
- Fix initialisation order errors in constructors.

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

* CI: enable all warnings and enforce as errors

- Remove GZ_ASSERT raising a warning on Ubuntu

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

* CI: enable all warnings and enforce as errors

- Renaming shadowing variables.

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

* Wave Body: update cmake dependencies

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

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

* Wave Body: update ci workflow

- Add build flag for HighFive dependency

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

* Wave Body: update waves bridge tests

- Remove breaking tests inherited from ROS project template.

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

Wave Body: post rebase fixes.

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
srmainwaring committed Dec 8, 2022
1 parent a1e3ff9 commit b4f7d10
Show file tree
Hide file tree
Showing 73 changed files with 36,391 additions and 11 deletions.
2 changes: 1 addition & 1 deletion .github/scripts/macos_build.sh
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
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
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 b4f7d10

Please sign in to comment.