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.

Author: rickstaa

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