diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 8a85a07da..671f4c78d 100644 --- 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 gravity_earth_; std::string arm_id_; diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 825e0f25c..9e6e98f48 100644 --- 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 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(joint->isInContact()); this->robot_state_.joint_collision[i] = static_cast(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