diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 5d278790f8..ac7f8fdf3d 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -296,7 +296,7 @@ void Hydrodynamics::PreUpdate( // These variables follow Fossen's scheme in "Guidance and Control // of Ocean Vehicles." The `state` vector contains the ship's current velocity - // in the formate [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]. + // in the format [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]. // `stateDot` consists of the first derivative in time of the state vector. // `Cmat` corresponds to the Centripetal matrix // `Dmat` is the drag matrix @@ -340,13 +340,15 @@ void Hydrodynamics::PreUpdate( state(4) = localRotationalVelocity.Y(); state(5) = localRotationalVelocity.Z(); + // TODO(anyone) Make this configurable auto dt = static_cast(_info.dt.count())/1e9; stateDot = (state - this->dataPtr->prevState)/dt; this->dataPtr->prevState = state; // The added mass - const Eigen::VectorXd kAmassVec = this->dataPtr->Ma * stateDot; + // Negative sign signifies the behaviour change + const Eigen::VectorXd kAmassVec = - this->dataPtr->Ma * stateDot; // Coriolis and Centripetal forces for under water vehicles (Fossen P. 37) // Note: this is significantly different from VRX because we need to account diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9b79fe53ce..ad026ed661 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -24,6 +24,7 @@ set(tests force_torque_system.cc fuel_cached_server.cc halt_motion.cc + hydrodynamics.cc imu_system.cc joint_controller_system.cc joint_position_controller_system.cc diff --git a/test/integration/hydrodynamics.cc b/test/integration/hydrodynamics.cc new file mode 100644 index 0000000000..cfcab01f93 --- /dev/null +++ b/test/integration/hydrodynamics.cc @@ -0,0 +1,170 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/World.hh" + +#include "ignition/gazebo/test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class HydrodynamicsTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file + /// \param[in] _world Path to world file + /// \param[in] _namespace Namespace for topic + /// \param[in] _density Fluid density + /// \param[in] _viscosity Fluid viscosity + /// \param[in] _radius Body's radius + /// \param[in] _area Body surface area + /// \param[in] _drag_coeff Body drag coefficient + public: std::vector TestWorld(const std::string &_world, + const std::string &_namespace); +}; + +////////////////////////////////////////////////// +std::vector HydrodynamicsTest::TestWorld( + const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + ignition::common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + std::vector bodyVels; + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + // Add force + math::Vector3d force(0, 0, 10.0); + body.AddWorldForce(_ecm, force); + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto bodyVel = body.WorldLinearVelocity(_ecm); + ASSERT_TRUE(bodyVel); + bodyVels.push_back(bodyVel.value()); + }). + Finalize(); + + fixture.Server()->Run(true, 1000, false); + EXPECT_EQ(1000u, bodyVels.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return bodyVels; +} + +///////////////////////////////////////////////// +/// This test evaluates whether the hydrodynamic plugin affects the motion +/// of the body when a force is applied. +TEST_F(HydrodynamicsTest, VelocityTestinOil) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "hydrodynamics.sdf"); + + auto sphere1Vels = this->TestWorld(world, "sphere1"); + auto sphere2Vels = this->TestWorld(world, "sphere2"); + + for (unsigned int i = 0; i < 1000; ++i) + { + // Sanity check + EXPECT_FLOAT_EQ(0.0, sphere1Vels[i].X()); + EXPECT_FLOAT_EQ(0.0, sphere1Vels[i].Y()); + EXPECT_FLOAT_EQ(0.0, sphere2Vels[i].X()); + EXPECT_FLOAT_EQ(0.0, sphere2Vels[i].Y()); + + // Wait a couple of iterations for the body to move + if(i > 4) + { + EXPECT_LT(sphere1Vels[i].Z(), sphere2Vels[i].Z()); + + if (i > 900) + { + // Expect for the velocity to stabilize + EXPECT_NEAR(sphere1Vels[i-1].Z(), sphere1Vels[i].Z(), 1e-6); + EXPECT_NEAR(sphere2Vels[i-1].Z(), sphere2Vels[i].Z(), 1e-6); + } + } + } +} + +///////////////////////////////////////////////// +/// This test makes sure that the transforms of the hydrodynamics +/// plugin are correct by comparing 3 cylinders in different +/// positions and orientations. +TEST_F(HydrodynamicsTest, TransformsTestinWater) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "hydrodynamics.sdf"); + + auto cylinder1Vels = this->TestWorld(world, "cylinder1"); + auto cylinder2Vels = this->TestWorld(world, "cylinder2"); + auto cylinder3Vels = this->TestWorld(world, "cylinder3"); + + for (unsigned int i = 900; i < 1000; ++i) + { + // Expect for the velocity to stabilize + EXPECT_NEAR(cylinder1Vels[i-1].Z(), cylinder1Vels[i].Z(), 1e-6); + EXPECT_NEAR(cylinder2Vels[i-1].Z(), cylinder2Vels[i].Z(), 1e-6); + EXPECT_NEAR(cylinder3Vels[i-1].Z(), cylinder3Vels[i].Z(), 1e-6); + + // Expect for final velocities to be similar + EXPECT_NEAR(cylinder1Vels[i].Z(), cylinder2Vels[i].Z(), 1e-4); + EXPECT_NEAR(cylinder2Vels[i].Z(), cylinder3Vels[i].Z(), 1e-4); + } +} diff --git a/test/worlds/hydrodynamics.sdf b/test/worlds/hydrodynamics.sdf new file mode 100644 index 0000000000..234d6314ea --- /dev/null +++ b/test/worlds/hydrodynamics.sdf @@ -0,0 +1,307 @@ + + + + + + 0.001 + + 0 + + + + 0 0 0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 1 0.25 0 0 0 0 + + 25 + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + + + 1 -0.25 0 0 0 0 + + 25 + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + sphere2_link + 918 + 0 + 0 + 15.381 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 11.5359 + 0.211869 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + + 40 + + 0.8 + 0 + 0 + 0.8 + 0 + 0.8 + + + + + + + 0.2 + 1 + + + + + + + 0.2 + 0.1 + + + + + + + cylinder1_link + 1000 + 0 + 0 + 12.5664 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 94.2475 + 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + -1 0.5 0 45 0 45 + + 40 + + 0.8 + 0 + 0 + 0.8 + 0 + 0.8 + + + + + + + 0.2 + 1 + + + + + + + 0.2 + 0.1 + + + + + + + cylinder2_link + 1000 + 0 + 0 + 12.5664 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 94.2475 + 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + -1 -0.5 0 0 90 0 + + 40 + + 0.8 + 0 + 0 + 0.8 + 0 + 0.8 + + + + + + + 0.2 + 1 + + + + + + + 0.2 + 0.1 + + + + + + + cylinder3_link + 1000 + 0 + 0 + 12.5664 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 94.2475 + 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + +