diff --git a/CHANGELOG.md b/CHANGELOG.md index 4a075741f..bc5c0216e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ Requires `libfranka` >= 0.8.0 * `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`: Add `set_franka_model_configuration` service. + * `franka_gazebo`: Fix joint collision checking by adding the gravity component to the total torque calculation. ## 0.10.1 - 2022-09-15 diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index 919dc3292..9c8df4d33 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -114,11 +114,11 @@ double Joint::getLinkMass() const { } bool Joint::isInCollision() const { - return std::abs(this->effort - this->command) > this->collision_threshold; + return std::abs(this->effort - this->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->command + this->gravity) > this->contact_threshold; } void Joint::setJointPosition(const double joint_position) {