diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 171ca8f021..c3cd80e9c1 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -340,7 +340,11 @@ void Hydrodynamics::Configure( << "\thttps://github.com/gazebosim/gz-sim/pull/1888" << std::endl; } + // Added mass according to Fossen's equations (p 37) + // Note: Adding added mass here is deprecated and will be removed in + // Gazebo J as this formulation has instabilities. + bool addedMassSpecified = false; this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6); for(auto i = 0; i < 6; i++) { @@ -350,12 +354,24 @@ void Hydrodynamics::Configure( prefix += "Dot"; prefix += snameConventionVel[j]; this->dataPtr->Ma(i, j) = SdfParamDouble(_sdf, prefix, 0); + addedMassSpecified = (std::abs(this->dataPtr->Ma(i, j)) > 1e-6) + && addedMassSpecified; } } _sdf->Get("disable_coriolis", this->dataPtr->disableCoriolis, false); _sdf->Get("disable_added_mass", this->dataPtr->disableAddedMass, false); - + if (!this->dataPtr->disableAddedMass || addedMassSpecified) + { + gzerr << "The use of added mass through this plugin is deprecated and will" + << "be removed in Gazebo J* as this formulation has instabilities." + << " We recommend using the SDF `` tag based method" + << "[http://sdformat.org/spec?ver=1.11&elem=link" + << "#inertial_fluid_added_mass]" + << "To get rid of this warning we recommend setting" + << "` to true." + << std::endl; + } // Create model object, to access convenient functions auto model = gz::sim::Model(_entity); diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index 1ada82b467..42d960a0ce 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -46,12 +46,12 @@ namespace systems /// quadratic drag and coriolis force. /// /// ### Diagonal terms: - /// * - Added mass in x direction [kg] - /// * - Added mass in y direction [kg] - /// * - Added mass in z direction [kg] - /// * - Added mass in roll direction [kgm^2] - /// * - Added mass in pitch direction [kgm^2] - /// * - Added mass in yaw direction [kgm^2] + /// * - (Deprecated) Added mass in x direction [kg] + /// * - (Deprecated) Added mass in y direction [kg] + /// * - (Deprecated) Added mass in z direction [kg] + /// * - (Deprecated) Added mass in roll direction [kgm^2] + /// * - (Deprecated) Added mass in pitch direction [kgm^2] + /// * - (Deprecated) Added mass in yaw direction [kgm^2] /// * - Quadratic damping, 2nd order, x component [kg/m] /// * - Linear damping, 1st order, x component [kg] /// * - Quadratic damping, 2nd order, y component [kg/m] @@ -70,10 +70,13 @@ namespace systems /// non-diagonal sides. We use the SNAMe convention of naming search terms. /// (x, y, z) correspond to the respective axis. (k, m, n) correspond to /// roll, pitch and yaw. Similarly U, V, W represent velocity vectors in - /// X, Y and Z axis while P, Q, R representangular velocity in roll, pitch + /// X, Y and Z axis while P, Q, R represent angular velocity in roll, pitch /// and yaw axis respectively. /// * Added Mass: <{x|y|z|k|m|n}Dot{U|V|W|P|Q|R}> e.g. - /// Units are either kg or kgm^2 depending on the choice of terms. + /// (Deprecated) Units are either kg or kgm^2 depending on the + /// choice of terms. You should use the sdf method based spec + // for `fluid_added_mass`: + /// http://sdformat.org/spec?ver=1.11&elem=link#inertial_fluid_added_mass /// * Quadratic Damping With abs term (this is probably what you want): /// <{x|y|z|k|m|n}{U|V|W|P|Q|R}abs{U|V|W|P|Q|R}> /// e.g.