feat(franka_gazebo): implement 'reset_joint_states' service

This commit implements a `reset_joint_states` service. This service can
be used to reset the joint states when they are pushed outside of the
joint limits. This can for example happen when Gazebo's
`set_model_configuration` is used (see
frankaemika#225 for more
information).

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -115,6 +115,7 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   ros::ServiceServer service_set_k_;
   ros::ServiceServer service_set_load_;
   ros::ServiceServer service_collision_behavior_;
+  ros::ServiceServer service_reset_joint_states_;
 
   std::vector<double> lower_force_thresholds_nominal_;
   std::vector<double> upper_force_thresholds_nominal_;

franka_gazebo/include/franka_gazebo/joint.h

@@ -87,9 +87,17 @@ struct Joint {
    */
   bool isInCollision() const;
 
+  /**
+   * Makes sure the joint states are reset to zero.
+   */
+  void resetJointStates();
+
  private:
   double lastVelocity = std::numeric_limits<double>::quiet_NaN();
   double lastAcceleration = std::numeric_limits<double>::quiet_NaN();
+
+  // Used to track whether a reset of the joint states was requested
+  bool resetRequested_ = false;
 };
 
 }  // namespace franka_gazebo

franka_gazebo/src/franka_hw_sim.cpp

@@ -5,11 +5,13 @@
 #include <franka_gazebo/model_kdl.h>
 #include <franka_hw/franka_hw.h>
 #include <franka_hw/services.h>
+#include <franka_msgs/ResetJointStates.h>
 #include <franka_msgs/SetEEFrame.h>
 #include <franka_msgs/SetForceTorqueCollisionBehavior.h>
 #include <franka_msgs/SetKFrame.h>
 #include <franka_msgs/SetLoad.h>
 #include <gazebo_ros_control/robot_hw_sim.h>
+#include <std_msgs/Empty.h>
 #include <Eigen/Dense>
 #include <iostream>
 #include <sstream>
@@ -305,6 +307,18 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
             response.success = true;
             return true;
           });
+  this->service_reset_joint_states_ = franka_hw::advertiseService<franka_msgs::ResetJointStates>(
+      nh, "reset_joint_states", [&](auto& request, auto& response) {
+        ROS_INFO_STREAM_NAMED("franka_hw_sim", "Resetting joint_states");
+
+        for (int i = 0; i < 7; i++) {
+          std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1);
+          this->joints_[name]->resetJointStates();
+        }
+
+        response.success = true;
+        return true;
+      });
 }
 
 void FrankaHWSim::readSim(ros::Time time, ros::Duration period) {

franka_gazebo/src/joint.cpp

@@ -9,6 +9,12 @@ void Joint::update(const ros::Duration& dt) {
     return;
   }
 
+  // Reset joint states if requested
+  if (this->resetRequested_) {
+    this->position = 0;
+    this->resetRequested_ = false;
+  }
+
   this->velocity = this->handle->GetVelocity(0);
 #if GAZEBO_MAJOR_VERSION >= 8
   double position = this->handle->Position(0);
@@ -70,4 +76,9 @@ bool Joint::isInCollision() const {
 bool Joint::isInContact() const {
   return std::abs(this->effort - this->command) > this->contact_threshold;
 }
+
+void Joint::resetJointStates() {
+  this->resetRequested_ = true;
+}
+
 }  // namespace franka_gazebo

franka_msgs/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_ms
 add_message_files(FILES Errors.msg FrankaState.msg)
 
 add_service_files(FILES
+  ResetJointStates.srv
   SetCartesianImpedance.srv
   SetEEFrame.srv
   SetForceTorqueCollisionBehavior.srv

franka_msgs/srv/ResetJointStates.srv

@@ -0,0 +1,3 @@
+---
+bool success
+string error