diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index bba781343d..45d8057870 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -24,6 +24,7 @@ #include #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 + gridField; + + public: std::optional> 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([&](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(_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(_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("lookup_current_x"); + } + + if(_sdf->HasElement("lookup_current_y")) + { + this->dataPtr->useCurrentTable = true; + this->dataPtr->axisComponents[1] = + _sdf->Get("lookup_current_y"); + } + + if(_sdf->HasElement("lookup_current_z")) + { + this->dataPtr->useCurrentTable = true; + this->dataPtr->axisComponents[2] = + _sdf->Get("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( diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index c8287b8b26..ea7b776301 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/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: /// * - Added mass in x direction [kg] /// * - Added mass in y direction [kg] @@ -63,6 +64,7 @@ namespace systems /// * - Linear damping, 1st order, pitch component [kg/m] /// * - Quadratic damping, 2nd order, yaw component [kg/m^2] /// * - 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 [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: + /// * - X axis to use for lookup current + /// * - Y axis to use for lookup current + /// * - 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 dataPtr; }; diff --git a/test/integration/hydrodynamics.cc b/test/integration/hydrodynamics.cc index bcfb53b072..b6de948349 100644 --- a/test/integration/hydrodynamics.cc +++ b/test/integration/hydrodynamics.cc @@ -50,6 +50,8 @@ class HydrodynamicsTest : public InternalFixture<::testing::Test> /// \param[in] _drag_coeff Body drag coefficient public: std::vector TestWorld(const std::string &_world, const std::string &_namespace); + + public: math::Vector3d defaultForce{0, 0, 10.0}; }; ////////////////////////////////////////////////// @@ -88,8 +90,7 @@ std::vector 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 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); + } +} diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt index dbc095a2c7..a522517795 100644 --- a/test/worlds/CMakeLists.txt +++ b/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) diff --git a/test/worlds/hydrodynamic_currents.csv b/test/worlds/hydrodynamic_currents.csv new file mode 100644 index 0000000000..aa967fc688 --- /dev/null +++ b/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 diff --git a/test/worlds/hydrodynamics.sdf b/test/worlds/hydrodynamics.sdf.in similarity index 74% rename from test/worlds/hydrodynamics.sdf rename to test/worlds/hydrodynamics.sdf.in index 7fc4790c84..e0a2f25be5 100644 --- a/test/worlds/hydrodynamics.sdf +++ b/test/worlds/hydrodynamics.sdf.in @@ -11,9 +11,24 @@ 0 0 0 + + + @CMAKE_SOURCE_DIR@/test/worlds/hydrodynamic_currents.csv + + + + x + y + z + + + + + name="gz::sim::systems::Physics"> @@ -94,8 +109,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> sphere2_link 918 0 @@ -153,8 +168,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> cylinder1_link 1000 0 @@ -212,8 +227,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> cylinder2_link 1000 0 @@ -271,8 +286,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> cylinder3_link 1000 0 @@ -295,5 +310,65 @@ 0 + + + + 4 0 0 0 0 0 + + 40 + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + sphere_current_link + 918 + 15.381 + 15.381 + 15.381 + 0 + 0 + 0 + 94.2475 + 0 + 94.2475 + 0 + 94.2475 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + current_x + current_y + current_z + +