From f63277e987b40e627d5ed2aaad76fbd604a1f063 Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Mon, 18 Jul 2022 18:00:36 -0400
Subject: [PATCH] add input command and output thrust clamping to thruster
 plugin

---
 usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc
index 98b09de09..d4d0152eb 100644
--- a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc
+++ b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc
@@ -366,6 +366,12 @@ void UsvThrust::Update()
       // Adjust thruster engine joint angle with PID
       this->RotateEngine(i, now - this->thrusters[i].lastAngleUpdateTime);
 
+      // Apply input command clamping
+      this->thrusters[i].currCmd = std::min(this->thrusters[i].currCmd,
+                                            this->thrusters[i].maxCmd);
+      this->thrusters[i].currCmd = std::max(this->thrusters[i].currCmd,
+                                            this->thrusters[i].minCmd);
+
       // Apply the thrust mapping
       ignition::math::Vector3d tforcev(0, 0, 0);
       switch (this->thrusters[i].mappingType)
@@ -390,6 +396,10 @@ void UsvThrust::Update()
             break;
       }
 
+      // Apply thrust clamping
+      tforcev.X() = std::max(tforcev.X(), this->thrusters[i].maxForceFwd);
+      tforcev.X() = std::min(tforcev.X(), this->thrusters[i].maxForceRev);
+
       // Apply force for each thruster
       this->thrusters[i].link->AddLinkForce(tforcev);