Skip to content

Commit 42b83ec

Browse files
arjo129arjo129
committed
Adds support for hydrodynamic cross terms (#1784)
* Adds support for hydrodynamic cross terms The current plugin implementation only supports diagonal parameters. This PR adds support for cross terms in the added mass and the damping matrix. Nothing. The plugin should continue to work as expected. Hence the test suite is not changed. For reference read: Fossen T. "Guidance and Control of Ocean Vehicles". Fossens equations rely on so called "stability derivatives" in the damping and the added mass terms. These are given by parameters such as `<xUU> `. What this means is `xUU` is the Quadratic Damping term (hence the repeated Us along the X axis for a velocity vector along the X axis (U, V, W are used by SNAME (Society of Naval Architects and Marine Engineers) to represent linear velocity along X, Y and Z axis repspectively). Now, in the case of many ocean going vehicles a push along the U (X-direction velocity) may result in translations and rotations in other axis (for instance my boat might yaw to the right). This may seem counter intuitive but fluid-solid interaction is often complex an unintuitive. In previous versions of the plugin, we only supported the parameters along the diagonal of the damping and added mass matrix. While this is often more than sufficient for simple simulations, there are cases where we may like to simulate cross terms. The adding of cross terms now opens up a lot more parameters. For instance, now we will have terms like `xQR` (quadratic damping in x axis with respect to angular velocity in roll and yaw axis). Signed-off-by: Arjo Chakravarty <[email protected]>
1 parent 2d19f93 commit 42b83ec

File tree

2 files changed

+123
-121
lines changed

2 files changed

+123
-121
lines changed

‎src/systems/hydrodynamics/Hydrodynamics.cc

+108-121
Original file line numberDiff line numberDiff line change
@@ -43,60 +43,16 @@ using namespace systems;
4343
/// \brief Private Hydrodynamics data class.
4444
class gz::sim::systems::HydrodynamicsPrivateData
4545
{
46-
/// \brief Values to set via Plugin Parameters.
47-
/// Plugin Parameter: Added mass in surge, X_\dot{u}.
48-
public: double paramXdotU;
46+
/// \brief Contains the quadratic stability derivatives like $X_{uv}$,
47+
/// Y_{vv}, Y_{wv} etc.
48+
public: double stabilityQuadraticDerivative[216];
4949

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

53-
/// \brief Plugin Parameter: Added mass in heave, Z_\dot{w}.
54-
public: double paramZdotW;
55-
56-
/// \brief Plugin Parameter: Added mass in roll, K_\dot{p}.
57-
public: double paramKdotP;
58-
59-
/// \brief Plugin Parameter: Added mass in pitch, M_\dot{q}.
60-
public: double paramMdotQ;
61-
62-
/// \brief Plugin Parameter: Added mass in yaw, N_\dot{r}.
63-
public: double paramNdotR;
64-
65-
/// \brief Plugin Parameter: Linear drag in surge.
66-
public: double paramXu;
67-
68-
/// \brief Plugin Parameter: Quadratic drag in surge.
69-
public: double paramXuu;
70-
71-
/// \brief Plugin Parameter: Linear drag in sway.
72-
public: double paramYv;
73-
74-
/// \brief Plugin Parameter: Quadratic drag in sway.
75-
public: double paramYvv;
76-
77-
/// \brief Plugin Parameter: Linear drag in heave.
78-
public: double paramZw;
79-
80-
/// \brief Plugin Parameter: Quadratic drag in heave.
81-
public: double paramZww;
82-
83-
/// \brief Plugin Parameter: Linear drag in roll.
84-
public: double paramKp;
85-
86-
/// \brief Plugin Parameter: Quadratic drag in roll.
87-
public: double paramKpp;
88-
89-
/// \brief Plugin Parameter: Linear drag in pitch.
90-
public: double paramMq;
91-
92-
/// \brief Plugin Parameter: Quadratic drag in pitch.
93-
public: double paramMqq;
94-
95-
/// \brief Plugin Parameter: Linear drag in yaw.
96-
public: double paramNr;
97-
98-
/// \brief Plugin Parameter: Quadratic drag in yaw.
99-
public: double paramNrr;
54+
/// \brief Contains the linear stability derivatives like $X_u, Y_v,$ etc.
55+
public: double stabilityLinearTerms[36] = {0};
10056

10157
/// \brief Water density [kg/m^3].
10258
public: double waterDensity;
@@ -122,7 +78,9 @@ class gz::sim::systems::HydrodynamicsPrivateData
12278
/// \brief Previous state.
12379
public: Eigen::VectorXd prevState;
12480

125-
/// \brief Link entity
81+
public: Eigen::VectorXd prevStateDot;
82+
83+
/// Link entity
12684
public: Entity linkEntity;
12785

12886
/// \brief Ocean current callback
@@ -212,25 +170,56 @@ void Hydrodynamics::Configure(
212170
gz::sim::EventManager &/*_eventMgr*/
213171
)
214172
{
215-
this->dataPtr->waterDensity = SdfParamDouble(_sdf, "water_density", 998);
216-
this->dataPtr->paramXdotU = SdfParamDouble(_sdf, "xDotU" , 5);
217-
this->dataPtr->paramYdotV = SdfParamDouble(_sdf, "yDotV" , 5);
218-
this->dataPtr->paramZdotW = SdfParamDouble(_sdf, "zDotW" , 0.1);
219-
this->dataPtr->paramKdotP = SdfParamDouble(_sdf, "kDotP" , 0.1);
220-
this->dataPtr->paramMdotQ = SdfParamDouble(_sdf, "mDotQ" , 0.1);
221-
this->dataPtr->paramNdotR = SdfParamDouble(_sdf, "nDotR" , 1);
222-
this->dataPtr->paramXu = SdfParamDouble(_sdf, "xU" , 20);
223-
this->dataPtr->paramXuu = SdfParamDouble(_sdf, "xUU" , 0);
224-
this->dataPtr->paramYv = SdfParamDouble(_sdf, "yV" , 20);
225-
this->dataPtr->paramYvv = SdfParamDouble(_sdf, "yVV" , 0);
226-
this->dataPtr->paramZw = SdfParamDouble(_sdf, "zW" , 20);
227-
this->dataPtr->paramZww = SdfParamDouble(_sdf, "zWW" , 0);
228-
this->dataPtr->paramKp = SdfParamDouble(_sdf, "kP" , 20);
229-
this->dataPtr->paramKpp = SdfParamDouble(_sdf, "kPP" , 0);
230-
this->dataPtr->paramMq = SdfParamDouble(_sdf, "mQ" , 20);
231-
this->dataPtr->paramMqq = SdfParamDouble(_sdf, "mQQ" , 0);
232-
this->dataPtr->paramNr = SdfParamDouble(_sdf, "nR" , 20);
233-
this->dataPtr->paramNrr = SdfParamDouble(_sdf, "nRR" , 0);
173+
if (_sdf->HasElement("waterDensity"))
174+
{
175+
ignwarn <<
176+
"<waterDensity> parameter is deprecated and will be removed Ignition G.\n"
177+
<< "\tPlease update your SDF to use <water_density> instead.";
178+
}
179+
180+
this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity",
181+
SdfParamDouble(_sdf, "water_density", 998)
182+
);
183+
// Load stability derivatives
184+
// Use SNAME 1950 convention to load the coeffecients.
185+
const auto snameConventionVel = "UVWPQR";
186+
const auto snameConventionMoment = "xyzkmn";
187+
for(auto i = 0; i < 6; i++)
188+
{
189+
for(auto j = 0; j < 6; j++)
190+
{
191+
std::string prefix = {snameConventionMoment[i]};
192+
prefix += snameConventionVel[j];
193+
this->dataPtr->stabilityLinearTerms[i*6 + j] =
194+
SdfParamDouble(_sdf, prefix, 0);
195+
for(auto k = 0; k < 6; k++)
196+
{
197+
this->dataPtr->stabilityQuadraticDerivative[i*36 + j*6 + k] =
198+
SdfParamDouble(
199+
_sdf,
200+
prefix + snameConventionVel[k],
201+
0);
202+
this->dataPtr->stabilityQuadraticAbsDerivative[i*36 + j*6 + k] =
203+
SdfParamDouble(
204+
_sdf,
205+
prefix + "abs" + snameConventionVel[k],
206+
0);
207+
}
208+
}
209+
}
210+
211+
// Added mass according to Fossen's equations (p 37)
212+
this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6);
213+
for(auto i = 0; i < 6; i++)
214+
{
215+
for(auto j = 0; j < 6; j++)
216+
{
217+
std::string prefix = {snameConventionMoment[i]};
218+
prefix += "Dot";
219+
prefix += snameConventionVel[j];
220+
this->dataPtr->Ma(i, j) = SdfParamDouble(_sdf, prefix, 0);
221+
}
222+
}
234223

235224
_sdf->Get<bool>("disable_coriolis", this->dataPtr->disableCoriolis, false);
236225
_sdf->Get<bool>("disable_added_mass", this->dataPtr->disableAddedMass, false);
@@ -276,17 +265,6 @@ void Hydrodynamics::Configure(
276265
AddWorldPose(this->dataPtr->linkEntity, _ecm);
277266
AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm);
278267
AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm);
279-
280-
281-
// Added mass according to Fossen's equations (p 37)
282-
this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6);
283-
284-
this->dataPtr->Ma(0, 0) = this->dataPtr->paramXdotU;
285-
this->dataPtr->Ma(1, 1) = this->dataPtr->paramYdotV;
286-
this->dataPtr->Ma(2, 2) = this->dataPtr->paramZdotW;
287-
this->dataPtr->Ma(3, 3) = this->dataPtr->paramKdotP;
288-
this->dataPtr->Ma(4, 4) = this->dataPtr->paramMdotQ;
289-
this->dataPtr->Ma(5, 5) = this->dataPtr->paramNdotR;
290268
}
291269

292270
/////////////////////////////////////////////////
@@ -330,7 +308,7 @@ void Hydrodynamics::PreUpdate(
330308
// Transform state to local frame
331309
auto pose = baseLink.WorldPose(_ecm);
332310
// Since we are transforming angular and linear velocity we only care about
333-
// rotation
311+
// rotation. Also this is where we apply the effects of current to the link
334312
auto localLinearVelocity = pose->Rot().Inverse() *
335313
(linearVelocity->Data() - currentVector);
336314
auto localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity;
@@ -343,7 +321,6 @@ void Hydrodynamics::PreUpdate(
343321
state(4) = localRotationalVelocity.Y();
344322
state(5) = localRotationalVelocity.Z();
345323

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

@@ -355,40 +332,50 @@ void Hydrodynamics::PreUpdate(
355332

356333
// Coriolis and Centripetal forces for under water vehicles (Fossen P. 37)
357334
// Note: this is significantly different from VRX because we need to account
358-
// for the under water vehicle's additional DOF
359-
Cmat(0, 4) = - this->dataPtr->paramZdotW * state(2);
360-
Cmat(0, 5) = - this->dataPtr->paramYdotV * state(1);
361-
Cmat(1, 3) = this->dataPtr->paramZdotW * state(2);
362-
Cmat(1, 5) = - this->dataPtr->paramXdotU * state(0);
363-
Cmat(2, 3) = - this->dataPtr->paramYdotV * state(1);
364-
Cmat(2, 4) = this->dataPtr->paramXdotU * state(0);
365-
Cmat(3, 1) = - this->dataPtr->paramZdotW * state(2);
366-
Cmat(3, 2) = this->dataPtr->paramYdotV * state(1);
367-
Cmat(3, 4) = - this->dataPtr->paramNdotR * state(5);
368-
Cmat(3, 5) = this->dataPtr->paramMdotQ * state(4);
369-
Cmat(4, 0) = this->dataPtr->paramZdotW * state(2);
370-
Cmat(4, 2) = - this->dataPtr->paramXdotU * state(0);
371-
Cmat(4, 3) = this->dataPtr->paramNdotR * state(5);
372-
Cmat(4, 5) = - this->dataPtr->paramKdotP * state(3);
373-
Cmat(5, 0) = this->dataPtr->paramZdotW * state(2);
374-
Cmat(5, 1) = this->dataPtr->paramXdotU * state(0);
375-
Cmat(5, 3) = - this->dataPtr->paramMdotQ * state(4);
376-
Cmat(5, 4) = this->dataPtr->paramKdotP * state(3);
335+
// for the under water vehicle's additional DOF. We are just considering
336+
// diagonal terms here. Have yet to add the cross terms here. Also note, since
337+
// $M_a(0,0) = \dot X_u $ , $M_a(1,1) = \dot Y_v $ and so forth, we simply
338+
// load the stability derivatives from $M_a$.
339+
Cmat(0, 4) = - this->dataPtr->Ma(2, 2) * state(2);
340+
Cmat(0, 5) = - this->dataPtr->Ma(1, 1) * state(1);
341+
Cmat(1, 3) = this->dataPtr->Ma(2, 2) * state(2);
342+
Cmat(1, 5) = - this->dataPtr->Ma(0, 0) * state(0);
343+
Cmat(2, 3) = - this->dataPtr->Ma(1, 1) * state(1);
344+
Cmat(2, 4) = this->dataPtr->Ma(0, 0) * state(0);
345+
Cmat(3, 1) = - this->dataPtr->Ma(2, 2) * state(2);
346+
Cmat(3, 2) = this->dataPtr->Ma(1, 1) * state(1);
347+
Cmat(3, 4) = - this->dataPtr->Ma(5, 5) * state(5);
348+
Cmat(3, 5) = this->dataPtr->Ma(4, 4) * state(4);
349+
Cmat(4, 0) = this->dataPtr->Ma(2, 2) * state(2);
350+
Cmat(4, 2) = - this->dataPtr->Ma(0, 0) * state(0);
351+
Cmat(4, 3) = this->dataPtr->Ma(5, 5) * state(5);
352+
Cmat(4, 5) = - this->dataPtr->Ma(3, 3) * state(3);
353+
Cmat(5, 0) = this->dataPtr->Ma(2, 2) * state(2);
354+
Cmat(5, 1) = this->dataPtr->Ma(0, 0) * state(0);
355+
Cmat(5, 3) = - this->dataPtr->Ma(4, 4) * state(4);
356+
Cmat(5, 4) = this->dataPtr->Ma(3, 3) * state(3);
377357
const Eigen::VectorXd kCmatVec = - Cmat * state;
378358

379-
// Damping forces (Fossen P. 43)
380-
Dmat(1, 1)
381-
= - this->dataPtr->paramYv - this->dataPtr->paramYvv * abs(state(1));
382-
Dmat(0, 0)
383-
= - this->dataPtr->paramXu - this->dataPtr->paramXuu * abs(state(0));
384-
Dmat(2, 2)
385-
= - this->dataPtr->paramZw - this->dataPtr->paramZww * abs(state(2));
386-
Dmat(3, 3)
387-
= - this->dataPtr->paramKp - this->dataPtr->paramKpp * abs(state(3));
388-
Dmat(4, 4)
389-
= - this->dataPtr->paramMq - this->dataPtr->paramMqq * abs(state(4));
390-
Dmat(5, 5)
391-
= - this->dataPtr->paramNr - this->dataPtr->paramNrr * abs(state(5));
359+
// Damping forces
360+
for(int i = 0; i < 6; i++)
361+
{
362+
for(int j = 0; j < 6; j++)
363+
{
364+
auto coeff = - this->dataPtr->stabilityLinearTerms[i * 6 + j];
365+
for(int k = 0; k < 6; k++)
366+
{
367+
auto index = i * 36 + j * 6 + k;
368+
auto absCoeff =
369+
this->dataPtr->stabilityQuadraticAbsDerivative[index] * abs(state(k));
370+
coeff -= absCoeff;
371+
auto velCoeff =
372+
this->dataPtr->stabilityQuadraticDerivative[index] * state(k);
373+
coeff -= velCoeff;
374+
}
375+
376+
Dmat(i, j) = coeff;
377+
}
378+
}
392379

393380
const Eigen::VectorXd kDvec = Dmat * state;
394381

@@ -403,10 +390,10 @@ void Hydrodynamics::PreUpdate(
403390
kTotalWrench += kCmatVec;
404391
}
405392

406-
gz::math::Vector3d
407-
totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));
408-
gz::math::Vector3d
409-
totalTorque(-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5));
393+
math::Vector3d totalForce(
394+
-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));
395+
math::Vector3d totalTorque(
396+
-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5));
410397

411398
baseLink.AddWorldWrench(
412399
_ecm,

‎src/systems/hydrodynamics/Hydrodynamics.hh

+15
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ namespace systems
4444
/// The exact description of these parameters can be found on p. 37 and
4545
/// p. 43 of Fossen's book. They are used to calculate added mass, linear and
4646
/// quadratic drag and coriolis force.
47+
/// ### Diagonal terms:
4748
/// * <xDotU> - Added mass in x direction [kg]
4849
/// * <yDotV> - Added mass in y direction [kg]
4950
/// * <zDotW> - Added mass in z direction [kg]
@@ -62,6 +63,20 @@ namespace systems
6263
/// * <mQ> - Linear damping, 1st order, pitch component [kg/m]
6364
/// * <nRR> - Quadratic damping, 2nd order, yaw component [kg/m^2]
6465
/// * <nR> - Linear damping, 1st order, yaw component [kg/m]
66+
/// ### Cross terms
67+
/// In general we support cross terms as well. These are terms which act on
68+
/// non-diagonal sides. We use the SNAMe convention of naming search terms.
69+
/// (x, y, z) correspond to the respective axis. (k, m, n) correspond to
70+
/// roll, pitch and yaw. Similarly U, V, W represent velocity vectors in
71+
/// X, Y and Z axis while P, Q, R representangular velocity in roll, pitch
72+
/// and yaw axis respectively.
73+
/// * Added Mass: <{x|y|z|k|m|n}Dot{U|V|W|P|Q|R}> e.g. <xDotR>
74+
/// Units are either kg or kgm^2 depending on the choice of terms.
75+
/// * Quadratic Damping: <{x|y|z|k|m|n}{U|V|W|P|Q|R}{U|V|W|P|Q|R}>
76+
/// e.g. <xRQ>
77+
/// Units are either kg/m or kg/m^2.
78+
/// * Linear Damping: <{x|y|z|k|m|n}{U|V|W|P|Q|R}>. e.g. <xR>
79+
/// Units are either kg or kg or kg/m.
6580
/// Additionally the system also supports the following parameters:
6681
/// * <water_density> - The density of the fluid its moving in.
6782
/// Defaults to 998kgm^-3. [kgm^-3]

0 commit comments

Comments
 (0)