Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding thrust coefficient calculation #1652

Merged
merged 14 commits into from
Oct 6, 2022
Merged
Prev Previous commit
Next Next commit
thrust coefficient test and behavior updates
Signed-off-by: Marco A. Gutierrez <[email protected]>
Marco A. Gutierrez committed Sep 7, 2022

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
commit 7351aab3928bb9c294b9c3df115b023f2c989098
20 changes: 10 additions & 10 deletions src/systems/thruster/Thruster.cc
Original file line number Diff line number Diff line change
@@ -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<double>("alpha_1");
this->dataPtr->alpha1 = _sdf->Get<double>("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<double>("alpha_2");
this->dataPtr->alpha2 = _sdf->Get<double>("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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@marcoag could the thrust coefficient ever become zero while moving (in other words, could either α 1 or α 2 be negative)? If so, maybe we should reset thrust coefficient to one if propeller angular velocity ever becomes zero. WDYT?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, I'm surprised the compiler didn't complain about a floating point comparison to zero. AFAIK std::abs(this->propellerAngVel) < std::numeric_limits<double>::epsilon() would be a slightly more robust test.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It did threw a warning, I assumed anything that is not a strict zero would work.

Changed to your suggestion: 51337cd.

{
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<std::mutex> lock(this->dataPtr->mtx);
desiredThrust = this->dataPtr->thrust;
this->dataPtr->propellerAngVel = this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust);
desiredPropellerAngVel = this->dataPtr->propellerAngVel;
}

13 changes: 10 additions & 3 deletions src/systems/thruster/Thruster.hh
Original file line number Diff line number Diff line change
@@ -71,14 +71,21 @@ namespace systems
/// - <min_thrust_cmd> - Minimum thrust command. [Optional,
/// defaults to -1000N]
/// - <wake_fraction> - 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
///
/// - <alpha_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 1]
/// - <alpha_2> - 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
64 changes: 58 additions & 6 deletions test/integration/thruster.cc
Original file line number Diff line number Diff line change
@@ -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);
}
114 changes: 114 additions & 0 deletions test/worlds/thrust_coefficient.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="thruster">

<physics name="fast" type="none">
<!-- Zero to run as fast as possible -->
<real_time_factor>0</real_time_factor>
</physics>

<!-- prevent sinking -->
<gravity>0 0 0</gravity>

<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="sub">

<link name="body">
<pose>0 0 0 0 1.57 0</pose>
<inertial>
<mass>100</mass>
<inertia>
<ixx>33.89</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>33.89</iyy>
<iyz>0</iyz>
<izz>1.125</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<length>2</length>
<radius>0.15</radius>
</cylinder>
</geometry>
</visual>
</link>

<link name="propeller">
<pose>-1.05 0 0 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000354167</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000021667</iyy>
<iyz>0</iyz>
<izz>0.000334167</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.01 0.25 0.05</size>
</box>
</geometry>
</visual>
</link>

<joint name="propeller_joint" type="revolute">
<parent>body</parent>
<child>propeller</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1e+12</lower>
<upper>1e+12</upper>
<effort>1e6</effort>
<velocity>1e6</velocity>
</limit>
</axis>
</joint>

<plugin
filename="ignition-gazebo-thruster-system"
name="ignition::gazebo::systems::Thruster">
<namespace>custom</namespace>
<joint_name>propeller_joint</joint_name>
<fluid_density>950</fluid_density>
<propeller_diameter>0.25</propeller_diameter>
<velocity_control>true</velocity_control>
<max_thrust_cmd>300</max_thrust_cmd>
<min_thrust_cmd>0</min_thrust_cmd>
<wake_fraction>0.2</wake_fraction>
<alpha_1>0.9</alpha_1>
<alpha_2>0.01</alpha_2>
</plugin>
</model>

</world>
</sdf>