Skip to content

Commit

Permalink
thrust coefficient test and behavior updates
Browse files Browse the repository at this point in the history 
Signed-off-by: Marco A. Gutierrez <marco@openrobotics.org>
  • Loading branch information
Marco A. Gutierrez committed Sep 7, 2022
1 parent 79abb52 commit 7351aab
Show file tree
Hide file tree
Showing 4 changed files with 192 additions and 19 deletions.
20 changes: 10 additions & 10 deletions src/systems/thruster/Thruster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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 "
Expand All @@ -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 "
Expand Down Expand Up @@ -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();
}
Expand All @@ -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;
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -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;
}

Expand Down
13 changes: 10 additions & 3 deletions src/systems/thruster/Thruster.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
64 changes: 58 additions & 6 deletions test/integration/thruster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -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;
Expand All @@ -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(
Expand All @@ -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)
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
View file
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>

0 comments on commit 7351aab

Please sign in to comment.