Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adds support for hydrodynamic cross terms #1784

Merged
merged 4 commits into from
Nov 14, 2022
Merged

Adds support for hydrodynamic cross terms #1784

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c9f030c
Adds support for hydrodynamic cross terms
arjo129 Nov 7, 2022
c85281f
Docs
arjo129 Nov 9, 2022
93cd67f
Address PR feedback
arjo129 Nov 14, 2022
5692ab7
Whoops missed one
arjo129 Nov 14, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
224 changes: 101 additions & 123 deletions src/systems/hydrodynamics/Hydrodynamics.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,60 +42,16 @@ using namespace systems;
/// \brief Private Hydrodynamics data class.
class ignition::gazebo::systems::HydrodynamicsPrivateData
{
/// \brief Values to set via Plugin Parameters.
/// Plugin Parameter: Added mass in surge, X_\dot{u}.
public: double paramXdotU;
/// \brief Contains the quadratic stability derivatives like $X_{uv}$,
/// Y_{vv}, Y_{wv} etc.
public: double stabilityQuadraticDerivative[216];

/// \brief Plugin Parameter: Added mass in sway, Y_\dot{v}.
public: double paramYdotV;
/// \brief Contains the quadratic stability derivatives like $X_{u|u|},
/// Y_{v|v|}, Y_{w|v|}$ etc.
public: double stabilityQuadraticAbsDerivative[216];

/// \brief Plugin Parameter: Added mass in heave, Z_\dot{w}.
public: double paramZdotW;

/// \brief Plugin Parameter: Added mass in roll, K_\dot{p}.
public: double paramKdotP;

/// \brief Plugin Parameter: Added mass in pitch, M_\dot{q}.
public: double paramMdotQ;

/// \brief Plugin Parameter: Added mass in yaw, N_\dot{r}.
public: double paramNdotR;

/// \brief Plugin Parameter: Linear drag in surge.
public: double paramXu;

/// \brief Plugin Parameter: Quadratic drag in surge.
public: double paramXuu;

/// \brief Plugin Parameter: Linear drag in sway.
public: double paramYv;

/// \brief Plugin Parameter: Quadratic drag in sway.
public: double paramYvv;

/// \brief Plugin Parameter: Linear drag in heave.
public: double paramZw;

/// \brief Plugin Parameter: Quadratic drag in heave.
public: double paramZww;

/// \brief Plugin Parameter: Linear drag in roll.
public: double paramKp;

/// \brief Plugin Parameter: Quadratic drag in roll.
public: double paramKpp;

/// \brief Plugin Parameter: Linear drag in pitch.
public: double paramMq;

/// \brief Plugin Parameter: Quadratic drag in pitch.
public: double paramMqq;

/// \brief Plugin Parameter: Linear drag in yaw.
public: double paramNr;

/// \brief Plugin Parameter: Quadratic drag in yaw.
public: double paramNrr;
/// \brief Contains the linear stability derivatives like $X_u, Y_v,$ etc.
public: double stabilityLinearTerms[36] = {0};

/// \brief Water density [kg/m^3].
public: double waterDensity;
Expand All @@ -121,7 +77,9 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData
/// \brief Previous state.
public: Eigen::VectorXd prevState;

/// \brief Link entity
public: Eigen::VectorXd prevStateDot;

/// Link entity
public: Entity linkEntity;

/// \brief Ocean current callback
Expand Down Expand Up @@ -218,27 +176,49 @@ void Hydrodynamics::Configure(
<< "\tPlease update your SDF to use <water_density> instead.";
}

this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity",
SdfParamDouble(_sdf, "water_density", 998)
);
this->dataPtr->paramXdotU = SdfParamDouble(_sdf, "xDotU" , 5);
this->dataPtr->paramYdotV = SdfParamDouble(_sdf, "yDotV" , 5);
this->dataPtr->paramZdotW = SdfParamDouble(_sdf, "zDotW" , 0.1);
this->dataPtr->paramKdotP = SdfParamDouble(_sdf, "kDotP" , 0.1);
this->dataPtr->paramMdotQ = SdfParamDouble(_sdf, "mDotQ" , 0.1);
this->dataPtr->paramNdotR = SdfParamDouble(_sdf, "nDotR" , 1);
this->dataPtr->paramXu = SdfParamDouble(_sdf, "xU" , 20);
this->dataPtr->paramXuu = SdfParamDouble(_sdf, "xUU" , 0);
this->dataPtr->paramYv = SdfParamDouble(_sdf, "yV" , 20);
this->dataPtr->paramYvv = SdfParamDouble(_sdf, "yVV" , 0);
this->dataPtr->paramZw = SdfParamDouble(_sdf, "zW" , 20);
this->dataPtr->paramZww = SdfParamDouble(_sdf, "zWW" , 0);
this->dataPtr->paramKp = SdfParamDouble(_sdf, "kP" , 20);
this->dataPtr->paramKpp = SdfParamDouble(_sdf, "kPP" , 0);
this->dataPtr->paramMq = SdfParamDouble(_sdf, "mQ" , 20);
this->dataPtr->paramMqq = SdfParamDouble(_sdf, "mQQ" , 0);
this->dataPtr->paramNr = SdfParamDouble(_sdf, "nR" , 20);
this->dataPtr->paramNrr = SdfParamDouble(_sdf, "nRR" , 0);
this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity",
SdfParamDouble(_sdf, "water_density", 998)
);
// Load stability derivatives
// Use SNAME 1950 convention to load the coeffecients.
const auto snameConventionVel = "UVWPQR";
const auto snameConventionMoment = "xyzkmn";
for(auto i = 0; i < 6; i++)
{
for(auto j = 0; j < 6; j++)
{
std::string prefix = {snameConventionMoment[i]};
prefix += snameConventionVel[j];
this->dataPtr->stabilityLinearTerms[i*6 + j] =
SdfParamDouble(_sdf, prefix, 0);
for(auto k = 0; k < 6; k++)
{
this->dataPtr->stabilityQuadraticDerivative[i*36 + j*6 + k] =
SdfParamDouble(
_sdf,
prefix + snameConventionVel[k],
0);
this->dataPtr->stabilityQuadraticAbsDerivative[i*36 + j*6 + k] =
SdfParamDouble(
_sdf,
prefix + "abs" + snameConventionVel[k],
0);
}
}
}

// Added mass according to Fossen's equations (p 37)
this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6);
for(auto i = 0; i < 6; i++)
{
for(auto j = 0; j < 6; j++)
{
std::string prefix = {snameConventionMoment[i]};
prefix += "Dot";
prefix += snameConventionVel[j];
this->dataPtr->Ma(i, j) = SdfParamDouble(_sdf, prefix, 0);
}
}

_sdf->Get<bool>("disable_coriolis", this->dataPtr->disableCoriolis, false);
_sdf->Get<bool>("disable_added_mass", this->dataPtr->disableAddedMass, false);
Expand Down Expand Up @@ -284,17 +264,6 @@ void Hydrodynamics::Configure(
AddWorldPose(this->dataPtr->linkEntity, _ecm);
AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm);
AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm);


// Added mass according to Fossen's equations (p 37)
this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6);

this->dataPtr->Ma(0, 0) = this->dataPtr->paramXdotU;
this->dataPtr->Ma(1, 1) = this->dataPtr->paramYdotV;
this->dataPtr->Ma(2, 2) = this->dataPtr->paramZdotW;
this->dataPtr->Ma(3, 3) = this->dataPtr->paramKdotP;
this->dataPtr->Ma(4, 4) = this->dataPtr->paramMdotQ;
this->dataPtr->Ma(5, 5) = this->dataPtr->paramNdotR;
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -338,7 +307,7 @@ void Hydrodynamics::PreUpdate(
// Transform state to local frame
auto pose = baseLink.WorldPose(_ecm);
// Since we are transforming angular and linear velocity we only care about
// rotation
// rotation. Also this is where we apply the effects of current to the link
auto localLinearVelocity = pose->Rot().Inverse() *
(linearVelocity->Data() - currentVector);
auto localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity;
Expand All @@ -351,7 +320,6 @@ void Hydrodynamics::PreUpdate(
state(4) = localRotationalVelocity.Y();
state(5) = localRotationalVelocity.Z();

// TODO(anyone) Make this configurable
auto dt = static_cast<double>(_info.dt.count())/1e9;
stateDot = (state - this->dataPtr->prevState)/dt;

Expand All @@ -363,40 +331,50 @@ void Hydrodynamics::PreUpdate(

// Coriolis and Centripetal forces for under water vehicles (Fossen P. 37)
// Note: this is significantly different from VRX because we need to account
// for the under water vehicle's additional DOF
Cmat(0, 4) = - this->dataPtr->paramZdotW * state(2);
Cmat(0, 5) = - this->dataPtr->paramYdotV * state(1);
Cmat(1, 3) = this->dataPtr->paramZdotW * state(2);
Cmat(1, 5) = - this->dataPtr->paramXdotU * state(0);
Cmat(2, 3) = - this->dataPtr->paramYdotV * state(1);
Cmat(2, 4) = this->dataPtr->paramXdotU * state(0);
Cmat(3, 1) = - this->dataPtr->paramZdotW * state(2);
Cmat(3, 2) = this->dataPtr->paramYdotV * state(1);
Cmat(3, 4) = - this->dataPtr->paramNdotR * state(5);
Cmat(3, 5) = this->dataPtr->paramMdotQ * state(4);
Cmat(4, 0) = this->dataPtr->paramZdotW * state(2);
Cmat(4, 2) = - this->dataPtr->paramXdotU * state(0);
Cmat(4, 3) = this->dataPtr->paramNdotR * state(5);
Cmat(4, 5) = - this->dataPtr->paramKdotP * state(3);
Cmat(5, 0) = this->dataPtr->paramZdotW * state(2);
Cmat(5, 1) = this->dataPtr->paramXdotU * state(0);
Cmat(5, 3) = - this->dataPtr->paramMdotQ * state(4);
Cmat(5, 4) = this->dataPtr->paramKdotP * state(3);
// for the under water vehicle's additional DOF. We are just considering
// diagonal terms here. Have yet to add the cross terms here. Also note, since
// $M_a(0,0) = \dot X_u $ , $M_a(1,1) = \dot Y_v $ and so forth, we simply
// load the stability derivatives from $M_a$.
Cmat(0, 4) = - this->dataPtr->Ma(2, 2) * state(2);
Cmat(0, 5) = - this->dataPtr->Ma(1, 1) * state(1);
Cmat(1, 3) = this->dataPtr->Ma(2, 2) * state(2);
Cmat(1, 5) = - this->dataPtr->Ma(0, 0) * state(0);
Cmat(2, 3) = - this->dataPtr->Ma(1, 1) * state(1);
Cmat(2, 4) = this->dataPtr->Ma(0, 0) * state(0);
Cmat(3, 1) = - this->dataPtr->Ma(2, 2) * state(2);
Cmat(3, 2) = this->dataPtr->Ma(1, 1) * state(1);
Cmat(3, 4) = - this->dataPtr->Ma(5, 5) * state(5);
Cmat(3, 5) = this->dataPtr->Ma(4, 4) * state(4);
Cmat(4, 0) = this->dataPtr->Ma(2, 2) * state(2);
Cmat(4, 2) = - this->dataPtr->Ma(0, 0) * state(0);
Cmat(4, 3) = this->dataPtr->Ma(5, 5) * state(5);
Cmat(4, 5) = - this->dataPtr->Ma(3, 3) * state(3);
Cmat(5, 0) = this->dataPtr->Ma(2, 2) * state(2);
Cmat(5, 1) = this->dataPtr->Ma(0, 0) * state(0);
Cmat(5, 3) = - this->dataPtr->Ma(4, 4) * state(4);
Cmat(5, 4) = this->dataPtr->Ma(3, 3) * state(3);
const Eigen::VectorXd kCmatVec = - Cmat * state;

// Damping forces (Fossen P. 43)
Dmat(1, 1)
= - this->dataPtr->paramYv - this->dataPtr->paramYvv * abs(state(1));
Dmat(0, 0)
= - this->dataPtr->paramXu - this->dataPtr->paramXuu * abs(state(0));
Dmat(2, 2)
= - this->dataPtr->paramZw - this->dataPtr->paramZww * abs(state(2));
Dmat(3, 3)
= - this->dataPtr->paramKp - this->dataPtr->paramKpp * abs(state(3));
Dmat(4, 4)
= - this->dataPtr->paramMq - this->dataPtr->paramMqq * abs(state(4));
Dmat(5, 5)
= - this->dataPtr->paramNr - this->dataPtr->paramNrr * abs(state(5));
// Damping forces
for(int i = 0; i < 6; i++)
{
for(int j = 0; j < 6; j++)
{
auto coeff = - this->dataPtr->stabilityLinearTerms[i * 6 + j];
for(int k = 0; k < 6; k++)
{
auto index = i * 36 + j * 6 + k;
auto absCoeff =
this->dataPtr->stabilityQuadraticAbsDerivative[index] * abs(state(k));
coeff -= absCoeff;
auto velCoeff =
this->dataPtr->stabilityQuadraticDerivative[index] * state(k);
coeff -= velCoeff;
}

Dmat(i, j) = coeff;
}
}
Comment on lines +358 to +377
Copy link
Contributor

@hidmic hidmic Nov 10, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@arjo129 meta: I'll start by saying I know next to nothing about underwater vessel modelling. That said, I can't relate these equations to those in Fossen's books (ie. Guidance of Ocean Vehicles and Marine Control Systems). I see you are including all cross terms, but none of the models proposed includes a pure quadratic term nor the means to estimate most of the rest (which begs the question, why bother?).

Mind to explain?

Copy link
Contributor Author

@arjo129 arjo129 Nov 14, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So the LRAUV's original model does, we had not implemented it as we were interested in getting the hydrodynamics plugin up and running early on. It is often hard to estimate and verify the cross terms in comparison to the diagonal terms, however there are definitely techniques that can be used. But yes, if you don't have a team dedicated for fluid dynamics the diagonal terms often are all that is needed.


const Eigen::VectorXd kDvec = Dmat * state;

Expand All @@ -411,10 +389,10 @@ void Hydrodynamics::PreUpdate(
kTotalWrench += kCmatVec;
}

ignition::math::Vector3d
totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));
ignition::math::Vector3d
totalTorque(-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5));
math::Vector3d totalForce(
-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));
math::Vector3d totalTorque(
-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5));

baseLink.AddWorldWrench(
_ecm,
Expand Down
15 changes: 15 additions & 0 deletions src/systems/hydrodynamics/Hydrodynamics.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ namespace systems
/// The exact description of these parameters can be found on p. 37 and
/// p. 43 of Fossen's book. They are used to calculate added mass, linear and
/// quadratic drag and coriolis force.
/// ### Diagonal terms:
/// * <xDotU> - Added mass in x direction [kg]
/// * <yDotV> - Added mass in y direction [kg]
/// * <zDotW> - Added mass in z direction [kg]
Expand All @@ -62,6 +63,20 @@ namespace systems
/// * <mQ> - Linear damping, 1st order, pitch component [kg/m]
/// * <nRR> - Quadratic damping, 2nd order, yaw component [kg/m^2]
/// * <nR> - Linear damping, 1st order, yaw component [kg/m]
/// ### Cross terms
/// In general we support cross terms as well. These are terms which act on
/// 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
/// and yaw axis respectively.
/// * Added Mass: <{x|y|z|k|m|n}Dot{U|V|W|P|Q|R}> e.g. <xDotR>
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
/// Units are either kg or kgm^2 depending on the choice of terms.
/// * Quadratic Damping: <{x|y|z|k|m|n}{U|V|W|P|Q|R}{U|V|W|P|Q|R}>
/// e.g. <xRQ>
/// Units are either kg/m or kg/m^2.
/// * Linear Damping: <{x|y|z|k|m|n}{U|V|W|P|Q|R}>. e.g. <xR>
/// Units are either kg or kg or kg/m.
/// Additionally the system also supports the following parameters:
/// * <waterDensity> - The density of the fluid its moving in.
/// Defaults to 998kgm^-3. [kgm^-3, deprecated]
Expand Down