From 1c419fb1b41c1fc349e6049c12d0909450fe9cee Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 9 Oct 2022 23:48:49 +0100 Subject: [PATCH] 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 Signed-off-by: Rhys Mainwaring --- gz-waves-models/worlds/ellipsoid_buoy.sdf | 4 +- .../linear_wave_body/LinearWaveBody.cc | 211 ++++++++++++++---- .../src/systems/linear_wave_body/mlinterp | 25 +++ .../src/systems/linear_wave_body/mlinterp.hpp | 169 ++++++++++++++ 4 files changed, 363 insertions(+), 46 deletions(-) create mode 100644 gz-waves/src/systems/linear_wave_body/mlinterp create mode 100644 gz-waves/src/systems/linear_wave_body/mlinterp.hpp diff --git a/gz-waves-models/worlds/ellipsoid_buoy.sdf b/gz-waves-models/worlds/ellipsoid_buoy.sdf index 5ed93fca..e335312e 100644 --- a/gz-waves-models/worlds/ellipsoid_buoy.sdf +++ b/gz-waves-models/worlds/ellipsoid_buoy.sdf @@ -3,7 +3,7 @@ 0.001 - -1 + 1 6 - 2 + 4 0 0 diff --git a/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc b/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc index 8d9af781..7229f532 100644 --- a/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc +++ b/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc @@ -51,6 +51,8 @@ #include #include +#include + using namespace gz; using namespace sim; using namespace systems; @@ -143,7 +145,7 @@ class gz::sim::systems::LinearWaveBodyPrivate /// /// 2. Improve pose notation to emphasise frame /// (e.g. Peter Corke's notation). - /// Done. Use Kane/monogram (Drake) notation. + /// Use Kane/monogram (Drake) notation. Done /// /// 3. Set waterplane pose in parameters. Done. /// @@ -172,6 +174,11 @@ class gz::sim::systems::LinearWaveBodyPrivate /// map[forceName] -> { enable, debug, publish, topic, ... } /// and perhaps a bitmasks for switching features on/off /// + /// 10. Interpolate hdf5 date for constant coefficient case. Done. + /// + /// 11. Optimise interpolation to eliminate unnecessary copies. + /// + /// /// \brief WEC-Sim BEMIO hydro data structure (read from HDF5 file) /// @@ -594,6 +601,78 @@ LinearWaveBody::~LinearWaveBody() { } +namespace +{ + /// \todo Optimise to reduce / eliminate copies. + /// The inefficiency is because we read data + /// in Eigen Vectors and Matrices and represent + /// the coefficients as a vector of matrices rathen + /// a matrix of time series suitable for interpolation. + + /// \brief Interpolation helpers + /// + /// \param _Xd data vector + /// \param _Xi interpolation point + /// \param _Yd data vector of matrix + /// \param _Yi interpolation matrix + template + void Interp( + const Eigen::VectorXd &_Xd, + double _Xi, + const std::vector &_Yd, + Matrix *_Yi) + { + // check there is data to interpolate + if (_Xd.size() == 0) + return; + + // get dimensions + size_t nf = _Xd.size(); + size_t n1 = _Yd[0].rows(); + size_t n2 = _Yd[0].cols(); + + // convert to std::vector + std::vector Xdd; + for (size_t k=0; k> Ydd(n1*n2); + for (size_t i=0; iHasElement("hdf5_file")) { - /// \todo look up omega and interpolate - /// index of nearest value of omega in hydro data - size_t iw = 7; - double w = this->dataPtr->hydroData.w[iw]; - /// \note we allow g and rho to override hdf5 values double g = this->dataPtr->simParams.g; double rho = this->dataPtr->simParams.rho; @@ -797,28 +871,69 @@ void LinearWaveBody::Configure(const Entity &_entity, this->dataPtr->hydroForceCoeffs.K_hs = this->dataPtr->hydroData.K_hs * rho * g; - // radiation - this->dataPtr->hydroForceCoeffs.A = - this->dataPtr->hydroData.A[iw] * rho; - - this->dataPtr->hydroForceCoeffs.B = - this->dataPtr->hydroData.B[iw] * rho * w; - - // excitation - this->dataPtr->hydroForceCoeffs.ex_re = - this->dataPtr->hydroData.ex_re[iw] * rho * w; - this->dataPtr->hydroForceCoeffs.ex_im = - this->dataPtr->hydroData.ex_im[iw] * rho * w; - - this->dataPtr->hydroForceCoeffs.fk_re = - this->dataPtr->hydroData.fk_re[iw] * rho * w; - this->dataPtr->hydroForceCoeffs.fk_im = - this->dataPtr->hydroData.fk_im[iw] * rho * w; - - this->dataPtr->hydroForceCoeffs.sc_re = - this->dataPtr->hydroData.sc_re[iw] * rho * w; - this->dataPtr->hydroForceCoeffs.sc_im = - this->dataPtr->hydroData.sc_im[iw] * rho * w; + // interpolate and scale. + double w = this->dataPtr->simParams.w; + + // radiation added mass [Ndof, Ndof, Nf] + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.A, // Yd + &this->dataPtr->hydroForceCoeffs.A // Yi + ); + this->dataPtr->hydroForceCoeffs.A *= rho; + + // radiation damping [Ndof, Ndof, Nf] + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.B, // Yd + &this->dataPtr->hydroForceCoeffs.B // Yi + ); + this->dataPtr->hydroForceCoeffs.B *= rho * w; + + // excitation combined [Ndof, Nh, Nf] + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.ex_re, // Yd + &this->dataPtr->hydroForceCoeffs.ex_re // Yi + ); + this->dataPtr->hydroForceCoeffs.ex_re *= rho * g; + + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.ex_im, // Yd + &this->dataPtr->hydroForceCoeffs.ex_im // Yi + ); + this->dataPtr->hydroForceCoeffs.ex_im *= rho * g; + + // excitation Froude-Krylov [Ndof, Nh, Nf] + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.fk_re, // Yd + &this->dataPtr->hydroForceCoeffs.fk_re // Yi + ); + this->dataPtr->hydroForceCoeffs.fk_re *= rho * g; + + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.fk_im, // Yd + &this->dataPtr->hydroForceCoeffs.fk_im // Yi + ); + this->dataPtr->hydroForceCoeffs.fk_im *= rho * g; + + // excitation scattering [Ndof, Nh, Nf] + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.sc_re, // Yd + &this->dataPtr->hydroForceCoeffs.sc_re // Yi + ); + this->dataPtr->hydroForceCoeffs.sc_re *= rho * g; + + Interp( + this->dataPtr->hydroData.w, w, // Xd, Xi + this->dataPtr->hydroData.sc_im, // Yd + &this->dataPtr->hydroForceCoeffs.sc_im // Yi + ); + this->dataPtr->hydroForceCoeffs.sc_im *= rho * g; } ////////// @@ -1120,9 +1235,8 @@ void LinearWaveBody::Configure(const Entity &_entity, } // display wave parameters - if (this->dataPtr->waveOverrideOn) { - gzmsg << "Wave Overrides\n" + gzmsg << "Wave Parameters\n" << ": " << this->dataPtr->waves.period << "\n" << ": " @@ -1133,10 +1247,22 @@ void LinearWaveBody::Configure(const Entity &_entity, << this->dataPtr->waves.phase << "\n"; } + // display simulation parameters + { + gzmsg << "Simulation Parameters\n" + << ": " + << this->dataPtr->simParams.g << "\n" + << ": " + << this->dataPtr->simParams.rho << "\n" + << "T: " + << this->dataPtr->simParams.T << "\n" + << "w: " + << this->dataPtr->simParams.w << "\n"; + } + // display geometry parameters - if (this->dataPtr->geometryOverrideOn) { - gzmsg << "Geometry Overrides\n" + gzmsg << "Geometry Parameters\n" << ": " << this->dataPtr->geometry.p_BoBwp_B << "\n" << ": " @@ -1146,17 +1272,15 @@ void LinearWaveBody::Configure(const Entity &_entity, } // display hydrostatic parameters - if (this->dataPtr->hydrostaticOverrideOn) { - gzmsg << "Hydrostatic Coefficient Overrides\n" + gzmsg << "Hydrostatic Coefficients\n" << ":\n" << this->dataPtr->hydroForceCoeffs.K_hs << "\n"; } // display radiation parameters - if (this->dataPtr->radiationOverrideOn) { - gzmsg << "Radiation Coefficient Overrides\n" + gzmsg << "Radiation Coefficients\n" << ":\n" << this->dataPtr->hydroForceCoeffs.A << "\n" << ":\n" @@ -1164,21 +1288,20 @@ void LinearWaveBody::Configure(const Entity &_entity, } // display excitation parameters - if (this->dataPtr->excitationOverrideOn) { - gzmsg << "Excitation Coefficient Overrides\n" + gzmsg << "Excitation Coefficients\n" << ":\n" - << this->dataPtr->hydroForceCoeffs.ex_re << "\n" + << this->dataPtr->hydroForceCoeffs.ex_re.transpose() << "\n" << ":\n" - << this->dataPtr->hydroForceCoeffs.ex_im << "\n" + << this->dataPtr->hydroForceCoeffs.ex_im.transpose() << "\n" << ":\n" - << this->dataPtr->hydroForceCoeffs.fk_re << "\n" + << this->dataPtr->hydroForceCoeffs.fk_re.transpose() << "\n" << ":\n" - << this->dataPtr->hydroForceCoeffs.fk_im << "\n" + << this->dataPtr->hydroForceCoeffs.fk_im.transpose() << "\n" << ":\n" - << this->dataPtr->hydroForceCoeffs.sc_re << "\n" + << this->dataPtr->hydroForceCoeffs.sc_re.transpose() << "\n" << ":\n" - << this->dataPtr->hydroForceCoeffs.sc_im << "\n"; + << this->dataPtr->hydroForceCoeffs.sc_im.transpose() << "\n"; } /// Radiation added mass - constant coeffients diff --git a/gz-waves/src/systems/linear_wave_body/mlinterp b/gz-waves/src/systems/linear_wave_body/mlinterp new file mode 100644 index 00000000..79995c62 --- /dev/null +++ b/gz-waves/src/systems/linear_wave_body/mlinterp @@ -0,0 +1,25 @@ +/** + * MIT License + * + * Copyright (c) 2017 Parsiad Azimzadeh + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. +*/ + +#include diff --git a/gz-waves/src/systems/linear_wave_body/mlinterp.hpp b/gz-waves/src/systems/linear_wave_body/mlinterp.hpp new file mode 100644 index 00000000..8fddd7a4 --- /dev/null +++ b/gz-waves/src/systems/linear_wave_body/mlinterp.hpp @@ -0,0 +1,169 @@ +/** + * MIT License + * + * Copyright (c) 2017 Parsiad Azimzadeh + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef MLINTERP_HPP +#define MLINTERP_HPP + +#include +#include +#include + +namespace mlinterp { + +namespace { + +template struct helper { + template void run(const Index *, Index, Index *, T *) {} +}; + +template +struct helper : helper { + const T *xd; + const T *xi; + + helper(T1 xd, T2 xi, Args... args) + : helper(args...), xd(xd), xi(xi) {} + + template + void run(const Index *nd, Index n, Index *indices, T *weights) { + // Must have at least one point per axis + assert(*nd > 0); + + const T x = xi[n]; + + Index mid; + T weight; + + if (*nd == 1 || x <= xd[0]) { + // Data point is less than left boundary + mid = 0; + weight = 1.; + } else if (x >= xd[*nd - 1]) { + // Data point is greater than right boundary + mid = *nd - 2; + weight = 0.; + } else { + // Binary search to find tick + Index lo = 0, hi = *nd - 2; + mid = 0; + weight = 0.; + while (lo <= hi) { + mid = lo + (hi - lo) / 2; + if (x < xd[mid]) { + hi = mid - 1; + } else if (x >= xd[mid + 1]) { + lo = mid + 1; + } else { + weight = (xd[mid + 1] - x) / (xd[mid + 1] - xd[mid]); + break; + } + } + } + + *indices = mid; + *weights = weight; + + helper &base = (*this); + base.run(nd + 1, n, indices + 1, weights + 1); + } +}; + +} // namespace + +struct natord { + template + static Index mux(const Index *nd, const Index *indices) { + Index index = 0, product = 1, i = Dimension - 1; + while (true) { + index += indices[i] * product; + if (i == 0) { + break; + } + product *= nd[i--]; + } + return index; + } +}; + +struct rnatord { + template + static Index mux(const Index *nd, const Index *indices) { + Index index = 0, product = 1, i = 0; + while (true) { + index += indices[i] * product; + if (i == Dimension - 1) { + break; + } + product *= nd[i++]; + } + return index; + } +}; + +template +static void interp(const Index *nd, Index ni, const T *yd, T *yi, + Args... args) { + // Cannot have a negative number of points + assert(ni >= 0); + + // Infer dimension from arguments + static_assert(sizeof...(Args) % 2 == 0, "needs 4+2*Dimension arguments"); + constexpr Index Dimension = sizeof...(Args) / 2; + + // Compute 2^Dimension + constexpr Index Power = 1 << Dimension; + + // Unpack arguments + helper h(args...); + + // Perform interpolation for each point + Index indices[Dimension]; + T weights[Dimension]; + Index buffer[Dimension]; + T factor; + for (Index n = 0; n < ni; ++n) { + yi[n] = 0.; + h.run(nd, n, indices, weights); + for (Index bitstr = 0; bitstr < Power; ++bitstr) { + factor = 1.; + for (Index i = 0; i < Dimension; ++i) { + if (bitstr & (1 << i)) { + buffer[i] = indices[i]; + factor *= weights[i]; + } else { + buffer[i] = indices[i] + 1; + factor *= 1 - weights[i]; + } + } + if (factor > std::numeric_limits::epsilon()) { + const Index k = Order::template mux(nd, buffer); + yi[n] += factor * yd[k]; + } + } + } +} + +} // namespace mlinterp + +#endif