Skip to content

Commit

Permalink
Preserve sign of thrust_coefficient (#1402)
Browse files Browse the repository at this point in the history 
Signed-off-by: Clyde McQueen <[email protected]>
  • Loading branch information
@clydemcqueen
clydemcqueen authored Mar 23, 2022
1 parent 09462f5 commit d316196
Show file tree
Hide file tree
Showing 5 changed files with 278 additions and 17 deletions.
10 changes: 5 additions & 5 deletions src/systems/thruster/Thruster.cc
View file
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
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 thrust. A positive coefficient corresponds to a clockwise
/// propeller, which is a propeller that spins clockwise under positive
/// thrust when viewed along the parent link from stern (-x) to bow (+x).
/// [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
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(CcwForceCmdControl))
{
auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "thruster_ccw_force_cmd.sdf");

// Viewed from stern to bow the propeller spins counter-clockwise
// 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(CcwAngVelCmdControl))
{
auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "thruster_ccw_ang_vel_cmd.sdf");

// Viewed from stern to bow the propeller spins counter-clockwise
// 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_ccw_ang_vel_cmd.sdf
View file
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>
<!-- Counter-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_ccw_force_cmd.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.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>
<!-- Counter-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>

0 comments on commit d316196

Please sign in to comment.