Limit thruster system's input thrust cmd

src/systems/thruster/Thruster.cc

@@ -79,12 +79,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
-  /// TODO(chapulina) Make it configurable from SDF.
+  /// \brief Maximum input force [N] for the propellerController,
+  /// default: 1000N
   public: double cmdMax = 1000;
 
-  /// \brief Minimum input force [N] for the propellerController, default: 1000N
-  /// TODO(chapulina) Make it configurable from SDF.
+  /// \brief Minimum input force [N] for the propellerController,
+  /// default: -1000N
   public: double cmdMin = -1000;
 
   /// \brief Thrust coefficient relating the propeller angular velocity to the
@@ -206,6 +206,29 @@ void Thruster::Configure(
   enableComponent<components::WorldAngularVelocity>(_ecm,
       this->dataPtr->linkEntity);
 
+  double minThrustCmd = this->dataPtr->cmdMin;
+  double maxThrustCmd = this->dataPtr->cmdMax;
+  if (_sdf->HasElement("max_thrust_cmd"))
+  {
+    maxThrustCmd = _sdf->Get<double>("max_thrust_cmd");
+  }
+  if (_sdf->HasElement("min_thrust_cmd"))
+  {
+    minThrustCmd = _sdf->Get<double>("min_thrust_cmd");
+  }
+  if (maxThrustCmd < minThrustCmd)
+  {
+    ignerr << "<max_thrust_cmd> must be greater than or equal to "
+           << "<min_thrust_cmd>. Revert to using default values: "
+           << "min: " << this->dataPtr->cmdMin << ", "
+           << "max: " << this->dataPtr->cmdMax << std::endl;
+  }
+  else
+  {
+    this->dataPtr->cmdMax = maxThrustCmd;
+    this->dataPtr->cmdMin = minThrustCmd;
+  }
+
   if (_sdf->HasElement("velocity_control"))
   {
     this->dataPtr->velocityControl = _sdf->Get<bool>("velocity_control");

src/systems/thruster/Thruster.hh

@@ -64,6 +64,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]
   ///
   /// ## Example
   /// An example configuration is installed with Gazebo. The example

test/integration/thruster.cc

@@ -134,9 +134,24 @@ void ThrusterTest::TestWorld(const std::string &_world,
   EXPECT_LT(sleep, maxSleep);
   EXPECT_TRUE(pub.HasConnections());
 
-  double force{300.0};
+  // input force cmd - this should be capped to 0
+  double forceCmd{-1000.0};
   msgs::Double msg;
-  msg.set_data(force);
+  msg.set_data(forceCmd);
+  pub.Publish(msg);
+
+  // Check no movement
+  fixture.Server()->Run(true, 100, false);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X());
+  EXPECT_EQ(100u, modelPoses.size());
+  EXPECT_EQ(100u, propellerAngVels.size());
+  modelPoses.clear();
+  propellerAngVels.clear();
+
+  // input force cmd this should be capped to 300
+  forceCmd = 1000.0;
+  msg.set_data(forceCmd);
   pub.Publish(msg);
 
   // Check movement
@@ -152,6 +167,9 @@ void ThrusterTest::TestWorld(const std::string &_world,
   EXPECT_EQ(100u * sleep, modelPoses.size());
   EXPECT_EQ(100u * sleep, propellerAngVels.size());
 
+  // max allowed force
+  double force{300.0};
+
   // F = m * a
   // s = a * t^2 / 2
   // F = m * 2 * s / t^2

test/worlds/thruster_pid.sdf

@@ -104,6 +104,8 @@
         <thrust_coefficient>0.004</thrust_coefficient>
         <fluid_density>1000</fluid_density>
         <propeller_diameter>0.2</propeller_diameter>
+        <max_thrust_cmd>300</max_thrust_cmd>
+        <min_thrust_cmd>0</min_thrust_cmd>
       </plugin>
     </model>
 

test/worlds/thruster_vel_cmd.sdf

@@ -103,6 +103,8 @@
         <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>
       </plugin>
     </model>