Allow using a CSV file to define currents for hydrodynamic system (#1839

)

* Adds support for Hydrodynamic Current

This PR adds support for looking up hydrodynamic currents from a data file via the new Environment component. This allows users to vary currents throughout the water column.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Codecheck

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* more style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Address PR Feedback

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Fix a few more outstanding issues.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Address second pass

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Final touches.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Retrigger CI

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

src/systems/hydrodynamics/Hydrodynamics.cc

@@ -24,6 +24,7 @@
 #include <gz/plugin/Register.hh>
 
 #include "gz/sim/components/AngularVelocity.hh"
+#include "gz/sim/components/Environment.hh"
 #include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/Pose.hh"
 #include "gz/sim/components/World.hh"
@@ -78,14 +79,136 @@ class gz::sim::systems::HydrodynamicsPrivateData
   /// \brief Previous state.
   public: Eigen::VectorXd prevState;
 
+  /// \brief Previous derivative of the state
   public: Eigen::VectorXd prevStateDot;
 
-  /// Link entity
+  /// \brief Use current table if true
+  public: bool useCurrentTable {false};
+
+  /// \brief Current table xComponent
+  public: std::string axisComponents[3];
+
+  public: std::shared_ptr<gz::sim::components::EnvironmentalData>
+    gridField;
+
+  public: std::optional<gz::math::InMemorySession<double, double>> session[3];
+
+  /// \brief Link entity
   public: Entity linkEntity;
 
   /// \brief Ocean current callback
   public: void UpdateCurrent(const msgs::Vector3d &_msg);
 
+  /////////////////////////////////////////////////
+  /// \brief Set the current table
+  /// \param[in] _ecm - The Entity Component Manager
+  /// \param[in] _currTime - The current time
+  public: void SetWaterCurrentTable(
+    const EntityComponentManager &_ecm,
+    const std::chrono::steady_clock::duration &_currTime)
+  {
+    _ecm.EachNew<components::Environment>([&](const Entity &/*_entity*/,
+      const components::Environment *_environment) -> bool
+    {
+      this->gridField = _environment->Data();
+
+      for (std::size_t i = 0; i < 3; i++)
+      {
+        if (!this->axisComponents[i].empty())
+        {
+          if (!this->gridField->frame.Has(this->axisComponents[i]))
+          {
+            gzwarn << "Environmental sensor could not find field "
+              << this->axisComponents[i] << "\n";
+            continue;
+          }
+
+          this->session[i] =
+            this->gridField->frame[this->axisComponents[i]].CreateSession();
+          if (!this->gridField->staticTime)
+          {
+            this->session[i] =
+              this->gridField->frame[this->axisComponents[i]].StepTo(
+                *this->session[i],
+                std::chrono::duration<double>(_currTime).count());
+          }
+
+          if(!this->session[i].has_value())
+          {
+            gzerr << "Exceeded time stamp." << std::endl;
+          }
+        }
+      }
+      return true;
+    });
+  }
+
+  /////////////////////////////////////////////////
+  /// \brief Retrieve water current data from the environment
+  /// \param[in] _ecm - The Entity Component Manager
+  /// \param[in] _currTime - The current time
+  /// \param[in] _position - Position of the vehicle in world coordinates.
+  /// \return The current vector to be applied at this position and time.
+  public: math::Vector3d GetWaterCurrentFromEnvironment(
+    const EntityComponentManager &_ecm,
+    const std::chrono::steady_clock::duration &_currTime,
+    math::Vector3d _position)
+  {
+    math::Vector3d current(0, 0, 0);
+
+    if (!this->gridField ||
+        !(this->session[0].has_value() ||
+          this->session[1].has_value() || this->session[2].has_value()))
+    {
+      return current;
+    }
+
+    for (std::size_t i = 0; i < 3; i++)
+    {
+      if (!this->axisComponents[i].empty())
+      {
+        if (!this->gridField->staticTime)
+        {
+          this->session[i] =
+            this->gridField->frame[this->axisComponents[i]].StepTo(
+              *this->session[i],
+              std::chrono::duration<double>(_currTime).count());
+        }
+
+        if (!this->session[i].has_value())
+        {
+          gzerr << "Time exceeded" << std::endl;
+          continue;
+        }
+
+        auto position = getGridFieldCoordinates(
+          _ecm, _position, this->gridField);
+
+        if (!position.has_value())
+        {
+          gzerr << "Coordinate conversion failed" << std::endl;
+          continue;
+        }
+
+        auto data = this->gridField->frame[this->axisComponents[i]].LookUp(
+          this->session[i].value(), position.value());
+        if (!data.has_value())
+        {
+          auto bounds =
+            this->gridField->frame[this->axisComponents[i]].Bounds(
+              this->session[i].value());
+          gzwarn << "Failed to acquire value perhaps out of field?\n"
+            << "Bounds are " << bounds.first << ", "
+            << bounds.second << std::endl;
+          continue;
+        }
+
+        current[i] = data.value();
+      }
+    }
+
+    return current;
+  }
   /// \brief Mutex
   public: std::mutex mtx;
 };
@@ -262,6 +385,28 @@ void Hydrodynamics::Configure(
 
   this->dataPtr->prevState = Eigen::VectorXd::Zero(6);
 
+  if(_sdf->HasElement("lookup_current_x"))
+  {
+    this->dataPtr->useCurrentTable = true;
+    this->dataPtr->axisComponents[0] =
+      _sdf->Get<std::string>("lookup_current_x");
+  }
+
+  if(_sdf->HasElement("lookup_current_y"))
+  {
+    this->dataPtr->useCurrentTable = true;
+    this->dataPtr->axisComponents[1] =
+      _sdf->Get<std::string>("lookup_current_y");
+  }
+
+  if(_sdf->HasElement("lookup_current_z"))
+  {
+    this->dataPtr->useCurrentTable = true;
+    this->dataPtr->axisComponents[2] =
+      _sdf->Get<std::string>("lookup_current_z");
+  }
+
+
   AddWorldPose(this->dataPtr->linkEntity, _ecm);
   AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm);
   AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm);
@@ -272,6 +417,11 @@ void Hydrodynamics::PreUpdate(
       const gz::sim::UpdateInfo &_info,
       gz::sim::EntityComponentManager &_ecm)
 {
+  if (this->dataPtr->useCurrentTable)
+  {
+    this->dataPtr->SetWaterCurrentTable(_ecm, _info.simTime);
+  }
+
   if (_info.paused)
     return;
 
@@ -300,7 +450,18 @@ void Hydrodynamics::PreUpdate(
   }
 
   // Get current vector
-  math::Vector3d currentVector;
+  math::Vector3d currentVector(0, 0, 0);
+
+  if (this->dataPtr->useCurrentTable)
+  {
+    auto position = baseLink.WorldInertialPose(_ecm);
+    if (position.has_value())
+    {
+      currentVector = this->dataPtr->GetWaterCurrentFromEnvironment(
+        _ecm, _info.simTime, position.value().Pos());
+    }
+  }
+  else
   {
     std::lock_guard lock(this->dataPtr->mtx);
     currentVector = this->dataPtr->currentVector;
@@ -401,10 +562,22 @@ void Hydrodynamics::PreUpdate(
     pose->Rot()*totalTorque);
 }
 
+/////////////////////////////////////////////////
+void Hydrodynamics::PostUpdate(
+      const gz::sim::UpdateInfo &_info,
+      const gz::sim::EntityComponentManager &_ecm)
+{
+  if (this->dataPtr->useCurrentTable)
+  {
+    this->dataPtr->SetWaterCurrentTable(_ecm, _info.simTime);
+  }
+}
+
 GZ_ADD_PLUGIN(
   Hydrodynamics, System,
   Hydrodynamics::ISystemConfigure,
-  Hydrodynamics::ISystemPreUpdate
+  Hydrodynamics::ISystemPreUpdate,
+  Hydrodynamics::ISystemPostUpdate
 )
 
 GZ_ADD_PLUGIN_ALIAS(

src/systems/hydrodynamics/Hydrodynamics.hh

@@ -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]
@@ -63,6 +64,7 @@ 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.
@@ -94,6 +96,17 @@ namespace systems
   ///   * <disable_added_mass> - Disable Added Mass [Boolean, Default: false].
   ///     To be deprecated in Garden.
   ///
+  /// ### Loading external currents
+  /// One can use the EnvironmentPreload system to preload currents into the
+  /// plugin using data files. To use the data you may give CSV column names by
+  /// using `lookup_current_*` tags listed below:
+  ///   * <lookup_current_x> - X axis to use for lookup current
+  ///   * <lookup_current_y> - Y axis to use for lookup current
+  ///   * <lookup_current_z> - Z axis to use for lookup current
+  /// If any one of the fields is present, it is assumed current is to be loaded
+  /// by a data file and the topic will be ignored. If one or two fields are
+  /// present, the missing fields are assumed to default to zero.
+  ///
   /// # Example
   /// An example configuration is provided in the examples folder. The example
   /// uses the LiftDrag plugin to apply steering controls. It also uses the
@@ -129,7 +142,8 @@ namespace systems
   class Hydrodynamics:
     public gz::sim::System,
     public gz::sim::ISystemConfigure,
-    public gz::sim::ISystemPreUpdate
+    public gz::sim::ISystemPreUpdate,
+    public gz::sim::ISystemPostUpdate
   {
     /// \brief Constructor
     public: Hydrodynamics();
@@ -149,6 +163,11 @@ namespace systems
         const gz::sim::UpdateInfo &_info,
         gz::sim::EntityComponentManager &_ecm) override;
 
+    /// Documentation inherited
+    public: void PostUpdate(
+        const gz::sim::UpdateInfo &_info,
+        const gz::sim::EntityComponentManager &_ecm) override;
+
     /// \brief Private data pointer
     private: std::unique_ptr<HydrodynamicsPrivateData> dataPtr;
   };

test/integration/hydrodynamics.cc

@@ -50,6 +50,8 @@ class HydrodynamicsTest : public InternalFixture<::testing::Test>
   /// \param[in] _drag_coeff Body drag coefficient
   public: std::vector<math::Vector3d> TestWorld(const std::string &_world,
    const std::string &_namespace);
+
+  public: math::Vector3d defaultForce{0, 0, 10.0};
 };
 
 //////////////////////////////////////////////////
@@ -88,8 +90,7 @@ std::vector<math::Vector3d> HydrodynamicsTest::TestWorld(
       body.EnableVelocityChecks(_ecm);
 
       // Add force
-      math::Vector3d force(0, 0, 10.0);
-      body.AddWorldForce(_ecm, force);
+      body.AddWorldForce(_ecm, this->defaultForce);
     }).
   OnPostUpdate([&](const UpdateInfo &/*_info*/,
                             const EntityComponentManager &_ecm)
@@ -114,7 +115,7 @@ std::vector<math::Vector3d> HydrodynamicsTest::TestWorld(
 /// of the body when a force is applied.
 TEST_F(HydrodynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityTestinOil))
 {
-  auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+  auto world = common::joinPaths(std::string(PROJECT_BINARY_PATH),
       "test", "worlds", "hydrodynamics.sdf");
 
   auto sphere1Vels = this->TestWorld(world, "sphere1");
@@ -155,7 +156,7 @@ TEST_F(HydrodynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityTestinOil))
 /// positions and orientations.
 TEST_F(HydrodynamicsTest, TransformsTestinWater)
 {
-  auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+  auto world = common::joinPaths(std::string(PROJECT_BINARY_PATH),
       "test", "worlds", "hydrodynamics.sdf");
 
   auto cylinder1Vels = this->TestWorld(world, "cylinder1");
@@ -174,3 +175,27 @@ TEST_F(HydrodynamicsTest, TransformsTestinWater)
     EXPECT_NEAR(cylinder2Vels[i].Z(), cylinder3Vels[i].Z(), 1e-4);
   }
 }
+
+/////////////////////////////////////////////////
+/// This tests the current. A current of (1, 0, 0) is loaded in via a csv file
+TEST_F(HydrodynamicsTest, TransformsTestIn)
+{
+  this->defaultForce = math::Vector3d(0, 0, 0);
+  auto world = common::joinPaths(std::string(PROJECT_BINARY_PATH),
+      "test", "worlds", "hydrodynamics.sdf");
+
+  auto sphereVel = this->TestWorld(world, "sphere_current");
+
+  for (unsigned int i = 990; i < 1000; ++i)
+  {
+    // Expect for the velocity to stabilize
+    EXPECT_NEAR(sphereVel[i-1].Z(), sphereVel[i].Z(), 1e-6);
+    EXPECT_NEAR(sphereVel[i-1].Y(), sphereVel[i].Y(), 1e-6);
+    EXPECT_NEAR(sphereVel[i-1].X(), sphereVel[i].X(), 1e-3);
+
+    // Given current of  (1,0,0), vehicle should move in similar direction.
+    EXPECT_NEAR(sphereVel[i-1].Z(), 0, 1e-6);
+    EXPECT_NEAR(sphereVel[i-1].Y(), 0, 1e-6);
+    EXPECT_GT(sphereVel[i-1].X(), 0);
+  }
+}

test/worlds/CMakeLists.txt

@@ -5,3 +5,4 @@ configure_file (heightmap.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/heightmap.sdf
 configure_file (environmental_data.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_data.sdf)
 
 configure_file (environmental_sensor.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_sensor.sdf)
+configure_file (hydrodynamics.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/hydrodynamics.sdf)

test/worlds/hydrodynamic_currents.csv

@@ -0,0 +1,17 @@
+timestamp,x,y,z,current_x,current_y,current_z
+0,-20,-20, 20,1,0,0
+0,20,-20, 20,1,0,0
+0,-20,-20,-20,1,0,0
+0,20,-20,-20,1,0,0
+0,-20, 20, 20,1,0,0
+0,20, 20, 20,1,0,0
+0,-20, 20,-20,1,0,0
+0,20, 20,-20,1,0,0
+10000, 20,-20, 20,1,0,0
+10000,-20,-20, 20,1,0,0
+10000, 20,-20,-20,1,0,0
+10000,-20,-20,-20,1,0,0
+10000, 20, 20, 20,1,0,0
+10000,-20, 20, 20,1,0,0
+10000, 20, 20,-20,1,0,0
+10000,-20, 20,-20,1,0,0