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`: `` 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 4e998c423..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
@@ -70,11 +74,6 @@ struct Joint {
/// be controlled, or if the joint is entirely uncontrolled
boost::optional control_method = boost::none;
- /// 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;
diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
index bdaa096c0..49f324e1f 100644
--- a/franka_gazebo/src/franka_hw_sim.cpp
+++ b/franka_gazebo/src/franka_hw_sim.cpp
@@ -679,8 +679,7 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
}
if (this->robot_initialized_) {
- // NOTE: Here we use the clamped command to filter out the internal controller
- // force when the joint is in its limits.
+ // 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
diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp
index 5145ca635..32da62995 100644
--- a/franka_gazebo/src/joint.cpp
+++ b/franka_gazebo/src/joint.cpp
@@ -51,8 +51,7 @@ 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.
+ /// 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
@@ -112,10 +111,12 @@ double Joint::getLinkMass() const {
}
bool Joint::isInCollision() const {
- return std::abs(this->effort - this->clamped_command + this->gravity) > 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->clamped_command + this->gravity) > 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