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

@@ -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