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:/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:/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,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_;

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

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,7 @@
 #include <iostream>
 #include <sstream>
 #include <string>
+#include "ros/ros.h"
 
 namespace franka_gazebo {
 
@@ -408,10 +411,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() {

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

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