diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index b775cf26e4..bae8bb04d5 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -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("thrust_coefficient"); + this->dataPtr->thrustCoefficientSet = true; } // Get propeller diameter @@ -174,6 +197,40 @@ void Thruster::Configure( this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } + // Get wake fraction number, default 0.2 otherwise + if (_sdf->HasElement("wake_fraction")) + { + this->dataPtr->wakeFraction = _sdf->Get("wake_fraction"); + } + + // Get alpha_1, default to 1 othwewise + if (_sdf->HasElement("alpha_1")) + { + this->dataPtr->alpha1 = _sdf->Get("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("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(_ecm, this->dataPtr->linkEntity); enableComponent(_ecm, this->dataPtr->linkEntity); + enableComponent(_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::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 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( + this->dataPtr->linkEntity)->Data().Length(); } ///////////////////////////////////////////////// diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 415fa8b660..e40627d465 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -70,6 +70,22 @@ namespace systems /// defaults to 1000N] /// - - Minimum thrust command. [Optional, /// defaults to -1000N] + /// - - 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 + /// + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 1] + /// - - 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 diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 1e147bfe69..92c60a7d89 100644 --- a/test/integration/thruster.cc +++ b/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 modelPoses; std::vector propellerAngVels; + std::vector 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( + jointEntity)->Data().Xyz(); + jointPose = _ecm.Component( + 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,6 +117,10 @@ 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(); @@ -106,6 +128,7 @@ void ThrusterTest::TestWorld(const std::string &_world, 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); +} diff --git a/test/worlds/thrust_coefficient.sdf b/test/worlds/thrust_coefficient.sdf new file mode 100644 index 0000000000..f45c5cb1c7 --- /dev/null +++ b/test/worlds/thrust_coefficient.sdf @@ -0,0 +1,114 @@ + + + + + + + 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 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + custom + propeller_joint + 950 + 0.25 + true + 300 + 0 + 0.2 + 0.9 + 0.01 + + + + +