From 8e93dec8d16ecc11038ad6247e5a3adcfbc254ea Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 17 Aug 2022 11:17:29 +0000 Subject: [PATCH 01/13] adding thrust coefficient calculation Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 79 ++++++++++++++++++++++++++++++++ src/systems/thruster/Thruster.hh | 9 ++++ 2 files changed, 88 insertions(+) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 92a74bb3a7..57f0fe94fa 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -98,15 +98,36 @@ class ignition::gazebo::systems::ThrusterPrivateData /// thrust public: double thrustCoefficient = 1; + /// \brief True if the thrust coefficient was set by conifguration. + public: bool thrustCoefficientSet = false; + + /// \brief Relative speed reduction between the water at the propeller vs + /// behind the vessel. + public: double wakeFraction = 0.2; + + /// Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coeficiennt. + public: double alpha_1 = 1; + + /// Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coeficiennt. + public: double alpha_2 = 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; + /// \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 @@ -157,6 +178,7 @@ void Thruster::Configure( if (_sdf->HasElement("thrust_coefficient")) { this->dataPtr->thrustCoefficient = _sdf->Get("thrust_coefficient"); + this->dataPtr->thrustCoefficientSet = true; } // Get propeller diameter @@ -171,6 +193,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->alpha_1 = _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->alpha_2 = _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; + } + } + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); if (kNullEntity == this->dataPtr->jointEntity) { @@ -218,6 +274,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; @@ -304,6 +362,11 @@ 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) + { + 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( @@ -316,6 +379,17 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) return propAngularVelocity; } +///////////////////////////////////////////////// +void ThrusterPrivateData::UpdateThrustCoefficient() +{ + double calculatedThrustCoefficient = this->alpha_1 + this->alpha_2 * + (((1 - this->wakeFraction) * this->linearVelocity) + / (this->propellerAngVel * this->propellerDiameter)); + if (!std::isnan(calculatedThrustCoefficient) + && calculatedThrustCoefficient!=0) + this->thrustCoefficient = calculatedThrustCoefficient; +} + ///////////////////////////////////////////////// bool ThrusterPrivateData::HasSufficientBattery( const EntityComponentManager &_ecm) const @@ -405,6 +479,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 6380eb7972..45727ed488 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -68,6 +68,15 @@ namespace systems /// defaults to 1000N] /// - - Minimum thrust command. [Optional, /// defaults to -1000N] + /// - - Relative speed reduction between the water + /// at the propeller vs behind the vessel. + /// [Optional, defults to 0.2] + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coeficiennt. + /// [Optional, defults to 1] + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coeficiennt. + /// [Optional, defults to 0] /// /// ## Example /// An example configuration is installed with Gazebo. The example From f8550f1a005930e97bb3e2cc9458c83301ef6369 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 17 Aug 2022 15:15:22 +0000 Subject: [PATCH 02/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 57f0fe94fa..00e9b5cabc 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -98,7 +98,7 @@ class ignition::gazebo::systems::ThrusterPrivateData /// thrust public: double thrustCoefficient = 1; - /// \brief True if the thrust coefficient was set by conifguration. + /// \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 From 4f34ef0e4592b4f75a3cde7f9cfc87a91f33f4d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:08 +0800 Subject: [PATCH 03/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 00e9b5cabc..c100b5dbf8 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -101,7 +101,7 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \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 + /// \brief Relative speed reduction between the water at the propeller vs /// behind the vessel. public: double wakeFraction = 0.2; From 0a6977707085c337adf30dd4274d0f56673cea4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:19 +0800 Subject: [PATCH 04/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index c100b5dbf8..175a1b94c9 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -109,8 +109,8 @@ class ignition::gazebo::systems::ThrusterPrivateData /// calculation of the thrust coeficiennt. public: double alpha_1 = 1; - /// Constant given by the open water propeller diagram. Used in the - /// calculation of the thrust coeficiennt. + /// \brief Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coefficient. public: double alpha_2 = 0; /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3 From 7afeb943b930d40096afef6fcc32e80761dd7e3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:27 +0800 Subject: [PATCH 05/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 175a1b94c9..230df20e05 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -120,7 +120,7 @@ class ignition::gazebo::systems::ThrusterPrivateData public: double propellerDiameter = 0.02; /// \brief Linear velocity of the vehicle. - public: double linearVelocity = 0; + public: double linearVelocity = 0.0; /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); From bd78fd6d077a5eb375939e43ff587bb3394cae77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:34 +0800 Subject: [PATCH 06/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 230df20e05..d56673ddcc 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -209,7 +209,7 @@ void Thruster::Configure( << "[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; + << "[thrust_coefficient] value from the SDF file." << std::endl; } } From 0ac7d989e7b68fc0557ff90d1d8afb3df16bba59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:40 +0800 Subject: [PATCH 07/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index d56673ddcc..63886465a9 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -105,7 +105,7 @@ class ignition::gazebo::systems::ThrusterPrivateData /// behind the vessel. public: double wakeFraction = 0.2; - /// Constant given by the open water propeller diagram. Used in the + /// \brief Constant given by the open water propeller diagram. Used in the /// calculation of the thrust coeficiennt. public: double alpha_1 = 1; From 87a1ae364976de2c51513035405203143fd6a5f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:47 +0800 Subject: [PATCH 08/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 63886465a9..330a6f5996 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -223,7 +223,7 @@ void Thruster::Configure( << "[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; + << "[thrust_coefficient] value from the SDF file." << std::endl; } } From fd99f004183dfddead5cb6ecb3dfb9cfc17c9fd2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:19:53 +0800 Subject: [PATCH 09/13] Update src/systems/thruster/Thruster.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 330a6f5996..f4b233c744 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -106,7 +106,7 @@ class ignition::gazebo::systems::ThrusterPrivateData public: double wakeFraction = 0.2; /// \brief Constant given by the open water propeller diagram. Used in the - /// calculation of the thrust coeficiennt. + /// calculation of the thrust coefficient. public: double alpha_1 = 1; /// \brief Constant given by the open water propeller diagram. Used in the From 0cd1d6504b20465a7acb5f956ceaafe9b60b2842 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Wed, 17 Aug 2022 23:20:01 +0800 Subject: [PATCH 10/13] Update src/systems/thruster/Thruster.hh MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 45727ed488..aa933ec78f 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -72,10 +72,10 @@ namespace systems /// at the propeller vs behind the vessel. /// [Optional, defults to 0.2] /// - - Constant given by the open water propeller diagram. Used - /// in the calculation of the thrust coeficiennt. + /// in the calculation of the thrust coefficient. /// [Optional, defults to 1] /// - - Constant given by the open water propeller diagram. Used - /// in the calculation of the thrust coeficiennt. + /// in the calculation of the thrust coefficient. /// [Optional, defults to 0] /// /// ## Example From 7351aab3928bb9c294b9c3df115b023f2c989098 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 7 Sep 2022 11:55:49 +0000 Subject: [PATCH 11/13] thrust coefficient test and behavior updates Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 20 ++--- src/systems/thruster/Thruster.hh | 13 +++- test/integration/thruster.cc | 64 ++++++++++++++-- test/worlds/thrust_coefficient.sdf | 114 +++++++++++++++++++++++++++++ 4 files changed, 192 insertions(+), 19 deletions(-) create mode 100644 test/worlds/thrust_coefficient.sdf 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 + + + + + From 51337cd036d004d73e14e96ecb362978e7089074 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 14 Sep 2022 03:54:20 +0000 Subject: [PATCH 12/13] making float comparision more robust Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 229af2ace3..2b014dc5cc 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -382,7 +382,8 @@ 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 && this->propellerAngVel !=0) + if (!this->thrustCoefficientSet && + std::abs(this->propellerAngVel) < std::numeric_limits::epsilon()) { this->UpdateThrustCoefficient(); } From b051ef0b239750454fa7467a4af0f2a7706e54da Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 15 Sep 2022 06:48:31 +0000 Subject: [PATCH 13/13] fix float comparision and lint Signed-off-by: Marco A. Gutierrez --- src/systems/thruster/Thruster.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 2b014dc5cc..bae8bb04d5 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -383,7 +384,7 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) // 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()) + std::abs(this->propellerAngVel) > std::numeric_limits::epsilon()) { this->UpdateThrustCoefficient(); } @@ -458,7 +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); + this->dataPtr->propellerAngVel = + this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust); desiredPropellerAngVel = this->dataPtr->propellerAngVel; }