From cbc6c4f7fcfdd01dec62ab99cd9284981cc74138 Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Fri, 1 Sep 2023 19:06:18 +0200
Subject: [PATCH] fix(franka_gazebo): fix external torque calculation

This commit ensures that the internal controller forces are not used in
the external torque (i.e. `tau_ext`) calculation when a joint is in its
limit.
---
 CHANGELOG.md                                |  1 +
 franka_gazebo/include/franka_gazebo/joint.h |  4 ++++
 franka_gazebo/src/franka_hw_sim.cpp         |  3 ++-
 franka_gazebo/src/joint.cpp                 | 12 ++++++++++--
 4 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 353abc4b7..0b13d2da7 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,7 @@ Requires `libfranka` >= 0.8.0
   * `franka_gazebo`: `FrankaHWSim` only acts on joints belonging to a Franka robot. This allows to combine a Franka robot and others (like mobile platforms) in same URDF ([#313](https://github.com/frankaemika/franka_ros/issues/313))
   * `franka_description`: `<xacro:franka_robot/>` macro now supports to customize the `parent` frame and its `xyz` + `rpy` offset
   * `franka_hw`: Fix the bug where the previous controller is still running after switching the controller. ([#326](https://github.com/frankaemika/franka_ros/issues/326))
+  * `franka_gazebo`: Fix `tau_ext` calculation by accounting for internal joint limits
 
 ## 0.10.1 - 2022-09-15
 
diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h
index a6e9bdeab..edc1cd17c 100644
--- a/franka_gazebo/include/franka_gazebo/joint.h
+++ b/franka_gazebo/include/franka_gazebo/joint.h
@@ -55,6 +55,10 @@ struct Joint {
   /// \f$Nm\f$ without gravity
   double command = 0;
 
+  /// The clamped applied command from a controller acting on this joint either in \f$N\f$ or
+  /// \f$Nm\f$ without gravity. Set to zero when joint command is saturated due to joint limits.
+  double clamped_command = 0;
+
   /// The current desired position that is used for the PID controller when the joints control
   /// method is "POSITION". When the control method is not "POSITION", this value will only be
   /// updated once at the start of the controller and stay the same until a new controller is
diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
index fb80907b2..49f324e1f 100644
--- a/franka_gazebo/src/franka_hw_sim.cpp
+++ b/franka_gazebo/src/franka_hw_sim.cpp
@@ -679,7 +679,8 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
     }
 
     if (this->robot_initialized_) {
-      double tau_ext = joint->effort - joint->command + joint->gravity;
+      // NOTE: Use clamped_command to account for internal joint saturation.
+      double tau_ext = joint->effort - joint->clamped_command + joint->gravity;
 
       // Exponential moving average filter from tau_ext -> tau_ext_hat_filtered
       this->robot_state_.tau_ext_hat_filtered[i] =
diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp
index 186f11927..32da62995 100644
--- a/franka_gazebo/src/joint.cpp
+++ b/franka_gazebo/src/joint.cpp
@@ -50,6 +50,12 @@ void Joint::update(const ros::Duration& dt) {
   }
   this->jerk = (this->acceleration - this->lastAcceleration) / dt.toSec();
   this->lastAcceleration = this->acceleration;
+
+  /// Store the clamped command, clamped to zero when joint is at its limits.
+  this->clamped_command =
+      (this->position > this->limits.min_position && this->position < this->limits.max_position)
+          ? this->command
+          : 0.0;
 }
 
 double Joint::getDesiredPosition(const franka::RobotMode& mode) const {
@@ -105,10 +111,12 @@ double Joint::getLinkMass() const {
 }
 
 bool Joint::isInCollision() const {
-  return std::abs(this->effort - this->command) > this->collision_threshold;
+  // NOTE: Clamped_command used to handle internal collisions.
+  return std::abs(this->effort - this->clamped_command) > this->collision_threshold;
 }
 
 bool Joint::isInContact() const {
-  return std::abs(this->effort - this->command) > this->contact_threshold;
+  // NOTE: Clamped_command used to handle internal contacts.
+  return std::abs(this->effort - this->clamped_command) > this->contact_threshold;
 }
 }  // namespace franka_gazebo