From 61d9210d1662f2f312347fa014f40c389c0a62fa Mon Sep 17 00:00:00 2001 From: rickstaa Date: Wed, 16 Feb 2022 12:43:07 +0100 Subject: [PATCH 1/2] 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 https://github.com/frankaemika/franka_ros/issues/225 for more information). --- CHANGELOG.md | 1 + .../include/franka_gazebo/franka_hw_sim.h | 2 + franka_gazebo/include/franka_gazebo/joint.h | 10 ++ franka_gazebo/src/franka_hw_sim.cpp | 116 ++++++++++++++++++ franka_gazebo/src/joint.cpp | 15 +++ franka_msgs/CMakeLists.txt | 7 +- franka_msgs/package.xml | 1 + franka_msgs/srv/SetJointConfiguration.srv | 4 + 8 files changed, 153 insertions(+), 3 deletions(-) create mode 100644 franka_msgs/srv/SetJointConfiguration.srv diff --git a/CHANGELOG.md b/CHANGELOG.md index 353abc4b7..4a075741f 100644 --- 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`: `` 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 index d94b9019e..a316da50d 100644 --- 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> action_recovery_; std::vector lower_force_thresholds_nominal_; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index a6e9bdeab..b301e8b7d 100644 --- 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::quiet_NaN(); double lastAcceleration = std::numeric_limits::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 index fb80907b2..2fcc8f725 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -9,8 +9,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -22,6 +24,8 @@ #include #include #include +#include +#include "ros/ros.h" 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 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/list_controllers"); this->service_controller_switch_ = nh.serviceClient( "controller_manager/switch_controller"); + this->service_gazebo_set_model_configuration_ = + nh.serviceClient("/gazebo/set_model_configuration"); } void FrankaHWSim::restartControllers() { diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index 186f11927..c12b3fe9a 100644 --- 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 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 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 index 1f6f92659..94b423342 100644 --- 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,6 +10,7 @@ add_service_files(FILES SetEEFrame.srv SetForceTorqueCollisionBehavior.srv SetFullCollisionBehavior.srv + SetJointConfiguration.srv SetJointImpedance.srv SetKFrame.srv SetLoad.srv @@ -17,6 +18,6 @@ add_service_files(FILES 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 index 04fc82073..41792d62d 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -16,6 +16,7 @@ message_generation std_msgs + sensor_msgs actionlib_msgs message_runtime diff --git a/franka_msgs/srv/SetJointConfiguration.srv b/franka_msgs/srv/SetJointConfiguration.srv new file mode 100644 index 000000000..f33b2bb65 --- /dev/null +++ b/franka_msgs/srv/SetJointConfiguration.srv @@ -0,0 +1,4 @@ +sensor_msgs/JointState configuration +--- +bool success +string error From bae0507f070dfac1a2a1b76bb7d5767ebac8cf7a Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Wed, 3 Jan 2024 21:40:56 +0100 Subject: [PATCH 2/2] fix: ensure changes work with new upstream This commit ensures the `set_franka_model_configuration` works with the changes made in 89d25713b741631ca9f4f417585e7835d160cf4a. It also simplifies the `SetJointConfiguration.srv` message. --- franka_gazebo/src/franka_hw_sim.cpp | 116 ---------------------- franka_gazebo/src/joint.cpp | 2 + franka_msgs/srv/SetJointConfiguration.srv | 3 +- 3 files changed, 4 insertions(+), 117 deletions(-) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 2fcc8f725..fb80907b2 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -9,10 +9,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -24,8 +22,6 @@ #include #include #include -#include -#include "ros/ros.h" namespace franka_gazebo { @@ -412,122 +408,10 @@ 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 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/list_controllers"); this->service_controller_switch_ = nh.serviceClient( "controller_manager/switch_controller"); - this->service_gazebo_set_model_configuration_ = - nh.serviceClient("/gazebo/set_model_configuration"); } void FrankaHWSim::restartControllers() { diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index c12b3fe9a..919dc3292 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -13,6 +13,8 @@ void Joint::update(const ros::Duration& dt) { if (this->setPositionRequested_) { std::lock_guard lock(this->requestedPositionMutex_); this->position = this->requestedPosition_; + this->desired_position = this->requestedPosition_; + this->stop_position = this->requestedPosition_; this->setPositionRequested_ = false; } diff --git a/franka_msgs/srv/SetJointConfiguration.srv b/franka_msgs/srv/SetJointConfiguration.srv index f33b2bb65..b4a3e55d5 100644 --- a/franka_msgs/srv/SetJointConfiguration.srv +++ b/franka_msgs/srv/SetJointConfiguration.srv @@ -1,4 +1,5 @@ -sensor_msgs/JointState configuration +string[] joint_names +float64[] joint_positions --- bool success string error