Adding thrust coefficient calculation

src/systems/thruster/Thruster.cc

@@ -16,6 +16,7 @@
  */
 #include <memory>
 #include <mutex>
+#include <limits>
 #include <string>
 
 #include <ignition/msgs/double.pb.h>
@@ -98,18 +99,39 @@ class ignition::gazebo::systems::ThrusterPrivateData
   /// thrust
   public: double thrustCoefficient = 1;
 
+  /// \brief True if the thrust coefficient was set by configuration.
+  public: bool thrustCoefficientSet = false;
+
+  /// \brief Relative speed reduction between the water at the propeller vs
+  /// behind the vessel.
+  public: double wakeFraction = 0.2;
+
+  /// \brief Constant given by the open water propeller diagram. Used in the
+  /// calculation of the thrust coefficient.
+  public: double alpha1 = 1;
+
+  /// \brief Constant given by the open water propeller diagram. Used in the
+  /// calculation of the thrust coefficient.
+  public: double alpha2 = 0;
+
   /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3
   public: double fluidDensity = 1000;
 
   /// \brief Diameter of propeller in m, default: 0.02
   public: double propellerDiameter = 0.02;
 
+  /// \brief Linear velocity of the vehicle.
+  public: double linearVelocity = 0.0;
+
   /// \brief Topic name used to control thrust.
   public: std::string topic = "";
 
   /// \brief Callback for handling thrust update
   public: void OnCmdThrust(const msgs::Double &_msg);
 
+  /// \brief Recalculates and updates the thrust coefficient.
+  public: void UpdateThrustCoefficient();
+
   /// \brief Function which computes angular velocity from thrust
   /// \param[in] _thrust Thrust in N
   /// \return Angular velocity in rad/s
@@ -160,6 +182,7 @@ void Thruster::Configure(
   if (_sdf->HasElement("thrust_coefficient"))
   {
     this->dataPtr->thrustCoefficient = _sdf->Get<double>("thrust_coefficient");
+    this->dataPtr->thrustCoefficientSet = true;
   }
 
   // Get propeller diameter
@@ -174,6 +197,40 @@ void Thruster::Configure(
     this->dataPtr->fluidDensity = _sdf->Get<double>("fluid_density");
   }
 
+  // Get wake fraction number, default 0.2 otherwise
+  if (_sdf->HasElement("wake_fraction"))
+  {
+    this->dataPtr->wakeFraction = _sdf->Get<double>("wake_fraction");
+  }
+
+  // Get alpha_1, default to 1 othwewise
+  if (_sdf->HasElement("alpha_1"))
+  {
+    this->dataPtr->alpha1 = _sdf->Get<double>("alpha_1");
+    if (this->dataPtr->thrustCoefficientSet)
+    {
+      ignwarn << " The [alpha_2] value will be ignored as a "
+              << "[thrust_coefficient] was also defined through the SDF file."
+              << " If you want the system to use the alpha values to calculate"
+              << " and update the thrust coefficient please remove the "
+              << "[thrust_coefficient] value from the SDF file." << std::endl;
+    }
+  }
+
+  // Get alpha_2, default to 1 othwewise
+  if (_sdf->HasElement("alpha_2"))
+  {
+    this->dataPtr->alpha2 = _sdf->Get<double>("alpha_2");
+    if (this->dataPtr->thrustCoefficientSet)
+    {
+      ignwarn << " The [alpha_2] value will be ignored as a "
+              << "[thrust_coefficient] was also defined through the SDF file."
+              << " If you want the system to use the alpha values to calculate"
+              << " and update the thrust coefficient please remove the "
+              << "[thrust_coefficient] value from the SDF file." << std::endl;
+    }
+  }
+
   // Get a custom topic.
   if (_sdf->HasElement("topic"))
   {
@@ -235,6 +292,8 @@ void Thruster::Configure(
   enableComponent<components::AngularVelocity>(_ecm, this->dataPtr->linkEntity);
   enableComponent<components::WorldAngularVelocity>(_ecm,
       this->dataPtr->linkEntity);
+  enableComponent<ignition::gazebo::components::WorldLinearVelocity>(_ecm,
+      this->dataPtr->linkEntity);
 
   double minThrustCmd = this->dataPtr->cmdMin;
   double maxThrustCmd = this->dataPtr->cmdMax;
@@ -321,6 +380,14 @@ void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg)
 /////////////////////////////////////////////////
 double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
 {
+  // Only update if the thrust coefficient was not set by configuration
+  // and angular velocity is not zero. Some velocity is needed to calculate
+  // the thrust coefficient otherwise it will never start moving.
+  if (!this->thrustCoefficientSet &&
+      std::abs(this->propellerAngVel) > std::numeric_limits<double>::epsilon())
+  {
+    this->UpdateThrustCoefficient();
+  }
   // Thrust is proportional to the Rotation Rate squared
   // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
   auto propAngularVelocity = sqrt(abs(
@@ -333,6 +400,14 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
   return propAngularVelocity;
 }
 
+/////////////////////////////////////////////////
+void ThrusterPrivateData::UpdateThrustCoefficient()
+{
+  this->thrustCoefficient = this->alpha1 + this->alpha2 *
+      (((1 - this->wakeFraction) * this->linearVelocity)
+      / (this->propellerAngVel * this->propellerDiameter));
+}
+
 /////////////////////////////////////////////////
 bool ThrusterPrivateData::HasSufficientBattery(
   const EntityComponentManager &_ecm) const
@@ -384,6 +459,8 @@ void Thruster::PreUpdate(
   {
     std::lock_guard<std::mutex> lock(this->dataPtr->mtx);
     desiredThrust = this->dataPtr->thrust;
+    this->dataPtr->propellerAngVel =
+        this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust);
     desiredPropellerAngVel = this->dataPtr->propellerAngVel;
   }
 
@@ -422,6 +499,11 @@ void Thruster::PreUpdate(
     _ecm,
     unitVector * desiredThrust,
     unitVector * torque);
+
+  // Update the LinearVelocity of the vehicle
+  this->dataPtr->linearVelocity =
+      _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
+      this->dataPtr->linkEntity)->Data().Length();
 }
 
 /////////////////////////////////////////////////

src/systems/thruster/Thruster.hh

@@ -70,6 +70,22 @@ namespace systems
   ///                      defaults to 1000N]
   /// - <min_thrust_cmd> - Minimum thrust command. [Optional,
   ///                      defaults to -1000N]
+  /// - <wake_fraction>  - Relative speed reduction between the water
+  ///                      at the propeller (Va) vs behind the vessel.
+  ///                      [Optional, defults to 0.2]
+  /// See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 95:
+  ///
+  ///                Va = (1 - wake_fraction) * advance_speed
+  ///
+  /// - <alpha_1> - Constant given by the open water propeller diagram. Used
+  ///               in the calculation of the thrust coefficient (Kt).
+  ///               [Optional, defults to 1]
+  /// - <alpha_2> - Constant given by the open water propeller diagram. Used
+  ///               in the calculation of the thrust coefficient (Kt).
+  ///               [Optional, defults to 0]
+  /// See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 95:
+  ///
+  /// Kt = alpha_1 * alpha_2 * (Va/(propeller_revolution * propeller_diameter))
   ///
   /// ## Example
   /// An example configuration is installed with Gazebo. The example

test/integration/thruster.cc

@@ -32,6 +32,9 @@
 #include "ignition/gazebo/Util.hh"
 #include "ignition/gazebo/World.hh"
 
+#include "ignition/gazebo/components/JointAxis.hh"
+#include "ignition/gazebo/components/Pose.hh"
+
 #include "ignition/gazebo/test_config.hh"
 #include "../helpers/EnvTestFixture.hh"
 
@@ -50,13 +53,17 @@ class ThrusterTest : public InternalFixture<::testing::Test>
   /// \param[in] _baseTol Base tolerance for most quantities
   public: void TestWorld(const std::string &_world,
       const std::string &_namespace, const std::string &_topic,
-      double _coefficient, double _density, double _diameter, double _baseTol);
+      double _thrustCoefficient, double _density, double _diameter,
+      double _baseTol, double _wakeFraction = 0.2, double _alpha_1 = 1,
+      double _alpha_2 = 0, bool _calculateCoefficient = false);
 };
 
 //////////////////////////////////////////////////
 void ThrusterTest::TestWorld(const std::string &_world,
     const std::string &_namespace, const std::string &_topic,
-    double _coefficient, double _density, double _diameter, double _baseTol)
+    double _thrustCoefficient, double _density, double _diameter,
+    double _baseTol, double _wakeFraction, double _alpha1, double _alpha2,
+    bool _calculateCoefficient)
 {
   // Start server
   ServerConfig serverConfig;
@@ -68,6 +75,9 @@ void ThrusterTest::TestWorld(const std::string &_world,
   Link propeller;
   std::vector<math::Pose3d> modelPoses;
   std::vector<math::Vector3d> propellerAngVels;
+  std::vector<math::Vector3d> propellerLinVels;
+  ignition::math::Pose3d jointPose, linkWorldPose;
+  ignition::math::Vector3d jointAxis;
   double dt{0.0};
   fixture.
   OnConfigure(
@@ -82,11 +92,19 @@ void ThrusterTest::TestWorld(const std::string &_world,
       EXPECT_NE(modelEntity, kNullEntity);
       model = Model(modelEntity);
 
+      auto jointEntity = model.JointByName(_ecm, "propeller_joint");
+      jointAxis =
+        _ecm.Component<ignition::gazebo::components::JointAxis>(
+        jointEntity)->Data().Xyz();
+      jointPose = _ecm.Component<components::Pose>(
+          jointEntity)->Data();
+
       auto propellerEntity = model.LinkByName(_ecm, "propeller");
       EXPECT_NE(propellerEntity, kNullEntity);
 
       propeller = Link(propellerEntity);
       propeller.EnableVelocityChecks(_ecm);
+
     }).
   OnPostUpdate([&](const gazebo::UpdateInfo &_info,
                             const gazebo::EntityComponentManager &_ecm)
@@ -99,13 +117,18 @@ void ThrusterTest::TestWorld(const std::string &_world,
       auto propellerAngVel = propeller.WorldAngularVelocity(_ecm);
       ASSERT_TRUE(propellerAngVel);
       propellerAngVels.push_back(propellerAngVel.value());
+
+      auto proellerLinVel = propeller.WorldLinearVelocity(_ecm);
+      ASSERT_TRUE(proellerLinVel);
+      propellerLinVels.push_back(proellerLinVel.value());
     }).
   Finalize();
 
   // Check initial position
   fixture.Server()->Run(true, 100, false);
   EXPECT_EQ(100u, modelPoses.size());
   EXPECT_EQ(100u, propellerAngVels.size());
+  EXPECT_EQ(100u, propellerLinVels.size());
 
   EXPECT_NE(model.Entity(), kNullEntity);
   EXPECT_NE(propeller.Entity(), kNullEntity);
@@ -120,6 +143,7 @@ void ThrusterTest::TestWorld(const std::string &_world,
     EXPECT_EQ(math::Vector3d::Zero, vel);
   }
   propellerAngVels.clear();
+  propellerLinVels.clear();
 
   // Publish command and check that vehicle moved
   transport::Node node;
@@ -146,8 +170,10 @@ void ThrusterTest::TestWorld(const std::string &_world,
   EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X());
   EXPECT_EQ(100u, modelPoses.size());
   EXPECT_EQ(100u, propellerAngVels.size());
+  EXPECT_EQ(100u, propellerLinVels.size());
   modelPoses.clear();
   propellerAngVels.clear();
+  propellerLinVels.clear();
 
   // input force cmd this should be capped to 300
   forceCmd = 1000.0;
@@ -168,6 +194,7 @@ void ThrusterTest::TestWorld(const std::string &_world,
 
     EXPECT_EQ(100u * sleep, modelPoses.size());
     EXPECT_EQ(100u * sleep, propellerAngVels.size());
+    EXPECT_EQ(100u * sleep, propellerLinVels.size());
   }
 
   // max allowed force
@@ -205,13 +232,26 @@ void ThrusterTest::TestWorld(const std::string &_world,
       EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol);
   }
 
-  // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
-  // omega = sqrt(thrust /
-  //     (fluid_density * thrust_coefficient * propeller_diameter ^ 4))
-  auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4)));
+  auto jointWorldPose = linkWorldPose * jointPose;
+  auto unitVector = jointWorldPose.Rot().RotateVector(jointAxis).Normalize();
+
   double omegaTol{1e-1};
   for (unsigned int i = 0; i < propellerAngVels.size(); ++i)
   {
+    auto angularVelocity = propellerAngVels[i].Dot(unitVector);
+    // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246:
+    // thrust_coefficient = alpha_1 + alpha_2 * (((1-wake_fraction) *
+    //    linear_velocity) / (angular_velocity * propeller_diameter))
+    // omega = sqrt(thrust /
+    //     (fluid_density * thrust_coefficient * propeller_diameter ^ 4))
+    if (_calculateCoefficient && angularVelocity != 0)
+    {
+      _thrustCoefficient = _alpha1 + _alpha2 * (((1 - _wakeFraction) *
+          propellerLinVels[i].Length()) / (angularVelocity * _diameter));
+    }
+    auto omega = sqrt(abs(force / (_density * _thrustCoefficient *
+        pow(_diameter, 4))));
+
     auto angVel = propellerAngVels[i];
     // It takes a few iterations to reach the speed
     if (i > 25)
@@ -270,3 +310,15 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration))
   this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2);
 }
 
+/////////////////////////////////////////////////
+TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ThrustCoefficient))
+{
+  const std::string ns = "custom";
+  const std::string topic = "/model/" + ns +
+      "/joint/propeller_joint/cmd_thrust";
+  auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "thrust_coefficient.sdf");
+
+  // Tolerance is high because the joint command disturbs the vehicle body
+  this->TestWorld(world, ns, topic, 1, 950, 0.25, 1e-2, 0.2, 0.9, 0.01, true);
+}