feat(franka_gazebo): implement 'set_franka_model_configuration' service

This commit implements a `set_franka_model_configuration` service. This service can be used to set the Franka configuration in the Gazebo simulation. Under the hood, this service calls Gazebo's 'set_model_configuration' service while ensuring that the reported joint positions stay within joint limits (see #225 for more information).
frankaemika · Sep 4, 2023 · 2e2813f · 2e2813f
1 parent d439fc7
commit 2e2813f
Show file tree

Hide file tree

Showing 8 changed files with 153 additions and 3 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -13,6 +13,7 @@ Requires `libfranka` >= 0.8.0
   * `franka_gazebo`: `FrankaHWSim` only acts on joints belonging to a Franka robot. This allows to combine a Franka robot and others (like mobile platforms) in same URDF ([#313](https://github.com/frankaemika/franka_ros/issues/313))
   * `franka_description`: `<xacro:franka_robot/>` macro now supports to customize the `parent` frame and its `xyz` + `rpy` offset
   * `franka_hw`: Fix the bug where the previous controller is still running after switching the controller. ([#326](https://github.com/frankaemika/franka_ros/issues/326))
+  * `franka_gazebo`: Add `set_franka_model_configuration` service.
 
 ## 0.10.1 - 2022-09-15
 

diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
@@ -158,8 +158,10 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   ros::ServiceServer service_set_load_;
   ros::ServiceServer service_collision_behavior_;
   ros::ServiceServer service_user_stop_;
+  ros::ServiceServer service_set_model_configuration_;
   ros::ServiceClient service_controller_list_;
   ros::ServiceClient service_controller_switch_;
+  ros::ServiceClient service_gazebo_set_model_configuration_;
   std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>> action_recovery_;
 
   std::vector<double> lower_force_thresholds_nominal_;

diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h
@@ -167,9 +167,19 @@ struct Joint {
   /// gains are ignored.
   control_toolbox::Pid velocity_controller;
 
+  /**
+   * Sets the joint position.
+   */
+  void setJointPosition(const double joint_position);
+
  private:
   double lastVelocity = std::numeric_limits<double>::quiet_NaN();
   double lastAcceleration = std::numeric_limits<double>::quiet_NaN();
+
+  // Track joint position set requests
+  bool setPositionRequested_ = false;
+  double requestedPosition_ = 0.0;
+  std::mutex requestedPositionMutex_;
 };
 
 }  // namespace franka_gazebo
diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
@@ -9,8 +9,10 @@
 #include <franka_hw/services.h>
 #include <franka_msgs/SetEEFrame.h>
 #include <franka_msgs/SetForceTorqueCollisionBehavior.h>
+#include <franka_msgs/SetJointConfiguration.h>
 #include <franka_msgs/SetKFrame.h>
 #include <franka_msgs/SetLoad.h>
+#include <gazebo_msgs/SetModelConfiguration.h>
 #include <gazebo_ros_control/robot_hw_sim.h>
 #include <joint_limits_interface/joint_limits_urdf.h>
 #include <std_msgs/Bool.h>
@@ -22,6 +24,8 @@
 #include <iostream>
 #include <sstream>
 #include <string>
+#include "ros/ros.h"
+#include <unordered_map>
 
 namespace franka_gazebo {
 
@@ -408,10 +412,122 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
             response.success = true;
             return true;
           });
+  this->service_set_model_configuration_ = franka_hw::advertiseService<
+      franka_msgs::SetJointConfiguration>(
+      nh, "set_franka_model_configuration", [&](auto& request, auto& response) {
+        // Check if joints are specified
+        if (request.configuration.name.size() == 1 && request.configuration.name[0].empty()) {
+          ROS_ERROR_STREAM_NAMED("franka_hw_sim",
+                                 "Failed to set Franka model configuration: the request is "
+                                 "invalid because no joints were specified.");
+          response.success = false;
+          response.error = "no joints specified";
+          return false;
+        }
+
+        // Check if 'position' and 'name' fields have different lengths
+        if (request.configuration.name.size() != request.configuration.position.size()) {
+          ROS_ERROR_STREAM_NAMED(
+              "franka_hw_sim",
+              "Failed to set Franka model configuration: the request is invalid because the "
+              "'position' and 'name' fields have different lengths.");
+          response.success = false;
+          response.error = "'position' and 'name' fields length unequal";
+          return false;
+        }
+
+        // Log request information
+        std::ostringstream requested_configuration_stream;
+        for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) {
+          requested_configuration_stream << request.configuration.name[ii] << ": "
+                                         << request.configuration.position[ii] << ", ";
+        }
+        std::string requested_configuration_string = requested_configuration_stream.str();
+        requested_configuration_string = requested_configuration_string.substr(
+            0, requested_configuration_string.length() - 2);  // Remove last 2 characters
+        ROS_INFO_STREAM_NAMED("franka_hw_sim",
+                              "Setting joint configuration: " << requested_configuration_string);
+
+        // Validate joint names and joint limits
+        std::unordered_map<std::string, double> model_configuration;
+        for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) {
+          std::string joint(request.configuration.name[ii]);
+          double position(request.configuration.position[ii]);
+
+          auto joint_iterator = this->joints_.find(joint);
+          if (joint_iterator != this->joints_.end()) {  // If joint exists
+            double min_position(joint_iterator->second->limits.min_position);
+            double max_position(joint_iterator->second->limits.max_position);
+
+            // Check if position is within joint limits
+            if (position == boost::algorithm::clamp(position, min_position, max_position)) {
+              model_configuration.emplace(std::make_pair(joint, position));
+            } else {
+              ROS_WARN_STREAM_NAMED("franka_hw_sim",
+                                    "Joint configuration of joint '"
+                                        << joint
+                                        << "' was not set since the requested joint position ("
+                                        << position << ") is not within joint limits (i.e. "
+                                        << min_position << " - " << max_position << ").");
+            }
+          } else {
+            ROS_WARN_STREAM_NAMED("franka_hw_sim",
+                                  "Joint configuration of joint '"
+                                      << joint << "' not set since it is not a valid panda joint.");
+          }
+        }
+
+        // Return if no valid positions were found
+        if (model_configuration.empty()) {
+          ROS_ERROR_STREAM_NAMED("franka_hw_sim",
+                                 "Setting of Franka model configuration aborted since no valid "
+                                 "joint configuration were found.");
+          response.success = false;
+          response.error = "no valid joint configurations";
+          return false;
+        }
+
+        // Throw warnings about unused request fields
+        if (!request.configuration.effort.empty()) {
+          ROS_WARN_STREAM_ONCE_NAMED(
+              "franka_hw_sim",
+              "The 'set_franka_model_configuration' service does not use the 'effort' field.");
+        }
+        if (!request.configuration.velocity.empty()) {
+          ROS_WARN_STREAM_ONCE_NAMED(
+              "franka_hw_sim",
+              "The 'set_franka_model_configuration' service does not use the 'velocity' field.");
+        }
+
+        // Call Gazebo 'set_model_configuration' service and update Franka joint positions
+        gazebo_msgs::SetModelConfiguration gazebo_model_configuration;
+        gazebo_model_configuration.request.model_name = "panda";
+        for (const auto& pair : model_configuration) {
+          gazebo_model_configuration.request.joint_names.push_back(pair.first);
+          gazebo_model_configuration.request.joint_positions.push_back(pair.second);
+        }
+        if (this->service_gazebo_set_model_configuration_.call(gazebo_model_configuration)) {
+          // Update franka positions
+          for (const auto& pair : model_configuration) {
+            this->joints_[pair.first]->setJointPosition(pair.second);
+          }
+
+          response.success = true;
+          return true;
+        }
+
+        ROS_WARN_STREAM_NAMED("franka_hw_sim",
+                              "Setting of Franka model configuration failed since "
+                              "a problem occurred when setting the joint configuration in Gazebo.");
+        response.success = false;
+        return false;
+      });
   this->service_controller_list_ = nh.serviceClient<controller_manager_msgs::ListControllers>(
       "controller_manager/list_controllers");
   this->service_controller_switch_ = nh.serviceClient<controller_manager_msgs::SwitchController>(
       "controller_manager/switch_controller");
+  this->service_gazebo_set_model_configuration_ =
+      nh.serviceClient<gazebo_msgs::SetModelConfiguration>("/gazebo/set_model_configuration");
 }
 
 void FrankaHWSim::restartControllers() {

diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp
@@ -9,6 +9,13 @@ void Joint::update(const ros::Duration& dt) {
     return;
   }
 
+  // Apply 'setJointPosition' position request.
+  if (this->setPositionRequested_) {
+    std::lock_guard<std::mutex> lock(this->requestedPositionMutex_);
+    this->position = this->requestedPosition_;
+    this->setPositionRequested_ = false;
+  }
+
   this->velocity = this->handle->GetVelocity(0);
 #if GAZEBO_MAJOR_VERSION >= 8
   double position = this->handle->Position(0);
@@ -111,4 +118,12 @@ bool Joint::isInCollision() const {
 bool Joint::isInContact() const {
   return std::abs(this->effort - this->command) > this->contact_threshold;
 }
+
+void Joint::setJointPosition(const double joint_position) {
+  // NOTE: Joint position is set in update() method to prevent racing conditions.
+  std::lock_guard<std::mutex> lock(this->requestedPositionMutex_);
+  this->requestedPosition_ = joint_position;
+  this->setPositionRequested_ = true;
+}
+
 }  // namespace franka_gazebo
diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.4)
 project(franka_msgs)
 
-find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_msgs)
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs sensor_msgs actionlib_msgs)
 
 add_message_files(FILES Errors.msg FrankaState.msg)
 
@@ -10,13 +10,14 @@ add_service_files(FILES
   SetEEFrame.srv
   SetForceTorqueCollisionBehavior.srv
   SetFullCollisionBehavior.srv
+  SetJointConfiguration.srv
   SetJointImpedance.srv
   SetKFrame.srv
   SetLoad.srv
 )
 
 add_action_files(FILES ErrorRecovery.action)
 
-generate_messages(DEPENDENCIES std_msgs actionlib_msgs)
+generate_messages(DEPENDENCIES std_msgs sensor_msgs actionlib_msgs)
 
-catkin_package(CATKIN_DEPENDS message_runtime std_msgs actionlib_msgs)
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs sensor_msgs actionlib_msgs)
diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml
@@ -16,6 +16,7 @@
   <build_depend>message_generation</build_depend>
 
   <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>actionlib_msgs</depend>
 
   <exec_depend>message_runtime</exec_depend>

diff --git a/franka_msgs/srv/SetJointConfiguration.srv b/franka_msgs/srv/SetJointConfiguration.srv
@@ -0,0 +1,4 @@
+sensor_msgs/JointState configuration
+---
+bool success
+string error