Enable/Disable individual hydrodynamic components. (#1692)

This commit enables and disables individual components of the hydrodynamics. This is often useful for debugging odd behaviours of a hydrodynamic model.

src/systems/hydrodynamics/Hydrodynamics.cc

@@ -103,6 +103,14 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData
   /// \brief The ignition transport node
   public: transport::Node node;
 
+  /// \brief Plugin Parameter: Disable coriolis as part of equation. This is
+  /// occasionally useful for testing.
+  public: bool disableCoriolis = false;
+
+  /// \brief Plugin Parameter: Disable added mass as part of equation. This is
+  /// occasionally useful for testing.
+  public: bool disableAddedMass = false;
+
   /// \brief Ocean current experienced by this body
   public: math::Vector3d currentVector {0, 0, 0};
 
@@ -232,6 +240,9 @@ void Hydrodynamics::Configure(
   this->dataPtr->paramNr          = SdfParamDouble(_sdf, "nR"          , 20);
   this->dataPtr->paramNrr         = SdfParamDouble(_sdf, "nRR"         , 0);
 
+  _sdf->Get<bool>("disable_coriolis", this->dataPtr->disableCoriolis, false);
+  _sdf->Get<bool>("disable_added_mass", this->dataPtr->disableAddedMass, false);
+
   // Create model object, to access convenient functions
   auto model = ignition::gazebo::Model(_entity);
 
@@ -389,7 +400,16 @@ void Hydrodynamics::PreUpdate(
 
   const Eigen::VectorXd kDvec = Dmat * state;
 
-  const Eigen::VectorXd kTotalWrench = kAmassVec + kDvec + kCmatVec;
+  Eigen::VectorXd kTotalWrench = kDvec;
+
+  if (!this->dataPtr->disableAddedMass)
+  {
+    kTotalWrench += kAmassVec;
+  }
+  if (!this->dataPtr->disableCoriolis)
+  {
+    kTotalWrench += kCmatVec;
+  }
 
   ignition::math::Vector3d
     totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));

src/systems/hydrodynamics/Hydrodynamics.hh

@@ -77,6 +77,9 @@ namespace systems
   ///     listens on `/model/{namespace name}/ocean_current`.[String, Optional]
   ///   * <default_current> - A generic current.
   ///      [vector3d m/s, optional, default = [0,0,0]m/s]
+  ///   * <disable_coriolis> - Disable Coriolis force [Boolean, Default: false]
+  ///   * <disable_added_mass> - Disable Added Mass [Boolean, Default: false].
+  ///     To be deprecated in Garden.
   ///
   /// # Example
   /// An example configuration is provided in the examples folder. The example

test/integration/CMakeLists.txt

@@ -25,6 +25,7 @@ set(tests
   fuel_cached_server.cc
   halt_motion.cc
   hydrodynamics.cc
+  hydrodynamics_flags.cc
   imu_system.cc
   joint_controller_system.cc
   joint_position_controller_system.cc

test/integration/hydrodynamics_flags.cc

@@ -0,0 +1,151 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include <ignition/msgs/double.pb.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/common/Util.hh>
+#include <ignition/transport/Node.hh>
+#include <ignition/utils/ExtraTestMacros.hh>
+
+#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 HydrodynamicsFlagsTest : public InternalFixture<::testing::Test>
+{
+  /// \brief Test a world file.
+  /// \param[in] _world Path to world file.
+  /// \param[in] _modelName Name of the model.
+  /// \param[in] _numsteps Number of steps to run the server.
+  /// \param[out] _linearVel Linear velocityies after each step.
+  /// \param[out] _angularVel Linear velocityies after each step.
+  public: void TestWorld(const std::string &_world,
+   const std::string &_modelName, const unsigned int &_numsteps,
+   std::vector<math::Vector3d> &_linearVel,
+   std::vector<math::Vector3d> &_angularVel);
+};
+
+//////////////////////////////////////////////////
+void HydrodynamicsFlagsTest::TestWorld(const std::string &_world,
+   const std::string &_modelName, const unsigned int &_numsteps,
+   std::vector<math::Vector3d> &_linearVel,
+  std::vector<math::Vector3d> &_angularVel)
+{
+  // 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<math::Vector3d> bodyVels;
+  fixture.
+  OnConfigure(
+    [&](const Entity &_worldEntity,
+      const std::shared_ptr<const sdf::Element> &/*_sdf*/,
+      EntityComponentManager &_ecm,
+      EventManager &/*eventMgr*/)
+    {
+      World world(_worldEntity);
+
+      auto modelEntity =  world.ModelByName(_ecm, _modelName);
+      EXPECT_NE(modelEntity, kNullEntity);
+      model = Model(modelEntity);
+
+      auto bodyEntity = model.LinkByName(_ecm, "base_link");
+      EXPECT_NE(bodyEntity, kNullEntity);
+
+      body = Link(bodyEntity);
+      body.EnableVelocityChecks(_ecm);
+
+      // Forces in Y and Z are needed to make the coriolis
+      // force appear.
+      math::Vector3d force(0.0, 1000.0, 1000.0);
+      math::Vector3d torque(0.0, 0.0, 10.0);
+      body.AddWorldWrench(_ecm, force, torque);
+
+    }).
+  OnPostUpdate([&](const UpdateInfo &/*_info*/,
+                            const EntityComponentManager &_ecm)
+    {
+      auto bodyVel = body.WorldLinearVelocity(_ecm);
+      ASSERT_TRUE(bodyVel);
+      _linearVel.push_back(bodyVel.value());
+      auto bodyAngularVel = body.WorldAngularVelocity(_ecm);
+      ASSERT_TRUE(bodyAngularVel);
+      _angularVel.push_back(bodyAngularVel.value());
+    }).
+  Finalize();
+
+  fixture.Server()->Run(true, _numsteps, false);
+  EXPECT_EQ(_numsteps, _linearVel.size());
+  EXPECT_EQ(_numsteps, _angularVel.size());
+
+  EXPECT_NE(model.Entity(), kNullEntity);
+  EXPECT_NE(body.Entity(), kNullEntity);
+
+}
+
+/////////////////////////////////////////////////
+/// This test makes sure that the linear velocity is reuduced
+/// disbling the coriolis force and also when disabling the added mass.
+TEST_F(HydrodynamicsFlagsTest, AddedMassCoriolisFlags)
+{
+  auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "hydrodynamics_flags.sdf");
+
+  unsigned int numsteps = 1000;
+
+  std::vector<math::Vector3d> angularVels, angularCoriolisVels,
+      angularAddedMassVels;
+  std::vector<math::Vector3d> linearVels, linearCoriolisVels,
+      linearAddedMassVels;
+
+  this->TestWorld(world, "tethys", numsteps, linearVels, angularVels);
+  this->TestWorld(world, "triton", numsteps, linearCoriolisVels,
+                  angularCoriolisVels);
+  this->TestWorld(world, "daphne", numsteps, linearAddedMassVels,
+                  angularAddedMassVels);
+
+  // Wait a couple of iterations for the body to move
+  for (unsigned int i = 4; i < numsteps; ++i)
+  {
+    // Angular and linear velocity should be lower
+    // with the produced coriolis and added mass
+    EXPECT_LT(angularCoriolisVels[i].Z(), angularVels[i].Z());
+    EXPECT_LT(linearCoriolisVels[i].Z(), linearVels[i].Z());
+    EXPECT_LT(angularAddedMassVels[i].Z(), angularVels[i].Z());
+    EXPECT_LT(linearAddedMassVels[i].Z(), linearVels[i].Z());
+  }
+}