From a07f15e3f0a6b554b727f616833227785a45373b Mon Sep 17 00:00:00 2001 From: rickstaa Date: Tue, 22 Feb 2022 11:45:55 +0100 Subject: [PATCH] fix(franka_gazebo): add gravity component to joint collision checking This commit makes sure the gravity contribution is accounted for in the joint collision checking. This needs to be done since it is not stored in `joint->command` variable. --- franka_gazebo/src/joint.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index d733539db..60b36e9af 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -64,10 +64,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->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; } } // namespace franka_gazebo