FIX: Only calculate tau_ext_hat_filtered when efforts are available

frankaemika · Feb 14, 2022 · 8957694 · 8957694
1 parent e57dba1
commit 8957694
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 12 deletions.
diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
@@ -90,6 +90,9 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   void eStopActive(const bool active) override;
 
  private:
+  /// If gazebo::Joint::GetForceTorque() yielded already a non-zero value
+  bool efforts_initialized_;
+
   std::array<double, 3> gravity_earth_;
 
   std::string arm_id_;

diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
@@ -32,6 +32,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
   }
 
   this->robot_ = parent;
+  this->efforts_initialized_ = false;
 
 #if GAZEBO_MAJOR_VERSION >= 8
   gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
@@ -468,17 +469,6 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
   // This is ensured, because a FrankaStateInterface checks for at least seven joints in the URDF
   assert(this->joints_.size() >= 7);
 
-  std::array<double, 7> g;
-  static bool state_initialized = false;
-  if (state_initialized) {
-    g = this->model_->gravity(this->robot_state_);
-  } else {
-    for (auto& x : g) {
-      x = 0;
-    }
-    state_initialized = true;
-  }
-
   for (int i = 0; i < 7; i++) {
     std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1);
     const auto& joint = this->joints_.at(name);
@@ -496,7 +486,9 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
     this->robot_state_.theta[i] = joint->position;
     this->robot_state_.dtheta[i] = joint->velocity;
 
-    this->robot_state_.tau_ext_hat_filtered[i] = joint->effort - joint->command + joint->gravity;
+    if (this->efforts_initialized_) {
+      this->robot_state_.tau_ext_hat_filtered[i] = joint->effort - joint->command + joint->gravity;
+    }
 
     this->robot_state_.joint_contact[i] = static_cast<double>(joint->isInContact());
     this->robot_state_.joint_collision[i] = static_cast<double>(joint->isInCollision());
@@ -530,6 +522,8 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
   this->robot_state_.control_command_success_rate = 1.0;
   this->robot_state_.time = franka::Duration(time.toNSec() / 1e6 /*ms*/);
   this->robot_state_.O_T_EE = this->model_->pose(franka::Frame::kEndEffector, this->robot_state_);
+
+  this->efforts_initialized_ = true;
 }
 
 }  // namespace franka_gazebo