Add franka_control package.

franka_control/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(franka_control)
+
+list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_CURRENT_SOURCE_DIR}/../cmake)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  controller_interface
+  controller_manager
+  franka_description
+  franka_hw
+  franka_msgs
+  geometry_msgs
+  message_generation
+  pluginlib
+  realtime_tools
+  roscpp
+  sensor_msgs
+  tf
+  tf2_msgs
+)
+
+find_package(Franka REQUIRED)
+
+add_service_files(FILES
+  SetCartesianImpedance.srv
+  SetEEFrame.srv
+  SetForceTorqueCollisionBehavior.srv
+  SetFullCollisionBehavior.srv
+  SetJointImpedance.srv
+  SetKFrame.srv
+  SetLoad.srv
+)
+
+add_action_files(FILES
+  ErrorRecovery.action
+)
+
+generate_messages(DEPENDENCIES actionlib_msgs)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES franka_state_controller
+  CATKIN_DEPENDS
+    actionlib
+    controller_interface
+    franka_hw
+    franka_msgs
+    geometry_msgs
+    message_runtime
+    pluginlib
+    realtime_tools
+    roscpp
+    sensor_msgs
+    tf2_msgs
+  DEPENDS Franka
+)
+
+add_library(franka_state_controller
+  src/franka_state_controller.cpp
+)
+
+add_dependencies(franka_state_controller
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(franka_state_controller
+  ${catkin_LIBRARIES}
+  Franka::Franka
+)
+
+target_include_directories(franka_state_controller PUBLIC
+  ${catkin_INCLUDE_DIRS}
+  include
+)
+
+add_executable(franka_control_node
+  src/franka_control_node.cpp
+  src/services.cpp
+)
+
+add_dependencies(franka_control_node
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(franka_control_node
+  ${catkin_LIBRARIES}
+  Franka::Franka
+)
+
+target_include_directories(franka_control_node PUBLIC
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Tools
+file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
+file(GLOB_RECURSE HEADERS
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
+)
+include(ClangTools)
+add_format_target(franka_control FILES ${SOURCES} ${HEADERS})
+add_tidy_target(franka_control
+  FILES ${SOURCES}
+  DEPENDS franka_control_node franka_state_controller
+)

franka_control/action/ErrorRecovery.action

+---
+---

franka_control/config/default_controllers.yaml

+position_joint_trajectory_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - franka_emika_joint1
+    - franka_emika_joint2
+    - franka_emika_joint3
+    - franka_emika_joint4
+    - franka_emika_joint5
+    - franka_emika_joint6
+    - franka_emika_joint7
+  constraints:
+    goal_time: 0.5
+    franka_emika_joint1:
+      goal: 0.05
+    franka_emika_joint2:
+      goal: 0.05
+    franka_emika_joint3:
+      goal: 0.05
+    franka_emika_joint4:
+      goal: 0.05
+    franka_emika_joint5:
+      goal: 0.05
+    franka_emika_joint6:
+      goal: 0.05
+    franka_emika_joint7:
+      goal: 0.05
+
+effort_joint_trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - franka_emika_joint1
+    - franka_emika_joint2
+    - franka_emika_joint3
+    - franka_emika_joint4
+    - franka_emika_joint5
+    - franka_emika_joint6
+    - franka_emika_joint7
+  constraints:
+    goal_time: 0.5
+    franka_emika_joint1:
+      goal: 0.05
+    franka_emika_joint2:
+      goal: 0.05
+    franka_emika_joint3:
+      goal: 0.05
+    franka_emika_joint4:
+      goal: 0.05
+    franka_emika_joint5:
+      goal: 0.05
+    franka_emika_joint6:
+      goal: 0.05
+    franka_emika_joint7:
+      goal: 0.05
+  gains:
+    franka_emika_joint1: {p: 100, d: 10, i: 0, i_clamp: 1}
+    franka_emika_joint2: {p: 100, d: 10, i: 0, i_clamp: 1}
+    franka_emika_joint3: {p: 100, d: 10, i: 0, i_clamp: 1}
+    franka_emika_joint4: {p: 100, d: 10, i: 0, i_clamp: 1}
+    franka_emika_joint5: {p: 100, d: 10, i: 0, i_clamp: 1}
+    franka_emika_joint6: {p: 100, d: 10, i: 0, i_clamp: 1}
+    franka_emika_joint7: {p: 100, d: 10, i: 0, i_clamp: 1}
+
+franka_state_controller:
+  type: franka_control/FrankaStateController
+  publish_rate: 30  # [Hz]
+

franka_control/config/franka_control.yaml

+joint_names:
+  - franka_emika_joint1
+  - franka_emika_joint2
+  - franka_emika_joint3
+  - franka_emika_joint4
+  - franka_emika_joint5
+  - franka_emika_joint6
+  - franka_emika_joint7
+
+arm_id: franka_emika
+robot_ip: 127.0.0.1

franka_control/franka_control_controller_plugin.xml

+<library path="lib/libfranka_state_controller">
+  <class name="franka_control/FrankaStateController" type="franka_control::FrankaStateController" base_class_type="controller_interface::ControllerBase">
+    <description>A controller that publishes the complete robot state</description>
+  </class>
+</library>

franka_control/include/franka_control/franka_state_controller.h

+#pragma once
+
+#include <cstdint>
+#include <memory>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <franka_hw/trigger_rate.h>
+#include <franka_msgs/FrankaState.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <sensor_msgs/JointState.h>
+#include <tf2_msgs/TFMessage.h>
+
+namespace franka_control {
+
+class FrankaStateController
+    : public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface> {
+ public:
+  FrankaStateController();
+
+  /**
+  * Initializes the controller with interfaces and publishers
+  *
+  * @param[in] hardware Pointer to the robot hardware
+  * @param[in] root_node_handle Nodehandle on root level passed from HW node
+  * @param[in] controller_node_handle Nodehandle in the controller namespace
+  * passed from HW node
+  */
+  bool init(hardware_interface::RobotHW* robot_hardware,
+            ros::NodeHandle& root_node_handle,
+            ros::NodeHandle& controller_node_handle) override;
+
+  /**
+  * Reads a new franka robot state and publishes it
+  *
+  * @param[in] time Current ros time
+  */
+  void update(const ros::Time& time, const ros::Duration&) override;
+
+ private:
+  /**
+  * Publishes all relevant data received from the Franka arm
+  *
+  * @param[in] time Current ros time
+  */
+  void publishFrankaStates(const ros::Time& time);
+
+  /**
+  * Publishes the joint states of the Franka arm
+  *
+  * @param[in] time Current ros time
+  */
+  void publishJointStates(const ros::Time& time);
+
+  /**
+  * Publishes the transforms for EE and K frame which define the end-effector
+  * (EE) and the Cartesian impedance reference frame (K)
+  *
+  * @param[in] time Current ros time
+  */
+  void publishTransforms(const ros::Time& time);
+
+  /**
+  * Publishes the estimated external wrench felt by the Franka
+  *
+  * @param[in] time Current ros time
+  */
+  void publishExternalWrench(const ros::Time& time);
+
+  std::string arm_id_;
+
+  franka_hw::FrankaStateInterface* franka_state_interface_;
+  std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
+
+  realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
+  realtime_tools::RealtimePublisher<franka_msgs::FrankaState> publisher_franka_states_;
+  realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_;
+  realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> publisher_external_wrench_;
+  franka_hw::TriggerRate trigger_publish_;
+  franka::RobotState robot_state_;
+  uint64_t sequence_number_ = 0;
+  std::vector<std::string> joint_names_;
+};
+
+}  // namespace franka_control

franka_control/launch/franka_control.launch

+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_ip" default="robot.franka.de" />
+  <arg name="arm_id" default="franka_emika" />
+  <arg name="load_gripper" default="true" />
+
+  <node name="$(arg arm_id)" pkg="franka_control" type="franka_control_node" output="screen" required="true">
+    <rosparam command="load" file="$(find franka_control)/config/franka_control.yaml" />
+    <param name="robot_ip" value="$(arg robot_ip)" />
+    <param name="arm_id" value="$(arg arm_id)" />
+    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01_Gripper.urdf.xacro'" if="$(arg load_gripper)" />
+    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01.urdf.xacro'" unless="$(arg load_gripper)" />
+  </node>
+
+  <group ns="$(arg arm_id)">
+    <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
+    <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="franka_state_controller --shutdown-timeout 2"/>
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
+    <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
+      <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01_Gripper.urdf.xacro'" />
+      <rosparam  if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper_node/joint_states] </rosparam>
+      <rosparam  unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
+      <param name="rate" value="30"/>
+    </node>
+  </group>
+</launch>

franka_control/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>franka_control</name>
+  <version>0.0.1</version>
+  <description>franka_control provides a hardware node to control a FRANKA EMIKA research robot</description>
+  <maintainer email="info@franka.de">FRANKA EMIKA Research Interface Team</maintainer>
+  <license>NONE</license>
+
+  <url type="website">http://wiki.ros.org/franka_control</url>
+  <url type="repository">https://github.com/frankaemika/franka_ros</url>
+  <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
+  <author>FRANKA EMIKA Research Interface Team</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>Franka</depend>
+  <depend>actionlib</depend>
+  <depend>actionlib_msgs</depend>
+  <depend>controller_interface</depend>
+  <depend>controller_manager</depend>
+  <depend>franka_description</depend>
+  <depend>franka_hw</depend>
+  <depend>franka_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>realtime_tools</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/franka_control_controller_plugin.xml"/>
+  </export>
+</package>

franka_control/src/franka_control_node.cpp

+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <string>
+#include <utility>
+
+#include <actionlib/server/simple_action_server.h>
+#include <controller_manager/controller_manager.h>
+#include <franka/exception.h>
+#include <franka/robot.h>
+#include <franka_control/ErrorRecoveryAction.h>
+#include <franka_hw/franka_hw.h>
+#include <ros/ros.h>
+
+#include "services.h"
+
+class ServiceContainer {
+ public:
+  template <typename T, typename... TArgs>
+  ServiceContainer& advertiseService(TArgs&&... args) {
+    ros::ServiceServer server = franka_control::advertiseService<T>(std::forward<TArgs>(args)...);
+    services_.push_back(server);
+    return *this;
+  }
+
+ private:
+  std::vector<ros::ServiceServer> services_;
+};
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "franka_hw");
+  ros::NodeHandle node_handle("~");
+
+  std::vector<std::string> joint_names_vector;
+  if (!node_handle.getParam("joint_names", joint_names_vector) || joint_names_vector.size() != 7) {
+    ROS_ERROR("Invalid or no joint_names parameters provided");
+    return 1;
+  }
+  std::array<std::string, 7> joint_names;
+  std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
+
+  std::string robot_ip;
+  node_handle.getParam("robot_ip", robot_ip);
+  std::string arm_id;
+  node_handle.getParam("arm_id", arm_id);
+  franka::Robot robot(robot_ip);
+
+  // Set default collision behavior
+  robot.setCollisionBehavior(
+      {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
+      {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
+      {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
+      {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
+
+  std::atomic_bool has_error(false);
+
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  ServiceContainer services;
+  services
+      .advertiseService<franka_control::SetJointImpedance>(
+          node_handle, "set_joint_impedance",
+          std::bind(franka_control::setJointImpedance, std::ref(robot), _1, _2))
+      .advertiseService<franka_control::SetCartesianImpedance>(
+          node_handle, "set_cartesian_impedance",
+          std::bind(franka_control::setCartesianImpedance, std::ref(robot), _1, _2))
+      .advertiseService<franka_control::SetEEFrame>(
+          node_handle, "set_EE_frame",
+          std::bind(franka_control::setEEFrame, std::ref(robot), _1, _2))
+      .advertiseService<franka_control::SetKFrame>(
+          node_handle, "set_K_frame", std::bind(franka_control::setKFrame, std::ref(robot), _1, _2))
+      .advertiseService<franka_control::SetForceTorqueCollisionBehavior>(
+          node_handle, "set_force_torque_collision_behavior",
+          std::bind(franka_control::setForceTorqueCollisionBehavior, std::ref(robot), _1, _2))
+      .advertiseService<franka_control::SetFullCollisionBehavior>(
+          node_handle, "set_full_collision_behavior",
+          std::bind(franka_control::setFullCollisionBehavior, std::ref(robot), _1, _2))
+      .advertiseService<franka_control::SetLoad>(
+          node_handle, "set_load", std::bind(franka_control::setLoad, std::ref(robot), _1, _2));
+
+  actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction> recovery_action_server(
+      node_handle, "error_recovery",
+      [&](const franka_control::ErrorRecoveryGoalConstPtr&) {
+        try {
+          robot.automaticErrorRecovery();
+          has_error = false;
+          recovery_action_server.setSucceeded();
+        } catch (const franka::Exception& ex) {
+          recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what());
+        }
+      },
+      false);
+
+  franka::Model model = robot.loadModel();
+  franka_hw::FrankaHW franka_control(joint_names, arm_id, node_handle, model);
+
+  // Initialize robot state before loading any controller
+  franka_control.update(robot.readOnce());
+
+  controller_manager::ControllerManager control_manager(&franka_control, node_handle);
+
+  recovery_action_server.start();
+
+  // Start background threads for message handling
+  ros::AsyncSpinner spinner(4);
+  spinner.start();
+
+  while (ros::ok()) {
+    ros::Time last_time = ros::Time::now();
+
+    // Wait until controller has been activated or error has been recovered
+    while (!franka_control.controllerActive() || has_error) {
+      franka_control.update(robot.readOnce());
+
+      ros::Time now = ros::Time::now();
+      control_manager.update(now, now - last_time);
+      last_time = now;
+
+      if (!ros::ok()) {
+        return 0;
+      }
+    }
+
+    try {
+      // Run control loop. Will exit if the controller is switched.
+      franka_control.control(robot, [&](const ros::Time& now, const ros::Duration& period) {
+        if (period.toSec() == 0.0) {
+          // Reset controllers before starting a motion
+          control_manager.update(now, period, true);
+          franka_control.reset();
+        } else {
+          control_manager.update(now, period);
+          franka_control.enforceLimits(period);
+        }
+        return ros::ok();
+      });
+    } catch (const franka::ControlException& e) {
+      ROS_ERROR("%s", e.what());
+      has_error = true;
+    }
+  }
+
+  return 0;
+}

franka_control/src/franka_state_controller.cpp

+#include <franka_control/franka_state_controller.h>
+
+#include <cmath>
+#include <mutex>
+#include <string>
+
+#include <franka/errors.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_msgs/Errors.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <tf/transform_datatypes.h>
+
+namespace {
+
+tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
+  tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],
+                         transform[9], transform[2], transform[6], transform[10]);
+  tf::Vector3 translation(transform[12], transform[13], transform[14]);
+  return tf::Transform(rotation, translation);
+}
+
+franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
+  franka_msgs::Errors message;
+  message.joint_position_limits_violation =
+      static_cast<decltype(message.joint_position_limits_violation)>(
+          error.joint_position_limits_violation);
+  message.cartesian_position_limits_violation =
+      static_cast<decltype(message.cartesian_position_limits_violation)>(
+          error.cartesian_position_limits_violation);
+  message.self_collision_avoidance_violation =
+      static_cast<decltype(message.self_collision_avoidance_violation)>(
+          error.self_collision_avoidance_violation);
+  message.joint_velocity_violation =
+      static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
+  message.cartesian_velocity_violation =
+      static_cast<decltype(message.cartesian_velocity_violation)>(
+          error.cartesian_velocity_violation);
+  message.force_control_safety_violation =
+      static_cast<decltype(message.force_control_safety_violation)>(
+          error.force_control_safety_violation);
+  message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
+  message.cartesian_reflex =
+      static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
+  message.max_goal_pose_deviation_violation =
+      static_cast<decltype(message.max_goal_pose_deviation_violation)>(
+          error.max_goal_pose_deviation_violation);
+  message.max_path_pose_deviation_violation =
+      static_cast<decltype(message.max_path_pose_deviation_violation)>(
+          error.max_path_pose_deviation_violation);
+  message.cartesian_velocity_profile_safety_violation =
+      static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
+          error.cartesian_velocity_profile_safety_violation);
+  message.joint_position_motion_generator_start_pose_invalid =
+      static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
+          error.joint_position_motion_generator_start_pose_invalid);
+  message.joint_motion_generator_position_limits_violation =
+      static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
+          error.joint_motion_generator_position_limits_violation);
+  message.joint_motion_generator_velocity_limits_violation =
+      static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
+          error.joint_motion_generator_velocity_limits_violation);
+  message.joint_motion_generator_velocity_discontinuity =
+      static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
+          error.joint_motion_generator_velocity_discontinuity);
+  message.joint_motion_generator_acceleration_discontinuity =
+      static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
+          error.joint_motion_generator_acceleration_discontinuity);
+  message.cartesian_position_motion_generator_start_pose_invalid =
+      static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
+          error.cartesian_position_motion_generator_start_pose_invalid);
+  message.cartesian_motion_generator_elbow_limit_violation =
+      static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
+          error.cartesian_motion_generator_elbow_limit_violation);
+  message.cartesian_motion_generator_velocity_limits_violation =
+      static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
+          error.cartesian_motion_generator_velocity_limits_violation);
+  message.cartesian_motion_generator_velocity_discontinuity =
+      static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
+          error.cartesian_motion_generator_velocity_discontinuity);
+  message.cartesian_motion_generator_acceleration_discontinuity =
+      static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
+          error.cartesian_motion_generator_acceleration_discontinuity);
+  message.cartesian_motion_generator_elbow_sign_inconsistent =
+      static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
+          error.cartesian_motion_generator_elbow_sign_inconsistent);
+  message.cartesian_motion_generator_start_elbow_invalid =
+      static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
+          error.cartesian_motion_generator_start_elbow_invalid);
+  message.cartesian_motion_generator_joint_position_limits_violation =
+      static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
+          error.cartesian_motion_generator_joint_position_limits_violation);
+  message.cartesian_motion_generator_joint_velocity_limits_violation =
+      static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
+          error.cartesian_motion_generator_joint_velocity_limits_violation);
+  message.cartesian_motion_generator_joint_velocity_discontinuity =
+      static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
+          error.cartesian_motion_generator_joint_velocity_discontinuity);
+  message.cartesian_motion_generator_joint_acceleration_discontinuity =
+      static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
+          error.cartesian_motion_generator_joint_acceleration_discontinuity);
+  message.cartesian_position_motion_generator_invalid_frame =
+      static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
+          error.cartesian_position_motion_generator_invalid_frame);
+  message.force_controller_desired_force_tolerance_violation =
+      static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
+          error.force_controller_desired_force_tolerance_violation);
+  message.controller_torque_discontinuity =
+      static_cast<decltype(message.controller_torque_discontinuity)>(
+          error.controller_torque_discontinuity);
+  message.start_elbow_sign_inconsistent =
+      static_cast<decltype(message.start_elbow_sign_inconsistent)>(
+          error.start_elbow_sign_inconsistent);
+  message.communication_constraints_violation =
+      static_cast<decltype(message.communication_constraints_violation)>(
+          error.communication_constraints_violation);
+  message.power_limit_violation =
+      static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
+  return message;
+}
+
+}  // anonymous namespace
+
+namespace franka_control {
+
+FrankaStateController::FrankaStateController()
+    : franka_state_interface_(nullptr),
+      franka_state_handle_(nullptr),
+      publisher_transforms_(),
+      publisher_franka_states_(),
+      publisher_joint_states_(),
+      publisher_external_wrench_(),
+      trigger_publish_(30.0) {}
+
+bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
+                                 ros::NodeHandle& root_node_handle,
+                                 ros::NodeHandle& controller_node_handle) {
+  franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
+  if (franka_state_interface_ == nullptr) {
+    ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
+    return false;
+  }
+  if (!root_node_handle.getParam("arm_id", arm_id_)) {
+    ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
+    return false;
+  }
+  double publish_rate(30.0);
+  if (controller_node_handle.getParam("publish_rate", publish_rate)) {
+    trigger_publish_ = franka_hw::TriggerRate(publish_rate);
+  } else {
+    ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
+                    << publish_rate << " [Hz].");
+  }
+
+  if (!root_node_handle.getParam("joint_names", joint_names_) ||
+      joint_names_.size() != robot_state_.q.size()) {
+    ROS_ERROR(
+        "FrankaStateController: Invalid or no joint_names parameters provided, aborting "
+        "controller init!");
+    return false;
+  }
+
+  try {
+    franka_state_handle_.reset(
+        new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot")));
+  } catch (const hardware_interface::HardwareInterfaceException& ex) {
+    ROS_ERROR_STREAM("FrankaStateController: Exception getting cartesian handle: " << ex.what());
+    return false;
+  }
+
+  publisher_transforms_.init(root_node_handle, "/tf", 1);
+  publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
+  publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
+  publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
+
+  {
+    std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > lock(
+        publisher_joint_states_);
+    publisher_joint_states_.msg_.name.resize(joint_names_.size());
+    publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
+    publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
+    publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
+  }
+  {
+    std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> > lock(
+        publisher_transforms_);
+    publisher_transforms_.msg_.transforms.resize(2);
+    tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
+    tf::Vector3 translation(0.0, 0.0, 0.05);
+    tf::Transform transform(quaternion, translation);
+    tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
+                                           arm_id_ + "_EE");
+    geometry_msgs::TransformStamped transform_message;
+    transformStampedTFToMsg(stamped_transform, transform_message);
+    publisher_transforms_.msg_.transforms[0] = transform_message;
+    translation = tf::Vector3(0.0, 0.0, 0.0);
+    transform = tf::Transform(quaternion, translation);
+    stamped_transform =
+        tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
+    transformStampedTFToMsg(stamped_transform, transform_message);
+    publisher_transforms_.msg_.transforms[1] = transform_message;
+  }
+  {
+    std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> > lock(
+        publisher_external_wrench_);
+    publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
+    publisher_external_wrench_.msg_.wrench.force.x = 0.0;
+    publisher_external_wrench_.msg_.wrench.force.y = 0.0;
+    publisher_external_wrench_.msg_.wrench.force.z = 0.0;
+    publisher_external_wrench_.msg_.wrench.torque.x = 0.0;
+    publisher_external_wrench_.msg_.wrench.torque.y = 0.0;
+    publisher_external_wrench_.msg_.wrench.torque.z = 0.0;
+  }
+  return true;
+}
+
+void FrankaStateController::update(const ros::Time& time, const ros::Duration& /*period*/) {
+  if (trigger_publish_()) {
+    robot_state_ = franka_state_handle_->getRobotState();
+    publishFrankaStates(time);
+    publishTransforms(time);
+    publishExternalWrench(time);
+    publishJointStates(time);
+    sequence_number_++;
+  }
+}
+
+void FrankaStateController::publishFrankaStates(const ros::Time& time) {
+  if (publisher_franka_states_.trylock()) {
+    static_assert(
+        sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
+        "Robot state Cartesian members do not have same size");
+    static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.K_F_ext_hat_K),
+                  "Robot state Cartesian members do not have same size");
+    static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_F_ext_hat_K),
+                  "Robot state Cartesian members do not have same size");
+    for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
+      publisher_franka_states_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
+      publisher_franka_states_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
+      publisher_franka_states_.msg_.K_F_ext_hat_K[i] = robot_state_.K_F_ext_hat_K[i];
+      publisher_franka_states_.msg_.O_F_ext_hat_K[i] = robot_state_.O_F_ext_hat_K[i];
+    }
+
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
+                  "Robot state joint members do not have same size");
+    for (size_t i = 0; i < robot_state_.q.size(); i++) {
+      publisher_franka_states_.msg_.q[i] = robot_state_.q[i];
+      publisher_franka_states_.msg_.q_d[i] = robot_state_.q_d[i];
+      publisher_franka_states_.msg_.dq[i] = robot_state_.dq[i];
+      publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
+      publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
+      publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
+      publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
+      publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
+      publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
+    }
+
+    for (size_t i = 0; i < robot_state_.elbow.size(); i++) {
+      publisher_franka_states_.msg_.elbow[i] = robot_state_.elbow[i];
+    }
+
+    for (size_t i = 0; i < robot_state_.elbow_d.size(); i++) {
+      publisher_franka_states_.msg_.elbow_d[i] = robot_state_.elbow_d[i];
+    }
+
+    static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
+                  "Robot state transforms do not have same size");
+    static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
+                  "Robot state transforms do not have same size");
+    static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
+                  "Robot state transforms do not have same size");
+    for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {
+      publisher_franka_states_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];
+      publisher_franka_states_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];
+      publisher_franka_states_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];
+      publisher_franka_states_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];
+    }
+    publisher_franka_states_.msg_.m_load = robot_state_.m_load;
+
+    for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
+      publisher_franka_states_.msg_.I_load[i] = robot_state_.I_load[i];
+    }
+
+    for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
+      publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
+    }
+
+    publisher_franka_states_.msg_.time = robot_state_.time.toSec();
+    publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
+    publisher_franka_states_.msg_.last_motion_errors =
+        errorsToMessage(robot_state_.last_motion_errors);
+
+    publisher_franka_states_.msg_.header.seq = sequence_number_;
+    publisher_franka_states_.msg_.header.stamp = time;
+    publisher_franka_states_.unlockAndPublish();
+  }
+}
+
+void FrankaStateController::publishJointStates(const ros::Time& time) {
+  if (publisher_joint_states_.trylock()) {
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
+                  "Robot state joint members do not have same size");
+    for (size_t i = 0; i < robot_state_.q.size(); i++) {
+      publisher_joint_states_.msg_.name[i] = joint_names_[i];
+      publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
+      publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
+      publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];
+    }
+    publisher_joint_states_.msg_.header.stamp = time;
+    publisher_joint_states_.msg_.header.seq = sequence_number_;
+    publisher_joint_states_.unlockAndPublish();
+  }
+}
+
+void FrankaStateController::publishTransforms(const ros::Time& time) {
+  if (publisher_transforms_.trylock()) {
+    tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_EE), time,
+                                           arm_id_ + "_link8", arm_id_ + "_EE");
+    geometry_msgs::TransformStamped transform_message;
+    transformStampedTFToMsg(stamped_transform, transform_message);
+    publisher_transforms_.msg_.transforms[0] = transform_message;
+    stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,
+                                             arm_id_ + "_EE", arm_id_ + "_K");
+    transformStampedTFToMsg(stamped_transform, transform_message);
+    publisher_transforms_.msg_.transforms[1] = transform_message;
+    publisher_transforms_.unlockAndPublish();
+  }
+}
+
+void FrankaStateController::publishExternalWrench(const ros::Time& time) {
+  if (publisher_external_wrench_.trylock()) {
+    publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
+    publisher_external_wrench_.msg_.header.stamp = time;
+    publisher_external_wrench_.msg_.wrench.force.x = robot_state_.K_F_ext_hat_K[0];
+    publisher_external_wrench_.msg_.wrench.force.y = robot_state_.K_F_ext_hat_K[1];
+    publisher_external_wrench_.msg_.wrench.force.z = robot_state_.K_F_ext_hat_K[2];
+    publisher_external_wrench_.msg_.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];
+    publisher_external_wrench_.msg_.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];
+    publisher_external_wrench_.msg_.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];
+    publisher_external_wrench_.unlockAndPublish();
+  }
+}
+
+}  // namespace franka_control
+
+PLUGINLIB_EXPORT_CLASS(franka_control::FrankaStateController, controller_interface::ControllerBase)

franka_control/src/services.cpp

+#include "services.h"
+
+namespace franka_control {
+
+void setCartesianImpedance(franka::Robot& robot,
+                           const SetCartesianImpedance::Request& req,
+                           SetCartesianImpedance::Response& /* res */) {
+  std::array<double, 6> cartesian_stiffness;
+  std::copy(req.cartesian_stiffness.cbegin(), req.cartesian_stiffness.cend(),
+            cartesian_stiffness.begin());
+  robot.setCartesianImpedance(cartesian_stiffness);
+}
+
+void setJointImpedance(franka::Robot& robot,
+                       const SetJointImpedance::Request& req,
+                       SetJointImpedance::Response& /* res */) {
+  std::array<double, 7> joint_stiffness;
+  std::copy(req.joint_stiffness.cbegin(), req.joint_stiffness.cend(), joint_stiffness.begin());
+  robot.setJointImpedance(joint_stiffness);
+}
+
+void setEEFrame(franka::Robot& robot,
+                const SetEEFrame::Request& req,
+                SetEEFrame::Response& /* res */) {
+  std::array<double, 16> F_T_EE;  // NOLINT [readability-identifier-naming]
+  std::copy(req.F_T_EE.cbegin(), req.F_T_EE.cend(), F_T_EE.begin());
+  robot.setEE(F_T_EE);
+}
+
+void setKFrame(franka::Robot& robot,
+               const SetKFrame::Request& req,
+               SetKFrame::Response& /* res */) {
+  std::array<double, 16> EE_T_K;  // NOLINT [readability-identifier-naming]
+  std::copy(req.EE_T_K.cbegin(), req.EE_T_K.cend(), EE_T_K.begin());
+  robot.setK(EE_T_K);
+}
+
+void setForceTorqueCollisionBehavior(franka::Robot& robot,
+                                     const SetForceTorqueCollisionBehavior::Request& req,
+                                     SetForceTorqueCollisionBehavior::Response& /* res */
+                                     ) {
+  std::array<double, 7> lower_torque_thresholds_nominal;
+  std::copy(req.lower_torque_thresholds_nominal.cbegin(),
+            req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
+  std::array<double, 7> upper_torque_thresholds_nominal;
+  std::copy(req.upper_torque_thresholds_nominal.cbegin(),
+            req.upper_torque_thresholds_nominal.cend(), upper_torque_thresholds_nominal.begin());
+  std::array<double, 6> lower_force_thresholds_nominal;
+  std::copy(req.lower_force_thresholds_nominal.cbegin(), req.lower_force_thresholds_nominal.cend(),
+            lower_force_thresholds_nominal.begin());
+  std::array<double, 6> upper_force_thresholds_nominal;
+  std::copy(req.upper_force_thresholds_nominal.cbegin(), req.upper_force_thresholds_nominal.cend(),
+            upper_force_thresholds_nominal.begin());
+
+  robot.setCollisionBehavior(lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
+                             lower_force_thresholds_nominal, upper_force_thresholds_nominal);
+}
+
+void setFullCollisionBehavior(franka::Robot& robot,
+                              const SetFullCollisionBehavior::Request& req,
+                              SetFullCollisionBehavior::Response& /* res */) {
+  std::array<double, 7> lower_torque_thresholds_acceleration;
+  std::copy(req.lower_torque_thresholds_acceleration.cbegin(),
+            req.lower_torque_thresholds_acceleration.cend(),
+            lower_torque_thresholds_acceleration.begin());
+  std::array<double, 7> upper_torque_thresholds_acceleration;
+  std::copy(req.upper_torque_thresholds_acceleration.cbegin(),
+            req.upper_torque_thresholds_acceleration.cend(),
+            upper_torque_thresholds_acceleration.begin());
+  std::array<double, 7> lower_torque_thresholds_nominal;
+  std::copy(req.lower_torque_thresholds_nominal.cbegin(),
+            req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
+  std::array<double, 7> upper_torque_thresholds_nominal;
+  std::copy(req.upper_torque_thresholds_nominal.cbegin(),
+            req.upper_torque_thresholds_nominal.cend(), upper_torque_thresholds_nominal.begin());
+  std::array<double, 6> lower_force_thresholds_acceleration;
+  std::copy(req.lower_force_thresholds_acceleration.cbegin(),
+            req.lower_force_thresholds_acceleration.cend(),
+            lower_force_thresholds_acceleration.begin());
+  std::array<double, 6> upper_force_thresholds_acceleration;
+  std::copy(req.upper_force_thresholds_acceleration.cbegin(),
+            req.upper_force_thresholds_acceleration.cend(),
+            upper_force_thresholds_acceleration.begin());
+  std::array<double, 6> lower_force_thresholds_nominal;
+  std::copy(req.lower_force_thresholds_nominal.cbegin(), req.lower_force_thresholds_nominal.cend(),
+            lower_force_thresholds_nominal.begin());
+  std::array<double, 6> upper_force_thresholds_nominal;
+  std::copy(req.upper_force_thresholds_nominal.cbegin(), req.upper_force_thresholds_nominal.cend(),
+            upper_force_thresholds_nominal.begin());
+  robot.setCollisionBehavior(lower_torque_thresholds_acceleration,
+                             upper_torque_thresholds_acceleration, lower_torque_thresholds_nominal,
+                             upper_torque_thresholds_nominal, lower_force_thresholds_acceleration,
+                             upper_force_thresholds_acceleration, lower_force_thresholds_nominal,
+                             upper_force_thresholds_nominal);
+}
+
+void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& /* res */) {
+  double mass(req.mass);
+  std::array<double, 3> F_x_center_load;  // NOLINT [readability-identifier-naming]
+  std::copy(req.F_x_center_load.cbegin(), req.F_x_center_load.cend(), F_x_center_load.begin());
+  std::array<double, 9> load_inertia;
+  std::copy(req.load_inertia.cbegin(), req.load_inertia.cend(), load_inertia.begin());
+
+  robot.setLoad(mass, F_x_center_load, load_inertia);
+}
+
+}  // namespace franka_control

franka_control/src/services.h

+#pragma once
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+#include <ros/console.h>
+#include <ros/node_handle.h>
+#include <ros/service_server.h>
+
+#include <franka_control/SetCartesianImpedance.h>
+#include <franka_control/SetEEFrame.h>
+#include <franka_control/SetForceTorqueCollisionBehavior.h>
+#include <franka_control/SetFullCollisionBehavior.h>
+#include <franka_control/SetJointImpedance.h>
+#include <franka_control/SetKFrame.h>
+#include <franka_control/SetLoad.h>
+
+namespace franka_control {
+
+template <typename T>
+ros::ServiceServer advertiseService(
+    ros::NodeHandle& node_handle,
+    const std::string& name,
+    std::function<void(typename T::Request&, typename T::Response&)> handler) {
+  return node_handle.advertiseService<typename T::Request, typename T::Response>(
+      name, [name, handler](typename T::Request& request, typename T::Response& response) {
+        try {
+          handler(request, response);
+          response.success = true;
+          ROS_DEBUG_STREAM(name << " succeeded.");
+        } catch (const franka::Exception& ex) {
+          ROS_ERROR_STREAM(name << " failed: " << ex.what());
+          response.success = false;
+          response.error = ex.what();
+        }
+        return true;
+      });
+}
+
+void setCartesianImpedance(franka::Robot& robot,
+                           const SetCartesianImpedance::Request& req,
+                           SetCartesianImpedance::Response& res);
+void setJointImpedance(franka::Robot& robot,
+                       const SetJointImpedance::Request& req,
+                       SetJointImpedance::Response& res);
+void setEEFrame(franka::Robot& robot, const SetEEFrame::Request& req, SetEEFrame::Response& res);
+void setKFrame(franka::Robot& robot, const SetKFrame::Request& req, SetKFrame::Response& res);
+void setForceTorqueCollisionBehavior(franka::Robot& robot,
+                                     const SetForceTorqueCollisionBehavior::Request& req,
+                                     SetForceTorqueCollisionBehavior::Response& res);
+void setFullCollisionBehavior(franka::Robot& robot,
+                              const SetFullCollisionBehavior::Request& req,
+                              SetFullCollisionBehavior::Response& res);
+void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& res);
+
+}  // namespace franka_control

franka_control/srv/SetCartesianImpedance.srv

+float64[6] cartesian_stiffness
+---
+bool success
+string error
+

franka_control/srv/SetEEFrame.srv

+float64[16] F_T_EE
+---
+bool success
+string error
+

franka_control/srv/SetForceTorqueCollisionBehavior.srv

+float64[7] lower_torque_thresholds_nominal
+float64[7] upper_torque_thresholds_nominal
+float64[6] lower_force_thresholds_nominal
+float64[6] upper_force_thresholds_nominal
+---
+bool success
+string error
+

franka_control/srv/SetFullCollisionBehavior.srv

+float64[7] lower_torque_thresholds_acceleration
+float64[7] upper_torque_thresholds_acceleration
+float64[7] lower_torque_thresholds_nominal
+float64[7] upper_torque_thresholds_nominal
+float64[6] lower_force_thresholds_acceleration
+float64[6] upper_force_thresholds_acceleration
+float64[6] lower_force_thresholds_nominal
+float64[6] upper_force_thresholds_nominal
+---
+bool success
+string error
+

franka_control/srv/SetJointImpedance.srv

+float64[7] joint_stiffness
+---
+bool success
+string error
+

franka_control/srv/SetKFrame.srv

+float64[16] EE_T_K
+---
+bool success
+string error
+

franka_control/srv/SetLoad.srv

+float64 mass
+float64[3] F_x_center_load
+float64[9] load_inertia
+---
+bool success
+string error
+