diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 4e995444db..229af2ace3 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -107,11 +107,11 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Constant given by the open water propeller diagram. Used in the /// calculation of the thrust coefficient. - public: double alpha_1 = 1; + public: double alpha1 = 1; /// \brief Constant given by the open water propeller diagram. Used in the /// calculation of the thrust coefficient. - public: double alpha_2 = 0; + public: double alpha2 = 0; /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3 public: double fluidDensity = 1000; @@ -205,7 +205,7 @@ void Thruster::Configure( // Get alpha_1, default to 1 othwewise if (_sdf->HasElement("alpha_1")) { - this->dataPtr->alpha_1 = _sdf->Get("alpha_1"); + this->dataPtr->alpha1 = _sdf->Get("alpha_1"); if (this->dataPtr->thrustCoefficientSet) { ignwarn << " The [alpha_2] value will be ignored as a " @@ -219,7 +219,7 @@ void Thruster::Configure( // Get alpha_2, default to 1 othwewise if (_sdf->HasElement("alpha_2")) { - this->dataPtr->alpha_2 = _sdf->Get("alpha_2"); + this->dataPtr->alpha2 = _sdf->Get("alpha_2"); if (this->dataPtr->thrustCoefficientSet) { ignwarn << " The [alpha_2] value will be ignored as a " @@ -379,8 +379,10 @@ void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg) ///////////////////////////////////////////////// double ThrusterPrivateData::ThrustToAngularVec(double _thrust) { - // Only update if the thrust coefficient was not set by configuration. - if (!this->thrustCoefficientSet) + // 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 && this->propellerAngVel !=0) { this->UpdateThrustCoefficient(); } @@ -399,12 +401,9 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) ///////////////////////////////////////////////// void ThrusterPrivateData::UpdateThrustCoefficient() { - double calculatedThrustCoefficient = this->alpha_1 + this->alpha_2 * + this->thrustCoefficient = this->alpha1 + this->alpha2 * (((1 - this->wakeFraction) * this->linearVelocity) / (this->propellerAngVel * this->propellerDiameter)); - if (!std::isnan(calculatedThrustCoefficient) - && calculatedThrustCoefficient!=0) - this->thrustCoefficient = calculatedThrustCoefficient; } ///////////////////////////////////////////////// @@ -458,6 +457,7 @@ 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; } diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index eeba1d3bf2..e40627d465 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -71,14 +71,21 @@ namespace systems /// - - Minimum thrust command. [Optional, /// defaults to -1000N] /// - - Relative speed reduction between the water - /// at the propeller vs behind the vessel. + /// 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. + /// 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. + /// 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 + + + + +