From 87a786424da9960071e907864cb6619c1b2d63a7 Mon Sep 17 00:00:00 2001 From: rickstaa Date: Fri, 8 Oct 2021 16:33:38 +0200 Subject: [PATCH] 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 | 1 + franka_gazebo/include/franka_gazebo/franka_hw_sim.h | 2 ++ franka_gazebo/src/franka_hw_sim.cpp | 6 +++++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c83cbbea5..65f128cfa 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index a509f5d95..8a85a07da 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/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 gravity_earth_; + std::string arm_id_; gazebo::physics::ModelPtr robot_; std::map> joints_; diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 661276c1c..6d56c9cb1 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/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;