From 0fa2a0f078686e8b99d4d5d0dc8241c3c46e8dc3 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). --- .../include/franka_gazebo/franka_hw_sim.h | 3 + franka_gazebo/include/franka_gazebo/joint.h | 14 +++ franka_gazebo/src/franka_hw_sim.cpp | 113 ++++++++++++++++++ franka_gazebo/src/joint.cpp | 13 ++ franka_msgs/CMakeLists.txt | 7 +- franka_msgs/package.xml | 1 + franka_msgs/srv/SetJointConfiguration.srv | 4 + 7 files changed, 152 insertions(+), 3 deletions(-) create mode 100644 franka_msgs/srv/SetJointConfiguration.srv diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 2cbd1fdd3..7469dbf87 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -115,6 +115,9 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { ros::ServiceServer service_set_k_; ros::ServiceServer service_set_load_; ros::ServiceServer service_collision_behavior_; + ros::ServiceServer service_set_model_configuration_; + + ros::ServiceClient gazebo_set_model_configuration_client_; std::vector lower_force_thresholds_nominal_; std::vector upper_force_thresholds_nominal_; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index be8afcf3c..1b3588187 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -35,6 +36,10 @@ struct Joint { /// http://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1Joint.html int type; + /// Joint limits @see + /// https://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1JointLimits.html + joint_limits_interface::JointLimits limits; + /// The axis of rotation/translation of this joint in local coordinates Eigen::Vector3d axis; @@ -87,9 +92,18 @@ struct Joint { */ bool isInCollision() const; + /** + * 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(); + + // Used to track whether the user requested a joint position to be set. + 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 d34d344fe..fb8013270 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -7,13 +7,18 @@ #include #include #include +#include #include #include +#include #include +#include #include #include #include #include +#include +#include "ros/ros.h" namespace franka_gazebo { @@ -85,6 +90,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, return false; } joint->type = urdf_joint->type; + joint_limits_interface::getJointLimits(urdf_joint, joint->limits); joint->axis = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); // Get a handle to the underlying Gazebo Joint @@ -160,6 +166,10 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, // Initialize ROS Services initServices(model_nh); + // Connect to '/gazebo/set_model_configuration' service + this->gazebo_set_model_configuration_client_ = + model_nh.serviceClient("/gazebo/set_model_configuration"); + return readParameters(model_nh, *urdf); } @@ -305,6 +315,109 @@ 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->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) { diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index d733539db..6abea2503 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); @@ -70,4 +77,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 a66270712..f5ab146cd 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