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
frankaemika#225 for more
information).

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
 

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -158,10 +158,13 @@ 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 gazebo_set_model_configuration_client_;
   std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>> action_recovery_;
 
+
   std::vector<double> lower_force_thresholds_nominal_;
   std::vector<double> upper_force_thresholds_nominal_;
 

franka_gazebo/include/franka_gazebo/joint.h

@@ -167,9 +167,18 @@ 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();
+
+  // Used to track whether the user requested a joint position to be set.
+  bool setPositionRequested_ = false;
+  double requestedPosition_ = 0.0;
 };
 
 }  // namespace franka_gazebo

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 <boost/algorithm/clamp.hpp>
+#include "ros/ros.h"
 
 namespace franka_gazebo {
 
@@ -242,6 +246,11 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
   // Initialize ROS Services
   initServices(model_nh);
   verifier_ = std::make_unique<ControllerVerifier>(joints_, arm_id_);
+
+  // Connect to '/gazebo/set_model_configuration' service
+  this->gazebo_set_model_configuration_client_ =
+      model_nh.serviceClient<gazebo_msgs::SetModelConfiguration>("/gazebo/set_model_configuration");
+
   return readParameters(model_nh, *urdf);
 }
 
@@ -442,6 +451,110 @@ void FrankaHWSim::restartControllers() {
     throw std::runtime_error("Service call '" + this->service_controller_switch_.getService() +
                              "' failed");
   }
+
+  this->service_set_model_configuration_ =
+      franka_hw::advertiseService<franka_msgs::SetJointConfiguration>(
+          nh, "set_franka_model_configuration", [&](auto& request, auto& response) {
+            // Check if positions equals the number of joints
+            if (request.configuration.name.size() == 1 && request.configuration.name[0].empty()) {
+              ROS_ERROR_STREAM_NAMED("franka_hw_sim",
+                                     "Setting of Franka model configuration failed since "
+                                     "no joints were specified in the request.");
+              response.success = false;
+              response.error = "no joints specified";
+              return false;
+            }
+            if (request.configuration.name.size() != request.configuration.position.size()) {
+              ROS_ERROR_STREAM_NAMED("franka_hw_sim",
+                                     "Setting of Franka model configuration failed since "
+                                     "the 'position' and 'name' fields were of unequal length.");
+              response.success = false;
+              response.error = "'position' and 'name' fields length unequal";
+              return false;
+            }
+
+            // Print request information
+            std::string requested_configuration_string;
+            for (int ii = 0; ii < request.configuration.name.size(); ii++) {
+                requested_configuration_string += request.configuration.name[ii] + ": " + std::to_string(request.configuration.position[ii])  + ", ";
+            }
+            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::map<std::string, double> model_configuration;
+            for (int ii = 0; ii < request.configuration.name.size(); ii++) {
+              std::string joint(request.configuration.name[ii]);
+              double position(request.configuration.position[ii]);
+
+              if (this->joints_.find(joint) != this->joints_.end()) { // If joint exists
+                double min_position(this->joints_[joint]->limits.min_position);
+                double max_position(this->joints_[joint]->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.size() == 0){
+              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.size() != 0) {
+              ROS_WARN_STREAM_ONCE_NAMED("franka_hw_sim",
+                                    "The 'set_franka_model_configuration' service does not use the "
+                                    "'effort' field.");
+            }
+            if (request.configuration.velocity.size() != 0) {
+              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->gazebo_set_model_configuration_client_.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;
+            } else {
+              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;
+            }
+          });
 }
 
 void FrankaHWSim::readSim(ros::Time time, ros::Duration period) {

franka_gazebo/src/joint.cpp

@@ -9,6 +9,13 @@ void Joint::update(const ros::Duration& dt) {
     return;
   }
 
+  // Set joint position to requested joint position
+  // NOTE: Implemented like this to prevent racing conditions.
+  if (this->setPositionRequested_) {
+    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,10 @@ 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) {
+  this->requestedPosition_ = joint_position;
+  this->setPositionRequested_ = true;
+}
+
 }  // namespace franka_gazebo

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)

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>

franka_msgs/srv/SetJointConfiguration.srv

@@ -0,0 +1,4 @@
+sensor_msgs/JointState configuration
+---
+bool success
+string error