fix: adds gazebo gravity vector to the gravity calculation

This commit makes sure that the gravity calculation uses the gravity
values of gazebo instead of using the default values.

CHANGELOG.md

@@ -2,6 +2,7 @@
 
 ## 0.x - UNRELEASED
 
+* Fix Gazebo simulation gravity compensation gravity vector
 * Add realistic hand/finger collision geometries to the Gazebo robot description
 * Add `joint_state_desired` publisher to `franka_gazebo`
 * No further ROS Kinetic support, since [End-of-Life was in April 2021](http://wiki.ros.org/Distributions)

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -90,6 +90,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   void eStopActive(const bool active) override;
 
  private:
+  std::array<double, 3> gravity_earth_;
+
   std::string arm_id_;
   gazebo::physics::ModelPtr robot_;
   std::map<std::string, std::shared_ptr<franka_gazebo::Joint>> joints_;

franka_gazebo/src/franka_hw_sim.cpp

@@ -41,6 +41,10 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
 
   ROS_INFO_STREAM_NAMED("franka_hw_sim", "Using physics type " << physics->GetType());
 
+  // Retrieve physics parameters
+  auto gravity = physics->World()->Gravity();
+  this->gravity_earth_ = {gravity.X(), gravity.Y(), gravity.Z()};
+
   // Generate a list of franka_gazebo::Joint to store all relevant information
   for (const auto& transmission : transmissions) {
     if (transmission.type_ != "transmission_interface/SimpleTransmission") {
@@ -307,7 +311,7 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) {
 }
 
 void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration /*period*/) {
-  auto g = this->model_->gravity(this->robot_state_);
+  auto g = this->model_->gravity(this->robot_state_, this->gravity_earth_);
 
   for (auto& pair : this->joints_) {
     auto joint = pair.second;