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