Merge pull request #196 in SWDEV/franka_ros from SRR-1528 to develop

* commit '249c593c8ff4d3f476dd74d5e7bfbf67d0a9d206': (22 commits)
  ADD: Integration test verifying that user stops stops joint in Gazebo
  ADD: Make interactive marker configurable in `panda.launch`
  FIX: Allow finger joints to still be controlled while not in Move
  FIX: Don't stop finger joints on error recovery
  FIX: ROS_WARN message on user stop
  FIX: Don't recovery from errors when user stop still pressed
  ADD: CHANGELOG entry
  FIX: Make linter happy
  ADD: Statemachine tests
  ADD: Warning to FrankaHWSim::error_recovery action
  CHANGE: Restart all running controllers on error_recovery
  CHANGE: Extract `getDesiredXXX()` logic into Joint class
  FIX: Don't switch control method on user stop
  ADD: `error_recovery` action to FrankaHWSim
  CHANGE: Extract motion generators into separate functions
  CHANGE: Set `control_method` in statemachine actions
  ADD: FrankaHWSim offers `set_user_stop` service
  ADD: Allow position control for prismatic joints
  CHANGE: Properly distinguish between Position & Internal control
  ADD: Control state machine to franka_gazebo
  ...

.ci/Dockerfile.melodic

@@ -7,6 +7,7 @@ RUN apt-get update -y && apt-get install -y \
     pycodestyle \
     python3-catkin-tools \
     ros-melodic-libfranka \
+    ros-melodic-boost-sml \
     ros-melodic-ros-control \
     ros-melodic-eigen-conversions \
     ros-melodic-gazebo-dev \

.ci/Dockerfile.noetic

@@ -8,6 +8,7 @@ RUN apt-get update -y && apt-get install -y \
     liborocos-kdl-dev \
     python3-catkin-tools \
     ros-noetic-libfranka \
+    ros-noetic-boost-sml \
     ros-noetic-ros-control \
     ros-noetic-eigen-conversions \
     ros-noetic-gazebo-dev \

CHANGELOG.md

@@ -15,6 +15,8 @@
   *  **BREAKING**: `gripper_action` goes now to the commanded gripper position when `max_effort` is zero
   * `franka_gazebo`: Drop `delayed_controller_spawner.py` script in favor of `--wait-for TOPIC` flag from controller_manager
   * `franka_gazebo`: Properly calculate inertial properties of `world/stone/model.sdf`
+  * `franka_gazebo`: `set_user_stop` service to simulate User stop in Gazebo
+  * `franka_gazebo`: `error_recovery` action similar to `franka_control`
 
 ## 0.9.0 - 2022-03-29
 

franka_gazebo/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   gazebo_ros_control
   kdl_parser
+  boost_sml
   control_toolbox
   controller_manager
   controller_interface
@@ -49,6 +50,7 @@ catkin_package(
     std_msgs
     gazebo_ros_control
     kdl_parser
+    boost_sml
     controller_manager
     controller_interface
     control_toolbox

franka_gazebo/include/franka_gazebo/controller_verifier.h

@@ -43,4 +43,4 @@ class ControllerVerifier {
 
   static bool hasControlMethodAndValidSize(const hardware_interface::InterfaceResources& resource);
 };
-}  // namespace franka_gazebo
+}  // namespace franka_gazebo

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -1,12 +1,14 @@
 #pragma once
 
-#include <control_toolbox/pid.h>
+#include <actionlib/server/simple_action_server.h>
 #include <franka/robot_state.h>
 #include <franka_gazebo/controller_verifier.h>
 #include <franka_gazebo/joint.h>
+#include <franka_gazebo/statemachine.h>
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
 #include <franka_hw/model_base.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 #include <gazebo_ros_control/robot_hw_sim.h>
 #include <hardware_interface/internal/hardware_resource_manager.h>
 #include <hardware_interface/joint_command_interface.h>
@@ -15,11 +17,13 @@
 #include <ros/ros.h>
 #include <urdf/model.h>
 #include <boost/optional.hpp>
+#include <boost_sml/sml.hpp>
 #include <cmath>
 #include <gazebo/common/common.hh>
 #include <gazebo/physics/physics.hh>
 #include <map>
 #include <memory>
+#include <mutex>
 
 namespace franka_gazebo {
 
@@ -45,6 +49,11 @@ const double kDefaultTauExtLowpassFilter = 1.0;  // no filtering per default of
  */
 class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
  public:
+  /**
+   * Create a new FrankaHWSim instance
+   */
+  FrankaHWSim();
+
   /**
    * Initialize the simulated robot hardware and parse all supported transmissions.
    *
@@ -125,16 +134,14 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   gazebo::physics::ModelPtr robot_;
   std::map<std::string, std::shared_ptr<franka_gazebo::Joint>> joints_;
 
-  std::map<std::string, control_toolbox::Pid> position_pid_controllers_;
-  std::map<std::string, control_toolbox::Pid> velocity_pid_controllers_;
-
   hardware_interface::JointStateInterface jsi_;
   hardware_interface::EffortJointInterface eji_;
   hardware_interface::PositionJointInterface pji_;
   hardware_interface::VelocityJointInterface vji_;
   franka_hw::FrankaStateInterface fsi_;
   franka_hw::FrankaModelInterface fmi_;
 
+  boost::sml::sm<franka_gazebo::StateMachine, boost::sml::thread_safe<std::mutex>> sm_;
   franka::RobotState robot_state_;
   std::unique_ptr<franka_hw::ModelBase> model_;
 
@@ -145,6 +152,10 @@ 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_user_stop_;
+  ros::ServiceClient service_controller_list_;
+  ros::ServiceClient service_controller_switch_;
+  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>> action_recovery_;
 
   std::vector<double> lower_force_thresholds_nominal_;
   std::vector<double> upper_force_thresholds_nominal_;
@@ -162,13 +173,18 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
                              double singularity_threshold);
   void initServices(ros::NodeHandle& nh);
 
+  void restartControllers();
+
   void updateRobotState(ros::Time time);
   void updateRobotStateDynamics();
 
   bool readParameters(const ros::NodeHandle& nh, const urdf::Model& urdf);
 
   void guessEndEffector(const ros::NodeHandle& nh, const urdf::Model& urdf);
 
+  static double positionControl(Joint& joint, double setpoint, const ros::Duration& period);
+  static double velocityControl(Joint& joint, double setpoint, const ros::Duration& period);
+
   template <int N>
   std::array<double, N> readArray(std::string param, std::string name = "") {
     std::array<double, N> x;

franka_gazebo/include/franka_gazebo/joint.h

@@ -1,9 +1,12 @@
 #pragma once
 
 #include <angles/angles.h>
+#include <control_toolbox/pid.h>
+#include <franka/robot_state.h>
 #include <joint_limits_interface/joint_limits.h>
 #include <ros/ros.h>
 #include <Eigen/Dense>
+#include <boost/optional.hpp>
 #include <gazebo/physics/Joint.hh>
 
 namespace franka_gazebo {
@@ -64,8 +67,8 @@ struct Joint {
   double desired_velocity = 0;
 
   /// Decides whether the joint is doing torque control or if the position or velocity should
-  /// be controlled.
-  ControlMethod control_method = POSITION;
+  /// be controlled, or if the joint is entirely uncontrolled
+  boost::optional<ControlMethod> control_method = boost::none;
 
   /// The currently acting gravity force or torque acting on this joint in \f$N\f$ or \f$Nm\f$
   double gravity = 0;
@@ -94,6 +97,50 @@ struct Joint {
   /// isInCollision
   double collision_threshold = std::numeric_limits<double>::infinity();
 
+  /// Position used as desired position if `control_method` is none
+  double stop_position = 0;
+
+  /**
+   * Decide what the desired position of this joint is based on:
+   * 1. If a reflex is present, return `position`
+   * 2. ...otherwise if the control method is POSITION, return `desired_position`
+   * 3. ...otherwise if the control method is EFFORT return `desired_position`
+   * 3. ...otherwise return `position`
+   * @param[in] mode - the current mode the robot is in
+   * @return either `position` or `desired_position`
+   */
+  double getDesiredPosition(const franka::RobotMode& mode) const;
+
+  /**
+   * Decide what the desired velocity of this joint is based on:
+   * 1. If a reflex is present, return `velocity`
+   * 2. ...otherwise if the control method is not VELOCITY, return `velocity`
+   * 3. ...otherwise return `desired_velocity`
+   * @param[in] mode - the current mode the robot is in
+   * @return either `velocity` or `desired_velocity`
+   */
+  double getDesiredVelocity(const franka::RobotMode& mode) const;
+
+  /**
+   * Decide what the desired acceleration of this joint is based on:
+   * 1. If a reflex is present, return `acceleration`
+   * 2. ...otherwise if the control method is EFFORT, return 0
+   * 3. ...otherwise return `acceleration`
+   * @param[in] mode - the current mode the robot is in
+   * @return either `acceleration` or `0`
+   */
+  double getDesiredAcceleration(const franka::RobotMode& mode) const;
+
+  /**
+   * Decide what the desired torque of this joint is:
+   * 1. If a reflex is present, return 0
+   * 2. ... otherwise if the control method is not EFFORT return 0
+   * 3. ... otherwise return `command`
+   * @param[in] mode - the current mode the robot is in
+   * @return either `command` or zero
+   */
+  double getDesiredTorque(const franka::RobotMode& mode) const;
+
   /**
    * Get the total link mass of this joint's child
    * @return the mass in \f$kg\f$
@@ -112,6 +159,14 @@ struct Joint {
    */
   bool isInCollision() const;
 
+  /// The PID gains used for the controller, when in "position" control mode. In other modes these
+  /// gains are ignored.
+  control_toolbox::Pid position_controller;
+
+  /// The PID gains used for the controller, when in "velocity" control mode. In other modes these
+  /// gains are ignored.
+  control_toolbox::Pid velocity_controller;
+
  private:
   double lastVelocity = std::numeric_limits<double>::quiet_NaN();
   double lastAcceleration = std::numeric_limits<double>::quiet_NaN();

franka_gazebo/include/franka_gazebo/statemachine.h

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <franka/robot_state.h>
+#include <franka_gazebo/joint.h>
+
+#include <ros/ros.h>
+#include <boost_sml/sml.hpp>
+#include <map>
+#include <memory>
+#include <string>
+
+namespace franka_gazebo {
+
+using JointMap = std::map<std::string, std::shared_ptr<Joint>>;
+
+// States
+struct Idle {};
+struct Move {};
+struct UserStopped {};
+
+// Events
+struct ErrorRecovery {};
+struct SwitchControl {};
+struct UserStop {
+  bool pressed;
+};
+
+// Guards
+auto contains = [](const auto& haystack, const auto& needle) {
+  return haystack.find(needle) != std::string::npos;
+};
+auto isPressed = [](const UserStop& event) { return event.pressed; };
+auto isReleased = [](const UserStop& event, const JointMap& joints) { return not event.pressed; };
+auto isStarting = [](const SwitchControl& event, const JointMap& joints) {
+  for (auto& joint : joints) {
+    if (contains(joint.first, "_finger_joint")) {
+      continue;
+    }
+    if (joint.second->control_method) {
+      return true;
+    }
+  }
+  return false;
+};
+auto isStopping = [](const SwitchControl& event, const JointMap& joints) {
+  return not isStarting(event, joints);
+};
+
+// Actions
+auto start = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kMove; };
+auto idle = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kIdle; };
+auto stop = [](franka::RobotState& state, JointMap& joints) {
+  ROS_WARN("User stop pressed, stopping robot");
+  state.robot_mode = franka::RobotMode::kUserStopped;
+  state.q_d = state.q;
+  state.dq_d = {0};
+  state.ddq_d = {0};
+
+  for (auto& joint : joints) {
+    if (contains(joint.first, "_finger_joint")) {
+      continue;
+    }
+    joint.second->stop_position = joint.second->position;
+    joint.second->desired_position = joint.second->position;
+    joint.second->desired_velocity = 0;
+  }
+};
+
+struct StateMachine {
+  auto operator()() const {
+    using namespace boost::sml;
+    return make_transition_table(
+        // clang-format off
+       *state<Idle>        + event<SwitchControl>[isStarting] / start = state<Move>,
+        state<Idle>        + event<UserStop>[isPressed]       / stop  = state<UserStopped>,
+        state<Idle>        + event<ErrorRecovery>             / start = state<Move>,
+        state<Move>        + event<SwitchControl>[isStopping] / idle  = state<Idle>,
+        state<Move>        + event<UserStop>[isPressed]       / stop  = state<UserStopped>,
+        state<UserStopped> + event<UserStop>[isReleased]      / idle  = state<Idle>
+        // clang-format on
+    );
+  }
+};
+}  // namespace franka_gazebo

franka_gazebo/launch/panda.launch

@@ -31,6 +31,7 @@
                 -J $(arg arm_id)_finger_joint1 0.001
                 -J $(arg arm_id)_finger_joint2 0.001"
        />
+  <arg name="interactive_marker" default="$(eval arg('controller') == 'cartesian_impedance_example_controller')" doc="Should the interactive marker node be started?" />
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch" if="$(arg gazebo)">
     <arg name="world_name" value="$(arg world)"/>
@@ -90,7 +91,7 @@
   <node name="interactive_marker"
         pkg="franka_example_controllers"
         type="interactive_marker.py"
-        if="$(eval arg('controller') == 'cartesian_impedance_example_controller')">
+        if="$(arg interactive_marker)">
     <param name="link_name" value="$(arg arm_id)_link0" />
     <remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
   </node>

franka_gazebo/package.xml

@@ -20,6 +20,7 @@
   <depend>gazebo_ros_control</depend>
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
+  <depend>boost_sml</depend>
   <depend>control_msgs</depend>
   <depend>control_toolbox</depend>
   <depend>controller_manager</depend>