Make WindEffects configurable on a location basis

src/systems/wind_effects/WindEffects.cc

@@ -22,12 +22,19 @@
 #include <ignition/msgs/entity_factory.pb.h>
 
 #include <string>
+#include <utility>
 #include <vector>
 
 #include <sdf/Root.hh>
 #include <sdf/Error.hh>
 
 #include <ignition/common/Profiler.hh>
+#include <ignition/math/AdditivelySeparableScalarField3.hh>
+#include <ignition/math/PiecewiseScalarField3.hh>
+#include <ignition/math/Polynomial3.hh>
+#include <ignition/math/Region3.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Vector4.hh>
 #include <ignition/msgs/Utility.hh>
 #include <ignition/plugin/Register.hh>
 #include <ignition/transport/Node.hh>
@@ -53,6 +60,153 @@ using namespace ignition;
 using namespace gazebo;
 using namespace systems;
 
+namespace {
+  using ScalingFactor =
+      math::AdditivelySeparableScalarField3d<math::Polynomial3d>;
+  using PiecewiseScalingFactor = math::PiecewiseScalarField3d<ScalingFactor>;
+
+  //////////////////////////////////////////////////
+  ScalingFactor MakeConstantScalingFactor(double _value)
+  {
+    return ScalingFactor(
+        _value / 3.,
+        math::Polynomial3d::Constant(_value),
+        math::Polynomial3d::Constant(_value),
+        math::Polynomial3d::Constant(_value));
+  }
+
+  //////////////////////////////////////////////////
+  math::Polynomial3d LoadPolynomial3d(const sdf::ElementPtr &_sdf)
+  {
+    math::Vector4<double> coeffs;
+    std::istringstream(_sdf->Get<std::string>()) >> coeffs;
+    return math::Polynomial3d(std::move(coeffs));
+  }
+
+  //////////////////////////////////////////////////
+  ScalingFactor LoadScalingFactor(const sdf::ElementPtr &_sdf)
+  {
+    if (!_sdf->GetFirstElement())
+    {
+      return MakeConstantScalingFactor(_sdf->Get<double>());
+    }
+    double k = 1.;
+    if (_sdf->HasElement("k"))
+    {
+      k = _sdf->GetElementImpl("k")->Get<double>();
+    }
+    math::Polynomial3d p = math::Polynomial3d::Constant(0.);
+    if (_sdf->HasElement("px"))
+    {
+      p = LoadPolynomial3d(_sdf->GetElementImpl("px"));
+    }
+    math::Polynomial3d q = math::Polynomial3d::Constant(0.);
+    if (_sdf->HasElement("qy"))
+    {
+      q = LoadPolynomial3d(_sdf->GetElementImpl("qy"));
+    }
+    math::Polynomial3d r = math::Polynomial3d::Constant(0.);
+    if (_sdf->HasElement("rz"))
+    {
+      r = LoadPolynomial3d(_sdf->GetElementImpl("rz"));
+    }
+    return ScalingFactor(k, std::move(p), std::move(q), std::move(r));
+  }
+
+  //////////////////////////////////////////////////
+  math::Intervald
+  LoadIntervald(const sdf::ElementPtr _sdf, const std::string &_prefix)
+  {
+    bool leftClosed = false;
+    double leftValue = -math::INF_D;
+    const std::string geAttrName = _prefix + "ge";
+    const std::string gtAttrName = _prefix + "gt";
+    if (_sdf->HasAttribute(geAttrName) && _sdf->HasAttribute(gtAttrName))
+    {
+      ignerr << "Attributes '" << geAttrName << "' and '" << gtAttrName << "'"
+             << " are mutually exclusive. Ignoring both." << std::endl;
+    }
+    else if (_sdf->HasAttribute(geAttrName))
+    {
+      sdf::ParamPtr sdfGeAttrValue = _sdf->GetAttribute(geAttrName);
+      if (!sdfGeAttrValue->Get<double>(leftValue))
+      {
+        ignerr << "Invalid '" << geAttrName << "' attribute value. "
+               << "Ignoring." << std::endl;
+      }
+      else
+      {
+        leftClosed = true;
+      }
+    }
+    else if (_sdf->HasAttribute(gtAttrName))
+    {
+      sdf::ParamPtr sdfGtAttrValue = _sdf->GetAttribute(gtAttrName);
+      if(!sdfGtAttrValue->Get<double>(leftValue))
+      {
+        ignerr << "Invalid '" << gtAttrName << "' attribute value. "
+               << "Ignoring." << std::endl;
+      }
+    }
+
+    bool rightClosed = false;
+    double rightValue = math::INF_D;
+    const std::string leAttrName = _prefix + "le";
+    const std::string ltAttrName = _prefix + "lt";
+    if (_sdf->HasAttribute(leAttrName) && _sdf->HasAttribute(ltAttrName))
+    {
+      ignerr << "Attributes '" << leAttrName << "' and '" << ltAttrName << "'"
+             << " are mutually exclusive. Ignoring both." << std::endl;
+    }
+    else if (_sdf->HasAttribute(leAttrName))
+    {
+      sdf::ParamPtr sdfLeAttrValue = _sdf->GetAttribute(leAttrName);
+      if (!sdfLeAttrValue->Get<double>(rightValue))
+      {
+        ignerr << "Invalid '" << leAttrName << "' attribute value. "
+               << "Ignoring." << std::endl;
+      }
+      else
+      {
+        rightClosed = true;
+      }
+    }
+    else if (_sdf->HasAttribute(ltAttrName))
+    {
+      sdf::ParamPtr sdfLtAttrValue = _sdf->GetAttribute(ltAttrName);
+      if (!sdfLtAttrValue->Get<double>(rightValue))
+      {
+        ignerr << "Invalid '" << gtAttrName << "'"
+               << "attribute value. Ignoring."
+               << std::endl;
+      }
+    }
+
+    return math::Intervald(leftValue, leftClosed, rightValue, rightClosed);
+  }
+
+  //////////////////////////////////////////////////
+  PiecewiseScalingFactor LoadPiecewiseScalingFactor(const sdf::ElementPtr _sdf)
+  {
+    if (!_sdf->HasElement("when"))
+    {
+      return PiecewiseScalingFactor::Throughout(LoadScalingFactor(_sdf));
+    }
+    std::vector<PiecewiseScalingFactor::Piece> pieces;
+    for (sdf::ElementPtr sdfPiece = _sdf->GetElement("when");
+         sdfPiece; sdfPiece = sdfPiece->GetNextElement("when"))
+    {
+      pieces.push_back({
+          math::Region3d(LoadIntervald(sdfPiece, "x"),
+                         LoadIntervald(sdfPiece, "y"),
+                         LoadIntervald(sdfPiece, "z")),
+          LoadScalingFactor(sdfPiece),
+      });
+    }
+    return PiecewiseScalingFactor(std::move(pieces));
+  }
+}  // namespace
+
 /// \brief Private WindEffects data class.
 class ignition::gazebo::systems::WindEffectsPrivate
 {
@@ -134,7 +288,7 @@ class ignition::gazebo::systems::WindEffectsPrivate
   public: double magnitudeMeanVertical{0.0};
 
   /// \brief The scaling factor to approximate wind as force on a mass.
-  public: double forceApproximationScalingFactor{1.0};
+  public: PiecewiseScalingFactor forceApproximationScalingFactor;
 
   /// \brief Noise added to magnitude.
   public: sensors::NoisePtr noiseMagnitude;
@@ -281,20 +435,23 @@ void WindEffectsPrivate::Load(EntityComponentManager &_ecm,
   {
     sdf::ElementPtr sdfForceApprox =
         _sdf->GetElementImpl("force_approximation_scaling_factor");
-
-    this->forceApproximationScalingFactor = sdfForceApprox->Get<double>();
+    this->forceApproximationScalingFactor =
+        LoadPiecewiseScalingFactor(sdfForceApprox);
+  }
+  else
+  {
+    this->forceApproximationScalingFactor =
+        PiecewiseScalingFactor::Throughout(MakeConstantScalingFactor(1.));
   }
 
-  // If the forceApproximationScalingFactor is very small don't update.
   // It doesn't make sense to be negative, that would be negative wind drag.
-  if (std::fabs(this->forceApproximationScalingFactor) < 1e-6)
+  if (this->forceApproximationScalingFactor.Minimum() < 0.)
   {
-    ignerr << "Please set <force_approximation_scaling_factor> to a value "
-           << "greater than 0" << std::endl;
+    ignerr << "<force_approximation_scaling_factor> must "
+           << "always be a nonnegative quantity" << std::endl;
     return;
   }
 
-
   this->validConfig = true;
 }
 
@@ -411,12 +568,16 @@ void WindEffectsPrivate::ApplyWindForce(const UpdateInfo &,
 
   Link link;
 
-  _ecm.Each<components::Link, components::Inertial, components::WindMode,
+  _ecm.Each<components::Link,
+            components::Inertial,
+            components::WindMode,
+            components::WorldPose,
             components::WorldLinearVelocity>(
       [&](const Entity &_entity,
           components::Link *,
           components::Inertial *_inertial,
           components::WindMode *_windMode,
+          components::WorldPose *_linkPose,
           components::WorldLinearVelocity *_linkVel) -> bool
       {
         // Skip links for which the wind is disabled
@@ -427,9 +588,11 @@ void WindEffectsPrivate::ApplyWindForce(const UpdateInfo &,
 
         link.ResetEntity(_entity);
 
-        math::Vector3d windForce = _inertial->Data().MassMatrix().Mass() *
-                                   this->forceApproximationScalingFactor *
-                                   (windVel->Data() - _linkVel->Data());
+        const math::Vector3d windForce =
+            _inertial->Data().MassMatrix().Mass() *
+            this->forceApproximationScalingFactor(
+                _linkPose->Data().Pos()) *
+            (windVel->Data() - _linkVel->Data());
 
         // Apply force at center of mass
         link.AddWorldForce(_ecm, windForce);

src/systems/wind_effects/WindEffects.hh

@@ -33,9 +33,8 @@ namespace systems
   class WindEffectsPrivate;
 
   /// \brief A system that simulates a simple wind model.
-  /// The wind is described as a uniform worldwide model. So it is independent
-  /// from model position for simple computations. Its components are computed
-  /// separately:
+  /// The wind is described as a uniform worldwide model.
+  /// Its components are computed separately:
   /// - Horizontal amplitude:
   ///      Low pass filtering on user input (complementary gain)
   ///      + small local fluctuations
@@ -51,6 +50,12 @@ namespace systems
   ///      + small local fluctuations
   ///      + noise on value
   ///
+  /// Forces exerted by the wind on model links are approximated from
+  /// link mass and velocity with respect to wind velocity, and applied
+  /// to the link frame origin. These approximations can be amplified or
+  /// attenuated on a per location basis by specifying a piecewise scalar
+  /// field for a scaling factor.
+  ///
   /// The following parameters are used by the system:
   ///
   /// - `<horizontal><magnitude><time_for_rise>`
@@ -86,6 +91,40 @@ namespace systems
   /// - `<vertical><noise>`
   /// Parameters for the noise that is added to the vertical wind velocity
   /// magnitude.
+  ///
+  /// - `<force_approximation_scaling_factor>`
+  /// Proportionality constant used for wind force approximations as a
+  /// piecewise, separable scalar field:
+  /// \verbatim
+  ///   <force_approximation_scaling_factor>
+  ///     <when xlt="0"> <!-- Half space where x < 0 -->
+  ///       <k>1</k>
+  ///       <px>0 0 0 1</px>  <!-- p(x) = x -->
+  ///       <qy>0 0 0 1</qy>  <!-- q(y) = 1 -->
+  ///       <rz>0 1 0 1</rz>  <!-- r(z) = z^2 -->
+  ///     </when>
+  ///   </force_approximation_scaling_factor>
+  /// \endverbatim
+  /// When the scaling factor is to be constant in a region, a numerical
+  /// constant may be used in place for the scalar field definition:
+  /// \verbatim
+  ///   <force_approximation_scaling_factor>
+  ///     <!-- First octant -->
+  ///     <when xge="0" yge="0" zge="0">1</when>
+  ///   </force_approximation_scaling_factor>
+  /// \endverbatim
+  /// To use the same constant or scalar field in all space, region
+  /// definition may be dropped:
+  /// \verbatim
+  ///   <force_approximation_scaling_factor>
+  ///     <k>2</k>
+  ///     <px>1 0 1 0</px>  <!-- p(x) = x^3 + x -->
+  ///     <qy>0 1 0 1</qy>  <!-- q(y) = x^2 + 1 -->
+  ///     <rz>1 0 1 0</rz>  <!-- r(z) = z^3 + z -->
+  ///   </force_approximation_scaling_factor>
+  /// \endverbatim
+  /// Regions may not overlap.
+  ///
   class WindEffects final:
     public System,
     public ISystemConfigure,

test/integration/wind_effects.cc

@@ -218,7 +218,6 @@ TEST_F(WindEffectsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WindEnabledInLink))
   EXPECT_TRUE(linkWindMode.values.back().Data());
 }
 
-
 ////////////////////////////////////////////////
 TEST_F(WindEffectsTest , IGN_UTILS_TEST_DISABLED_ON_WIN32(WindForce))
 {
@@ -256,6 +255,48 @@ TEST_F(WindEffectsTest , IGN_UTILS_TEST_DISABLED_ON_WIN32(WindForce))
   }
 }
 
+////////////////////////////////////////////////
+TEST_F(WindEffectsTest , IGN_UTILS_TEST_DISABLED_ON_WIN32(ComplexWindForce))
+{
+  this->StartServer("/test/worlds/sea_storm_effects.sdf");
+  LinkComponentRecorder<components::WorldLinearAcceleration>
+      belowSurfaceAccelerations("box_below_surface", true);
+  LinkComponentRecorder<components::WorldLinearAcceleration>
+      aboveSurfaceAccelerations("box_above_surface", true);
+  LinkComponentRecorder<components::WorldLinearAcceleration>
+      upHighAccelerations("box_up_high", true);
+
+  using namespace std::chrono_literals;
+  this->server->SetUpdatePeriod(0ns);
+
+  this->server->AddSystem(belowSurfaceAccelerations.systemPtr);
+  this->server->AddSystem(aboveSurfaceAccelerations.systemPtr);
+  this->server->AddSystem(upHighAccelerations.systemPtr);
+
+  const std::size_t nIters{3000};
+  this->server->Run(true, nIters, false);
+
+  ASSERT_EQ(nIters, belowSurfaceAccelerations.values.size());
+  ASSERT_EQ(nIters, aboveSurfaceAccelerations.values.size());
+  ASSERT_EQ(nIters, upHighAccelerations.values.size());
+
+  double maxAboveSurfaceAccelMagnitude = 0.;
+  for (std::size_t i = 0; i < nIters; ++i)
+  {
+    const double belowSurfaceAccelMagnitude =
+        belowSurfaceAccelerations.values[i].Data().Length();
+    const double aboveSurfaceAccelMagnitude =
+        aboveSurfaceAccelerations.values[i].Data().Length();
+    const double upHighAccelMagnitude =
+        upHighAccelerations.values[i].Data().Length();
+    maxAboveSurfaceAccelMagnitude = std::max(
+        maxAboveSurfaceAccelMagnitude, aboveSurfaceAccelMagnitude);
+    EXPECT_LE(aboveSurfaceAccelMagnitude, upHighAccelMagnitude);
+    EXPECT_LT(belowSurfaceAccelMagnitude, 1e-6);
+  }
+  EXPECT_GT(maxAboveSurfaceAccelMagnitude, 1e-6);
+}
+
 ////////////////////////////////////////////////
 TEST_F(WindEffectsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TopicsAndServices))
 {