Skip to content

Preserve sign of thrust_coefficient #1402

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.

Sign up for GitHub

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

Merged
merged 5 commits into from
Mar 23, 2022
Merged

Preserve sign of thrust_coefficient #1402

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
bb7d6f9
Preserve sign of thrust_coefficient
clydemcqueen Mar 19, 2022
91790e8
Support negative thrust_coefficient, add 1 test
clydemcqueen Mar 19, 2022
fc3fb7f
Test clockwise thruster with ang vel input
clydemcqueen Mar 21, 2022
14c0dac
Doc: note support for cw thrusters; clarify meaning of min/max cmd pa…
clydemcqueen Mar 21, 2022
254067c
Swap cw and ccw, note frame of reference
clydemcqueen Mar 23, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions src/systems/thruster/Thruster.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,12 +92,12 @@ class ignition::gazebo::systems::ThrusterPrivateData
/// and writes the angular velocity directly to the joint. default: false
public: bool velocityControl = false;

/// \brief Maximum input force [N] for the propellerController,
/// default: 1000N
/// \brief Maximum input force [N] or angular velocity [rad/s] for the
/// propellerController, default: 1000
public: double cmdMax = 1000;

/// \brief Minimum input force [N] for the propellerController,
/// default: -1000N
/// \brief Minimum input force [N] or angular velocity [rad/s] for the
/// propellerController, default: -1000
public: double cmdMin = -1000;

/// \brief Thrust coefficient relating the propeller angular velocity to the
Expand Down Expand Up @@ -371,7 +371,7 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
(this->fluidDensity
* this->thrustCoefficient * pow(this->propellerDiameter, 4))));

propAngularVelocity *= (_thrust > 0) ? 1: -1;
propAngularVelocity *= (_thrust * this->thrustCoefficient > 0) ? 1: -1;

return propAngularVelocity;
}
Expand Down
17 changes: 10 additions & 7 deletions src/systems/thruster/Thruster.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,15 +53,18 @@ namespace systems
/// [Optional]
/// - <joint_name> - This is the joint in the model which corresponds to the
/// propeller. [Required]
/// - <use_angvel_cmd> - Default false, if set to true will make the thruster
/// - <use_angvel_cmd> - If set to true will make the thruster
/// plugin accept commands in angular velocity in radians per seconds in
/// terms of newtons. [Optional, Boolean, Default True]
/// terms of newtons. [Optional, Boolean, defaults to false]
/// - <fluid_density> - The fluid density of the liquid in which the thruster
/// is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3]
/// - <propeller_diameter> - The diameter of the propeller in meters.
/// [Optional, m, defaults to 0.02m]
/// - <thrust_coefficient> - This is the coefficient which relates the angular
/// velocity to actual thrust. [Optional, no units, defaults to 1.0]
/// velocity to actual thrust. A positive coefficient spins the propeller
/// counter-clockwise for a positive thrust force; a negative coefficient
/// spins the propeller clockwise for positive thrust force. [Optional,
/// no units, defaults to 1.0]
///
/// omega = sqrt(thrust /
/// (fluid_density * thrust_coefficient * propeller_diameter ^ 4))
Expand All @@ -76,10 +79,10 @@ namespace systems
/// no units, defaults to 0.0]
/// - <d_gain> - Derivative gain for joint PID controller. [Optional,
/// no units, defaults to 0.0]
/// - <max_thrust_cmd> - Maximum thrust command. [Optional,
/// defaults to 1000N]
/// - <min_thrust_cmd> - Minimum thrust command. [Optional,
/// defaults to -1000N]
/// - <max_thrust_cmd> - Maximum input thrust or angular velocity command.
/// [Optional, defaults to 1000N or 1000rad/s]
/// - <min_thrust_cmd> - Minimum input thrust or angular velocity command.
/// [Optional, defaults to -1000N or -1000rad/s]
///
/// ## Example
/// An example configuration is installed with Gazebo. The example
Expand Down
39 changes: 34 additions & 5 deletions test/integration/thruster.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,10 +147,16 @@ void ThrusterTest::TestWorld(const std::string &_world,
EXPECT_LT(sleep, maxSleep);
EXPECT_TRUE(pub.HasConnections());

// input force cmd - this should be capped to 0
double forceCmd{-1000.0};
// Test the cmd limits specified in the world file. These should be:
// if (use_angvel_cmd && thrust_coefficient < 0):
// min_thrust = -300
// max_thrust = 0
// else:
// min_thrust = 0
// max_thrust = 300
double invalidCmd = (_useAngVelCmd && _coefficient < 0) ? 1000 : -1000;
msgs::Double msg;
msg.set_data(forceCmd);
msg.set_data(invalidCmd);
pub.Publish(msg);

// Check no movement
Expand All @@ -168,7 +174,9 @@ void ThrusterTest::TestWorld(const std::string &_world,
// 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 omega = sqrt(abs(force / (_density * _coefficient * pow(_diameter, 4))));
// Account for negative thrust and/or negative thrust coefficient
omega *= (force * _coefficient > 0 ? 1 : -1);

msg.Clear();
if(!_useAngVelCmd)
Expand Down Expand Up @@ -241,6 +249,28 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(AngVelCmdControl))
this->TestWorld(world, "custom", 0.005, 950, 0.2, 1e-2, true, 100.01);
}

/////////////////////////////////////////////////
TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(CwForceCmdControl))
{
auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "thruster_cw_force_cmd.sdf");

// Thruster spins clockwise (thrust coefficient is negative)
// Tolerance is high because the joint command disturbs the vehicle body
this->TestWorld(world, "custom", -0.005, 950, 0.2, 1e-2);
}

/////////////////////////////////////////////////
TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(CwAngVelCmdControl))
{
auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "thruster_cw_ang_vel_cmd.sdf");

// Thruster spins clockwise (thrust coefficient is negative)
// Tolerance is high because the joint command disturbs the vehicle body
this->TestWorld(world, "custom", -0.005, 950, 0.2, 1e-2, true);
}

/////////////////////////////////////////////////
// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175
TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PIDControl))
Expand All @@ -262,4 +292,3 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl))
// Tolerance is high because the joint command disturbs the vehicle body
this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2);
}

115 changes: 115 additions & 0 deletions test/worlds/thruster_cw_ang_vel_cmd.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,115 @@
<?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.0000354167</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0000021667</iyy>
<iyz>0</iyz>
<izz>0.0000334167</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>
<!-- Clockwise thruster -->
<thrust_coefficient>-0.005</thrust_coefficient>
<fluid_density>950</fluid_density>
<propeller_diameter>0.2</propeller_diameter>
<velocity_control>true</velocity_control>
<use_angvel_cmd>true</use_angvel_cmd>
<!-- Max and min also apply to cmd_vel inputs -->
<max_thrust_cmd>0</max_thrust_cmd>
<min_thrust_cmd>-300</min_thrust_cmd>
</plugin>
</model>

</world>
</sdf>
114 changes: 114 additions & 0 deletions test/worlds/thruster_cw_force_cmd.sdf
View file
Open in desktop
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.0000354167</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0000021667</iyy>
<iyz>0</iyz>
<izz>0.0000334167</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>
<!-- Clockwise thruster -->
<thrust_coefficient>-0.005</thrust_coefficient>
<fluid_density>950</fluid_density>
<propeller_diameter>0.2</propeller_diameter>
<velocity_control>true</velocity_control>
<use_angvel_cmd>false</use_angvel_cmd>
<max_thrust_cmd>300</max_thrust_cmd>
<min_thrust_cmd>0</min_thrust_cmd>
</plugin>
</model>

</world>
</sdf>