fix(franka_gazebo): fix tau_ext internal controller bug

This commit makes sure that internal controller forces are not
used while calculating the external torque when a joint is in its
joint limits.

franka_gazebo/include/franka_gazebo/joint.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <angles/angles.h>
+#include <joint_limits_interface/joint_limits.h>
 #include <ros/ros.h>
 #include <Eigen/Dense>
 #include <gazebo/physics/Joint.hh>
@@ -35,13 +36,22 @@ struct Joint {
   /// http://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1Joint.html
   int type;
 
+  /// Joint limits @see
+  /// https://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1JointLimits.html
+  joint_limits_interface::JointLimits limits;
+
   /// The axis of rotation/translation of this joint in local coordinates
   Eigen::Vector3d axis;
 
   /// The currently applied command from a controller acting on this joint either in \f$N\f$ or
   /// \f$Nm\f$ without gravity
   double command = 0;
 
+  /// The currently CLAMPED applied command from the controller acting on this joint either in \f$N\f$ or
+  /// \f$Nm\f$ without gravity.
+  /// NOTE: Clamped to zero when the joint is in its limits.
+  double clamped_command = 0;
+
   /// The currently acting gravity force or torque acting on this joint in \f$N\f$ or \f$Nm\f$
   double gravity = 0;
 

franka_gazebo/src/franka_hw_sim.cpp

@@ -10,6 +10,7 @@
 #include <franka_msgs/SetKFrame.h>
 #include <franka_msgs/SetLoad.h>
 #include <gazebo_ros_control/robot_hw_sim.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
 #include <Eigen/Dense>
 #include <iostream>
 #include <sstream>
@@ -85,6 +86,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
       return false;
     }
     joint->type = urdf_joint->type;
+    joint_limits_interface::getJointLimits(urdf_joint, joint->limits);
     joint->axis = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
 
     // Get a handle to the underlying Gazebo Joint
@@ -492,7 +494,9 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
     this->robot_state_.dtheta[i] = joint->velocity;
 
     if (this->efforts_initialized_) {
-      double tau_ext = joint->effort - joint->command + joint->gravity;
+      // NOTE: Here we use the clamped command to filter out the internal controller
+      // force when the joint is in its limits.
+      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] =

franka_gazebo/src/joint.cpp

@@ -50,6 +50,13 @@ void Joint::update(const ros::Duration& dt) {
   }
   this->jerk = (this->acceleration - this->lastAcceleration) / dt.toSec();
   this->lastAcceleration = this->acceleration;
+
+  // Store the clamped command
+  // NOTE: Clamped to zero when joint is in its joint limits.
+  this->clamped_command =
+      (this->position > this->limits.min_position && this->position < this->limits.max_position)
+          ? this->command
+          : 0.0;
 }
 
 double Joint::getLinkMass() const {
@@ -64,10 +71,10 @@ double Joint::getLinkMass() const {
 }
 
 bool Joint::isInCollision() const {
-  return std::abs(this->effort - this->command) > this->collision_threshold;
+  return std::abs(this->effort - this->clamped_command + this->gravity) > this->collision_threshold;
 }
 
 bool Joint::isInContact() const {
-  return std::abs(this->effort - this->command) > this->contact_threshold;
+  return std::abs(this->effort - this->clamped_command + this->gravity) > this->contact_threshold;
 }
 }  // namespace franka_gazebo