add input command and output thrust clamping to thruster plugin

osrf · Jul 18, 2022 · f63277e · f63277e
1 parent a354e55
commit f63277e
Showing 1 changed file with 10 additions and 0 deletions.
diff --git 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);