From 24481c59be00d3d02fffa969136b18ce331ebc4f Mon Sep 17 00:00:00 2001 From: rickstaa Date: Wed, 16 Feb 2022 12:43:07 +0100 Subject: [PATCH] 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 | 9 ++ franka_gazebo/src/franka_hw_sim.cpp | 115 ++++++++++++++++++ franka_gazebo/src/joint.cpp | 13 ++ franka_msgs/CMakeLists.txt | 7 +- franka_msgs/package.xml | 1 + franka_msgs/srv/SetJointConfiguration.srv | 4 + 8 files changed, 149 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..503e03036 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/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::quiet_NaN(); double lastAcceleration = std::numeric_limits::quiet_NaN(); + + // Track joint position set requests + bool setPositionRequested_ = false; + double requestedPosition_ = 0.0; }; } // namespace franka_gazebo diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index fb80907b2..fcec214d1 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,7 @@ #include #include #include +#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( + 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 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->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; + } 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; + } + }); 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..a148b41e6 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; } + // 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 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