Skip to content

Commit

Permalink
Allow using a CSV file to define currents for hydrodynamic system (#1839
Browse files Browse the repository at this point in the history 
)

* 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 <[email protected]>

* Codecheck

Signed-off-by: Arjo Chakravarty <[email protected]>

* more style

Signed-off-by: Arjo Chakravarty <[email protected]>

* Address PR Feedback

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fix a few more outstanding issues.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Address second pass

Signed-off-by: Arjo Chakravarty <[email protected]>

* Final touches.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Retrigger CI

Signed-off-by: Arjo Chakravarty <[email protected]>

* style

Signed-off-by: Arjo Chakravarty <[email protected]>

Signed-off-by: Arjo Chakravarty <[email protected]>
  • Loading branch information
@arjo129
arjo129 authored Dec 18, 2022
1 parent 652e238 commit 31f7d5b
Show file tree
Hide file tree
Showing 6 changed files with 327 additions and 17 deletions.
179 changes: 176 additions & 3 deletions src/systems/hydrodynamics/Hydrodynamics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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;
};
Expand Down Expand Up @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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(
Expand Down
21 changes: 20 additions & 1 deletion src/systems/hydrodynamics/Hydrodynamics.hh
View file
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]
Expand All @@ -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.
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand All @@ -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;
};
Expand Down
33 changes: 29 additions & 4 deletions test/integration/hydrodynamics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -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)
Expand All @@ -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");
Expand Down Expand Up @@ -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");
Expand All @@ -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);
}
}
1 change: 1 addition & 0 deletions test/worlds/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
17 changes: 17 additions & 0 deletions test/worlds/hydrodynamic_currents.csv
View file
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 31f7d5b

Please sign in to comment.