Merge pull request frankaemika#226 from rickstaa/fix_gazebo_set_model…

…_config_problem

feat(franka_gazebo): implement `set_franka_model_configuration` service

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,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/joint.cpp

@@ -9,6 +9,15 @@ 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->desired_position = this->requestedPosition_;
+    this->stop_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 +120,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,5 @@
+string[] joint_names
+float64[] joint_positions
+---
+bool success
+string error