diff --git a/franka_combinable_hw/CMakeLists.txt b/franka_combinable_hw/CMakeLists.txt
deleted file mode 100644
index 319b2df18a63b98be38dd986d15bc3dfdca253ab..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/CMakeLists.txt
+++ /dev/null
@@ -1,95 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_combinable_hw)
-
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_BUILD_TYPE Release)
-
-find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  combined_robot_hw
-  controller_interface
-  franka_hw
-  franka_control
-  hardware_interface
-  joint_limits_interface
-  pluginlib
-  roscpp
-  std_msgs
-)
-
-find_package(Franka 0.5.0 REQUIRED)
-
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES franka_combinable_hw combined_robot_hw
-  CATKIN_DEPENDS
-    actionlib
-    combined_robot_hw
-    controller_interface
-    franka_hw
-    franka_control
-    hardware_interface
-    joint_limits_interface
-    pluginlib
-    roscpp
-    std_msgs
-  DEPENDS Franka
-)
-
-add_library(franka_combinable_hw
-  src/franka_combinable_hw.cpp
-  src/franka_combined_hw.cpp
-)
-
-add_dependencies(franka_combinable_hw
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_combinable_hw
-  ${Franka_LIBRARIES}
-  ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_combinable_hw SYSTEM PUBLIC
-  ${Franka_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-)
-target_include_directories(franka_combinable_hw PUBLIC
-  include
-)
-
-## Installation
-install(TARGETS franka_combinable_hw
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-install(FILES franka_combinable_hw_plugin.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-if(CATKIN_ENABLE_TESTING)
-  add_subdirectory(test)
-endif()
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
-  RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_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
-  )
-  add_format_target(franka_combinable_hw FILES ${SOURCES} ${HEADERS})
-  add_tidy_target(franka_combinable_hw
-    FILES ${SOURCES}
-    DEPENDS franka_combinable_hw
-  )
-endif()
diff --git a/franka_combinable_hw/franka_combinable_hw_plugin.xml b/franka_combinable_hw/franka_combinable_hw_plugin.xml
deleted file mode 100644
index aa7a5c1aeda0c6619f7b6a6c1762ddc10f4d7e71..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/franka_combinable_hw_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="lib/libfranka_combinable_hw">
-  <class name="franka_combinable_hw/FrankaCombinableHW" type="franka_combinable_hw::FrankaCombinableHW" base_class_type="hardware_interface::RobotHW">
-    <description>
-      A robot hardware interface class for franka robots that is combinable with other FrankaCombinableHW.
-    </description>
-  </class>
-</library>
diff --git a/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h b/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h
deleted file mode 100644
index ba8c91e49d1078c9bece5f48962d8e4c7380125a..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h
+++ /dev/null
@@ -1,314 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#pragma once
-
-#include <array>
-#include <atomic>
-#include <exception>
-#include <functional>
-#include <string>
-#include <thread>
-
-#include <actionlib/server/simple_action_server.h>
-#include <franka/control_types.h>
-#include <franka/duration.h>
-#include <franka/exception.h>
-#include <franka/model.h>
-#include <franka/robot.h>
-#include <franka/robot_state.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <hardware_interface/joint_state_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <joint_limits_interface/joint_limits_interface.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-#include <urdf/model.h>
-
-#include <franka_control/ErrorRecoveryAction.h>
-#include <franka_control/services.h>
-#include <franka_hw/control_mode.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
-
-using franka_control::ServiceContainer;
-using franka_hw::ControlMode;
-
-namespace franka_combinable_hw {
-
-class FrankaCombinableHW : public hardware_interface::RobotHW {
- public:
-  /**
-   * Creates an instance of FrankaCombinableHW.
-   *
-   */
-  FrankaCombinableHW();
-
-  /**
-   * Initializes the FrankaCombinableHW class as far as possible without connecting to a robot. This
-   * includes setting up state, limit and command interfaces, publishers, services and actions.
-   * Note: This method is mainly for testing purposes. Use the \ref init() method to control real
-   * hardware.
-   *
-   * @param[in] root_nh A NodeHandle in the root of the caller namespace.
-   * @param[in] robot_hw_nh A NodeHandle in the namespace from which the RobotHW.
-   * @return True if the disconnected initialization was successful, false otherwise.
-   */
-  bool initROSInterfaces(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);
-
-  /**
-   * Initializes the FrankaCombinableHW with model, state, limit and command interfaces and connects
-   * to a panda robot via an IP provided via the parameter server.
-   *
-   * @param[in] root_nh A NodeHandle in the root of the caller namespace.
-   * @param[in] robot_hw_nh A NodeHandle in the namespace from which the RobotHW.
-   * should read its configuration.
-   *
-   * @return True if initialization was successful, false otherwise.
-   */
-  bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
-
-  ~FrankaCombinableHW() override = default;
-
-  /**
-   * Runs the currently active controller in a realtime loop.
-   *
-   * If no controller is active, the function immediately exits. When running a controller,
-   * the function only exits when ros_callback returns false.
-   *
-   * @param[in] robot  A libfranka Robot instance.
-   *
-   * @throw franka::ControlException if an error related to torque control or motion generation
-   * occurred.
-   * @throw franka::InvalidOperationException if a conflicting operation is already running.
-   * @throw franka::NetworkException if the connection is lost, e.g. after a timeout.
-   * @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
-   */
-  void control(franka::Robot& robot);
-
-  /**
-   * Updates the robot state in the controller interfaces from the given robot state.
-   *
-   * @param[in] robot_state Current robot state.
-   */
-  void update(const franka::RobotState& robot_state);
-
-  /**
-   * Indicates whether there is an active controller.
-   *
-   * @return True if a controller is currently active, false otherwise.
-   */
-  bool controllerActive() const noexcept;
-
-  /**
-   * Checks whether a requested controller can be run, based on the resources and interfaces it
-   * claims.
-   *
-   * @param[in] info Controllers to be running at the same time.
-   *
-   * @return True in case of a conflict, false in case of valid controllers.
-   */
-  bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override;
-
-  /**
-   * Performs controller switching (real-time capable).
-   */
-  void doSwitch(const std::list<hardware_interface::ControllerInfo>&,
-                const std::list<hardware_interface::ControllerInfo>&) override;
-
-  /**
-   * Prepares switching between controllers (not real-time capable).
-   *
-   * @param[in] start_list Controllers requested to be started.
-   * @param[in] stop_list Controllers requested to be stopped.
-   *
-   * @return True if the preparation has been successful, false otherwise.
-   */
-  bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
-                     const std::list<hardware_interface::ControllerInfo>& stop_list) override;
-
-  /**
-   * Reads data from the franka robot.
-   *
-   * @param[in] time The current time.
-   * @param[in] period The time passed since the last call to \ref read.
-   */
-  void read(const ros::Time& time, const ros::Duration& period) override;
-
-  /**
-   * Writes data to the franka robot.
-   *
-   * @param[in] time The current time.
-   * @param[in] period The time passed since the last call to \ref write.
-   */
-  void write(const ros::Time& time, const ros::Duration& period) override;
-
-  /**
-   * Gets the current joint torque command.
-   *
-   * @return Current joint torque command.
-   */
-  std::array<double, 7> getJointEffortCommand() const noexcept;
-
-  /**
-   * Enforces limits on torque level.
-   *
-   * @param[in] period Duration of the current cycle.
-   */
-  void enforceLimits(const ros::Duration& period);
-
-  /**
-   * Gets arm id.
-   */
-  std::string getArmID();
-
-  /**
-   * Trigger error state to stop controller.
-   */
-  void triggerError();
-
-  /**
-   * Returns error state.
-   *
-   * @return true if there is currently error.
-   */
-  bool hasError();
-
-  /**
-   * Reset error state.
-   */
-  void resetError();
-
-  /**
-   * Returns whether the controller needs to be reset.
-   */
-  bool controllerNeedsReset();
-
-  /**
-   * Returns whether the torque command contains NaN values.
-   *
-   * @param[in] command The torque commmand to check.
-   *
-   * @return true if the command contains NaN, false otherwise.
-   */
-  static bool commandHasNaN(const franka::Torques& command);
-
-  /**
-   * Returns whether the Cartesian pose command contains NaN values.
-   *
-   * @param[in] command The Cartesian pose commmand to check.
-   *
-   * @return true if the command contains NaN, false otherwise.
-   */
-  static bool commandHasNaN(const franka::CartesianPose& command);
-
-  /**
-   * Returns whether the Cartesian velocity command contains NaN values.
-   *
-   * @param[in] command The Cartesian velocity commmand to check.
-   *
-   * @return true if the command contains NaN, false otherwise.
-   */
-  static bool commandHasNaN(const franka::CartesianVelocities& command);
-
- private:
-  template <size_t size>
-  static bool arrayHasNaN(const std::array<double, size> array) {
-    return std::any_of(array.begin(), array.end(), [](const double& e) { return std::isnan(e); });
-  }
-
-  template <typename T>
-  T controlCallback(const T& command,
-                    const franka::RobotState& robot_state,
-                    franka::Duration time_step) {
-    if (commandHasNaN(command)) {
-      ROS_ERROR("FrankaCombinableHW: Got NaN value in command!");
-      throw franka::CommandException("Got NaN value in command");
-    }
-
-    checkJointLimits();
-
-    libfranka_state_mutex_.lock();
-    robot_state_libfranka_ = robot_state;
-    libfranka_state_mutex_.unlock();
-
-    libfranka_cmd_mutex_.lock();
-    T current_cmd = command;
-    libfranka_cmd_mutex_.unlock();
-
-    if (has_error_ || !controller_active_) {
-      return franka::MotionFinished(current_cmd);
-    }
-    return current_cmd;
-  }
-
-  void controlLoop();
-
-  void setupServicesAndActionServers(ros::NodeHandle& node_handle);
-
-  static std::array<double, 7> saturateTorqueRate(const std::array<double, 7>& tau_d_calculated,
-                                                  const std::array<double, 7>& tau_J_d);
-
-  bool initRobot(ros::NodeHandle& robot_hw_nh);
-
-  void publishErrorState(bool error);
-
-  void checkJointLimits();
-
-  std::unique_ptr<franka::Robot> robot_;
-  std::unique_ptr<franka::Model> model_;
-
-  hardware_interface::JointStateInterface joint_state_interface_{};
-  franka_hw::FrankaStateInterface franka_state_interface_{};
-  hardware_interface::EffortJointInterface effort_joint_interface_{};
-  franka_hw::FrankaPoseCartesianInterface franka_pose_cartesian_interface_{};
-  franka_hw::FrankaVelocityCartesianInterface franka_velocity_cartesian_interface_{};
-  franka_hw::FrankaModelInterface franka_model_interface_{};
-
-  joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limit_interface_{};
-
-  urdf::Model urdf_model_;
-
-  franka::RobotState robot_state_libfranka_{};
-  franka::RobotState robot_state_ros_{};
-
-  std::mutex libfranka_state_mutex_;
-  std::mutex ros_state_mutex_;
-
-  std::array<std::string, 7> joint_names_;
-  std::string arm_id_;
-  std::string robot_ip_;
-
-  // command data of libfranka
-  std::mutex libfranka_cmd_mutex_;
-  franka::Torques effort_joint_command_libfranka_;
-  franka::CartesianPose pose_cartesian_command_libfranka_;
-  franka::CartesianVelocities velocity_cartesian_command_libfranka_;
-
-  // command data of frankahw ros
-  std::mutex ros_cmd_mutex_;
-  franka::Torques effort_joint_command_ros_;
-  franka::CartesianPose pose_cartesian_command_ros_;
-  franka::CartesianVelocities velocity_cartesian_command_ros_;
-
-  std::function<void(franka::Robot&)> run_function_;
-
-  bool limit_rate_{true};
-  std::atomic_bool controller_active_{false};
-  ControlMode current_control_mode_ = ControlMode::None;
-  double joint_limit_warning_threshold_{10 * 3.14 / 180};
-
-  std::unique_ptr<std::thread> control_loop_thread_;
-
-  ros::Publisher has_error_pub_;
-  ServiceContainer services_;
-  std::unique_ptr<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>
-      recovery_action_server_;
-
-  std::atomic<bool> has_error_;
-  std::atomic<bool> error_recovered_;
-  bool controller_needs_reset_;
-  bool initialized_ = false;
-};
-
-}  // namespace franka_combinable_hw
diff --git a/franka_combinable_hw/mainpage.dox b/franka_combinable_hw/mainpage.dox
deleted file mode 100644
index 941d0bf97714038b58c39c7dcd816e901e22c023..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_combinable_hw/package.xml b/franka_combinable_hw/package.xml
deleted file mode 100644
index 62015d2c745938e8e8708a67008609e6a402211d..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/package.xml
+++ /dev/null
@@ -1,35 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>franka_combinable_hw</name>
-  <version>0.7.0</version>
-  <description>franka_combinable_hw provides hardware interfaces for using multiple Panda robots with ros_control</description>
-  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
-  <license>Apache 2.0</license>
-
-  <url type="website">http://wiki.ros.org/franka_combinable_hw</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 GmbH</author>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <depend>libfranka</depend>
-  <depend>actionlib</depend>
-  <depend>pluginlib</depend>
-  <depend>controller_interface</depend>
-  <depend>franka_hw</depend>
-  <depend>franka_control</depend>
-  <depend>hardware_interface</depend>
-  <depend>joint_limits_interface</depend>
-  <depend>combined_robot_hw</depend>
-  <depend>roscpp</depend>
-  <depend>std_msgs</depend>
-
-  <test_depend>franka_description</test_depend>
-  <test_depend>gtest</test_depend>
-  <test_depend>rostest</test_depend>
-
-  <export>
-    <hardware_interface plugin="${prefix}/franka_combinable_hw_plugin.xml"/>
-  </export>
-</package>
diff --git a/franka_combinable_hw/rosdoc.yaml b/franka_combinable_hw/rosdoc.yaml
deleted file mode 100644
index 96ee597ef5cacd0f223f676c738f396f0810b78c..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
-  javadoc_autobrief: YES
diff --git a/franka_combinable_hw/src/franka_combinable_hw.cpp b/franka_combinable_hw/src/franka_combinable_hw.cpp
deleted file mode 100644
index 7de5008d02448cf58a952b910b83ee4a95a24512..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/src/franka_combinable_hw.cpp
+++ /dev/null
@@ -1,663 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-
-#include <franka/control_types.h>
-#include <franka/exception.h>
-#include <franka/rate_limiting.h>
-#include <franka_combinable_hw/franka_combinable_hw.h>
-#include <franka_control/ErrorRecoveryAction.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
-#include <franka_hw/resource_helpers.h>
-
-#include <pluginlib/class_list_macros.h>
-
-#include <cstdint>
-#include <exception>
-
-#include <hardware_interface/hardware_interface.h>
-#include <joint_limits_interface/joint_limits_urdf.h>
-#include <std_msgs/Bool.h>
-
-using franka_control::ErrorRecoveryResult;
-using franka_hw::ArmClaimedMap;
-using franka_hw::getResourceMap;
-using franka_hw::ResourceWithClaimsMap;
-
-namespace franka_combinable_hw {
-
-FrankaCombinableHW::FrankaCombinableHW()
-    : effort_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      pose_cartesian_command_libfranka_(
-          {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
-      velocity_cartesian_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      effort_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      pose_cartesian_command_ros_(
-          {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
-      velocity_cartesian_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      has_error_(false),
-      error_recovered_(false) {}
-
-/*
- * Data from ROS parameter server:
- *  1. const std::array<std::string, 7>& joint_names
- *  2. const std::string& arm_id
- *
-    : joint_names_(joint_names),
-      arm_id_(arm_id),
-*/
-bool FrankaCombinableHW::initROSInterfaces(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
-  if (initialized_) {
-    ROS_ERROR("FrankaCombinableHW: Cannot be initialized twice.");
-    return false;
-  }
-
-  // initialize arm_id and joint_names
-  if (!robot_hw_nh.getParam("arm_id", arm_id_)) {
-    ROS_ERROR("FrankaCombinableHW: no parameter arm_id is found.");
-    return false;
-  }
-
-  std::vector<std::string> joint_names;
-  if (robot_hw_nh.getParam("joint_names", joint_names) &&
-      joint_names.size() == joint_names_.size()) {
-    std::copy_n(joint_names.begin(), joint_names.size(), joint_names_.begin());
-  } else {
-    ROS_ERROR("FrankaCombinableHW: no valid parameter joint_names is found.");
-    return false;
-  }
-
-  if (!robot_hw_nh.getParam("joint_limit_warning_threshold", arm_id_)) {
-    ROS_INFO(
-        "FrankaCombinableHW: no parameter joint_limit_warning_threshold is found, using default "
-        "value %f",
-        joint_limit_warning_threshold_);
-  }
-
-  for (size_t i = 0; i < joint_names_.size(); ++i) {
-    hardware_interface::JointStateHandle joint_handle_q(joint_names_[i], &robot_state_ros_.q[i],
-                                                        &robot_state_ros_.dq[i],
-                                                        &robot_state_ros_.tau_J[i]);
-
-    joint_state_interface_.registerHandle(joint_handle_q);
-
-    hardware_interface::JointHandle effort_joint_handle(joint_handle_q,
-                                                        &effort_joint_command_ros_.tau_J[i]);
-    effort_joint_interface_.registerHandle(effort_joint_handle);
-  }
-
-  if (root_nh.hasParam("robot_description")) {
-    if (!urdf_model_.initParamWithNodeHandle("robot_description", root_nh)) {
-      ROS_ERROR("FrankaCombinableHW: Could not initialize urdf model from robot_description");
-    } else {
-      joint_limits_interface::SoftJointLimits soft_limits;
-      joint_limits_interface::JointLimits joint_limits;
-
-      for (const auto& joint_name : joint_names_) {
-        int joint_index(std::stoi(joint_name.substr(joint_name.size() - 1)) - 1);
-        auto urdf_joint = urdf_model_.getJoint(joint_name);
-        if (!urdf_joint) {
-          ROS_ERROR_STREAM("FrankaCombinableHW: Could not get joint " << joint_name
-                                                                      << " from urdf");
-        }
-        if (!urdf_joint->safety) {
-          ROS_ERROR_STREAM("FrankaCombinableHW: Joint " << joint_name << " has no safety");
-        }
-        if (!urdf_joint->limits) {
-          ROS_ERROR_STREAM("FrankaCombinableHW: Joint " << joint_name << " has no limits");
-        }
-
-        if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
-          if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
-            joint_limits.max_acceleration = franka::kMaxJointAcceleration[joint_index];
-            joint_limits.has_acceleration_limits = true;
-            joint_limits.max_jerk = franka::kMaxJointJerk[joint_index];
-            joint_limits.has_jerk_limits = true;
-            joint_limits_interface::EffortJointSoftLimitsHandle effort_limit_handle(
-                effort_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
-            effort_joint_limit_interface_.registerHandle(effort_limit_handle);
-          } else {
-            ROS_ERROR_STREAM("FrankaCombinableHW: Could not parse joint limit for joint "
-                             << joint_name << " for joint limit interfaces");
-          }
-        } else {
-          ROS_ERROR_STREAM("FrankaCombinableHW: Could not parse soft joint limit for joint "
-                           << joint_name << " for joint limit interfaces");
-        }
-      }
-    }
-  } else {
-    ROS_WARN("FrankaCombinableHW: No parameter robot_description found to set joint limits!");
-  }
-
-  franka_hw::FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state_ros_);
-  franka_state_interface_.registerHandle(franka_state_handle);
-  franka_hw::FrankaCartesianPoseHandle franka_cartesian_pose_handle(
-      franka_state_handle, pose_cartesian_command_ros_.O_T_EE, pose_cartesian_command_ros_.elbow);
-  franka_pose_cartesian_interface_.registerHandle(franka_cartesian_pose_handle);
-  franka_hw::FrankaCartesianVelocityHandle franka_cartesian_velocity_handle(
-      franka_state_handle, velocity_cartesian_command_ros_.O_dP_EE,
-      velocity_cartesian_command_ros_.elbow);
-  franka_velocity_cartesian_interface_.registerHandle(franka_cartesian_velocity_handle);
-
-  registerInterface(&franka_state_interface_);
-  registerInterface(&joint_state_interface_);
-  registerInterface(&effort_joint_interface_);
-  registerInterface(&franka_pose_cartesian_interface_);
-  registerInterface(&franka_velocity_cartesian_interface_);
-
-  // Setup error state publisher
-  has_error_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("has_error", 1, true);
-  publishErrorState(has_error_);
-
-  // Setup ROS services and action server
-  setupServicesAndActionServers(robot_hw_nh);
-
-  initialized_ = true;
-  return true;
-}
-
-/*
- * Data from ROS parameter server:
- *  1. const std::array<std::string, 7>& joint_names
- *  2. const std::string& arm_id
- *
-    : joint_names_(joint_names),
-      arm_id_(arm_id),
-*/
-bool FrankaCombinableHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
-  // Initialize ROS-interfaces
-  if (not initROSInterfaces(root_nh, robot_hw_nh)) {
-    ROS_ERROR("FrankaCombinableHW: Failed to initialize interfaces.");
-    return false;
-  }
-
-  // Initialize and connect to franka::Robot, register model interface and start control loop
-  try {
-    if (!initRobot(robot_hw_nh)) {
-      ROS_ERROR("FrankaCombinableHW: Failed to initialize libfranka robot.");
-      return false;
-    }
-  } catch (const std::runtime_error& error) {
-    ROS_ERROR("FrankaCombinableHW: Failed to initialize libfranka robot. %s", error.what());
-    return false;
-  }
-
-  return true;
-}
-
-bool FrankaCombinableHW::initRobot(ros::NodeHandle& robot_hw_nh) {
-  if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) {
-    ROS_ERROR("FrankaCombinableHW: no parameter robot_ip is found.");
-    return false;
-  }
-
-  robot_ = std::make_unique<franka::Robot>(robot_ip_);
-
-  // Set default collision behaviour
-  robot_->setCollisionBehavior(
-      {{40.0, 40.0, 38.0, 38.0, 32.0, 30.0, 24.0}}, {{40.0, 40.0, 38.0, 36.0, 32.0, 30.0, 24.0}},
-      {{40.0, 40.0, 38.0, 38.0, 32.0, 30.0, 24.0}}, {{40.0, 40.0, 38.0, 36.0, 32.0, 30.0, 24.0}},
-      {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}},
-      {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}});
-
-  // Initialize robot state before loading any controller
-  update(robot_->readOnce());
-
-  model_ = std::make_unique<franka::Model>(robot_->loadModel());
-  franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", *model_, robot_state_ros_);
-  franka_model_interface_.registerHandle(model_handle);
-  registerInterface(&franka_model_interface_);
-  control_loop_thread_ = std::make_unique<std::thread>(&FrankaCombinableHW::controlLoop, this);
-  return true;
-}
-
-void FrankaCombinableHW::publishErrorState(bool error) {
-  std_msgs::Bool msg;
-  msg.data = error;  // NOLINT (readability-implicit-bool-cast)
-  has_error_pub_.publish(msg);
-}
-
-void FrankaCombinableHW::checkJointLimits() {
-  // Check joint limits and print warning if any joint is close to limit
-  std::string joint_limits_warning;
-  for (const auto& k_joint_name : joint_names_) {
-    try {
-      auto joint_handle = joint_state_interface_.getHandle(k_joint_name);
-      auto urdf_joint = urdf_model_.getJoint(k_joint_name);
-      joint_limits_interface::JointLimits joint_limits;
-      if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
-        double joint_lower = joint_limits.min_position;
-        double joint_upper = joint_limits.max_position;
-        double joint_position = joint_handle.getPosition();
-        double dist = fmin(fabs(joint_position - joint_lower), fabs(joint_position - joint_upper));
-        if (dist < joint_limit_warning_threshold_) {
-          joint_limits_warning += "\n\t" + k_joint_name + ": " + std::to_string(dist * 180 / 3.14) +
-                                  " degrees to joint limits (limits: [" +
-                                  std::to_string(joint_lower) + ", " + std::to_string(joint_upper) +
-                                  "]" + " q: " + std::to_string(joint_position) + ") ";
-        }
-      } else {
-        ROS_ERROR_STREAM_ONCE("FrankaCombinableHW: Could not parse joint limit for joint "
-                              << k_joint_name << " for joint limit interfaces");
-      }
-    } catch (const hardware_interface::HardwareInterfaceException& ex) {
-      ROS_ERROR_STREAM_ONCE("FrankaCombinableHW: Could not get joint handle " << k_joint_name
-                                                                              << " .\n"
-                                                                              << ex.what());
-      return;
-    }
-  }
-
-  if (!joint_limits_warning.empty()) {
-    ROS_WARN_THROTTLE(5, "FrankaCombinableHW: %s", joint_limits_warning.c_str());
-  }
-}
-
-void FrankaCombinableHW::update(const franka::RobotState& robot_state) {
-  ros_state_mutex_.lock();
-  robot_state_ros_ = robot_state;
-  ros_state_mutex_.unlock();
-}
-
-bool FrankaCombinableHW::controllerActive() const noexcept {
-  return controller_active_;
-}
-
-void FrankaCombinableHW::controlLoop() {
-  while (ros::ok()) {
-    ros::Time last_time = ros::Time::now();
-
-    // Wait until controller has been activated or error has been recovered
-    while (!controllerActive() || has_error_) {
-      if (!controllerActive()) {
-        ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): controller is not active.",
-                           arm_id_.c_str());
-      }
-      if (has_error_) {
-        ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): an error has occured.",
-                           arm_id_.c_str());
-      }
-
-      checkJointLimits();
-
-      ros_state_mutex_.lock();
-      libfranka_state_mutex_.lock();
-      robot_state_libfranka_ = robot_->readOnce();
-      robot_state_ros_ = robot_->readOnce();
-      libfranka_state_mutex_.unlock();
-      ros_state_mutex_.unlock();
-      if (!ros::ok()) {
-        return;
-      }
-    }
-
-    ROS_INFO("FrankaCombinableHW::%s::control_loop(): controller is active.", arm_id_.c_str());
-
-    // Reset commands
-    libfranka_cmd_mutex_.lock();
-    pose_cartesian_command_libfranka_ = franka::CartesianPose(robot_state_libfranka_.O_T_EE_d);
-    effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.});
-    velocity_cartesian_command_libfranka_ = franka::CartesianVelocities({0., 0., 0., 0., 0., 0.});
-    libfranka_cmd_mutex_.unlock();
-
-    try {
-      // Run control loop. It will exit if the controller is switched.
-      control(*robot_);
-    } catch (const franka::ControlException& e) {
-      // Reflex could be caught and it needs to wait for automatic error recovery
-      ROS_ERROR("%s", e.what());
-      has_error_ = true;
-      publishErrorState(has_error_);
-    }
-  }
-}
-
-void FrankaCombinableHW::setupServicesAndActionServers(ros::NodeHandle& node_handle) {
-  services_
-      .advertiseService<franka_control::SetJointImpedance>(
-          node_handle, "set_joint_impedance",
-          [this](auto&& req, auto&& res) {
-            return franka_control::setJointImpedance(*(this->robot_), req, res);
-          })
-      .advertiseService<franka_control::SetCartesianImpedance>(
-          node_handle, "set_cartesian_impedance",
-          [this](auto&& req, auto&& res) {
-            return franka_control::setCartesianImpedance(*(this->robot_), req, res);
-          })
-      .advertiseService<franka_control::SetEEFrame>(node_handle, "set_EE_frame",
-                                                    [this](auto&& req, auto&& res) {
-                                                      return franka_control::setEEFrame(
-                                                          *(this->robot_), req, res);
-                                                    })
-      .advertiseService<franka_control::SetKFrame>(node_handle, "set_K_frame",
-                                                   [this](auto&& req, auto&& res) {
-                                                     return franka_control::setKFrame(
-                                                         *(this->robot_), req, res);
-                                                   })
-      .advertiseService<franka_control::SetForceTorqueCollisionBehavior>(
-          node_handle, "set_force_torque_collision_behavior",
-          [this](auto&& req, auto&& res) {
-            return franka_control::setForceTorqueCollisionBehavior(*(this->robot_), req, res);
-          })
-      .advertiseService<franka_control::SetFullCollisionBehavior>(
-          node_handle, "set_full_collision_behavior",
-          [this](auto&& req, auto&& res) {
-            return franka_control::setFullCollisionBehavior(*(this->robot_), req, res);
-          })
-      .advertiseService<franka_control::SetLoad>(
-          node_handle, "set_load", [this](auto&& req, auto&& res) {
-            return franka_control::setLoad(*(this->robot_), req, res);
-          });
-
-  recovery_action_server_ =
-      std::make_unique<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>(
-          node_handle, "error_recovery",
-          [&](const franka_control::ErrorRecoveryGoalConstPtr&) {
-            try {
-              robot_->automaticErrorRecovery();
-              // error recovered => reset controller
-              if (has_error_) {
-                error_recovered_ = true;
-              }
-              has_error_ = false;
-              publishErrorState(has_error_);
-              recovery_action_server_->setSucceeded();
-            } catch (const franka::Exception& ex) {
-              recovery_action_server_->setAborted(ErrorRecoveryResult(), ex.what());
-            }
-          },
-          false);
-  recovery_action_server_->start();
-}
-
-void FrankaCombinableHW::control(franka::Robot& robot) {
-  if (!controller_active_) {
-    return;
-  }
-  run_function_(robot);
-}
-
-void FrankaCombinableHW::enforceLimits(const ros::Duration& period) {
-  if (period.toSec() > 0.0) {
-    effort_joint_limit_interface_.enforceLimits(period);
-  }
-}
-
-bool FrankaCombinableHW::checkForConflict(
-    const std::list<hardware_interface::ControllerInfo>& info) const {
-  ResourceWithClaimsMap resource_map = getResourceMap(info);
-  // check for conflicts in single resources: no triple claims,
-  // for 2 claims it must be one torque and one non-torque claim
-  for (auto map_it = resource_map.begin(); map_it != resource_map.end(); map_it++) {
-    if (map_it->second.size() > 2) {
-      ROS_ERROR_STREAM("FrankaCombinableHW: Resource "
-                       << map_it->first << " claimed with more than two interfaces. Conflict!");
-      return true;
-    }
-    uint8_t torque_claims = 0;
-    uint8_t other_claims = 0;
-    if (map_it->second.size() == 2) {
-      for (auto& claimed_by : map_it->second) {
-        if (claimed_by[2] == "hardware_interface::EffortJointInterface") {
-          torque_claims++;
-        } else {
-          other_claims++;
-        }
-      }
-      if (torque_claims == 0) {
-        ROS_ERROR_STREAM("FrankaCombinableHW: Resource "
-                         << map_it->first
-                         << " is claimed with two non-compatible interfaces. Conflict!");
-        return true;
-      }
-    }
-  }
-
-  ArmClaimedMap arm_claim_map;
-  if (!getArmClaimedMap(resource_map, arm_claim_map)) {
-    ROS_ERROR("FrankaCombinableHW: Unknown interface claimed. Conflict!");
-    return true;
-  }
-
-  // check for any claim to joint_position or joint velocity interface which are not supported
-  if (arm_claim_map.find(arm_id_) != arm_claim_map.end()) {
-    if (arm_claim_map[arm_id_].joint_position_claims +
-            arm_claim_map[arm_id_].joint_velocity_claims >
-        0) {
-      ROS_ERROR_STREAM("FrankaCombinableHW: Invalid claim joint position or velocity interface."
-                       << "Note: joint position and joint velocity interfaces are not supported"
-                       << " in FrankaCombinableHW. Arm:" << arm_id_ << ". Conflict!");
-      return true;
-    }
-  }
-
-  // check for any claim to cartesian interfaces without torque interface
-  if (arm_claim_map.find(arm_id_) != arm_claim_map.end()) {
-    if (arm_claim_map[arm_id_].cartesian_velocity_claims +
-                arm_claim_map[arm_id_].cartesian_pose_claims >
-            0 &&
-        arm_claim_map[arm_id_].joint_torque_claims == 0) {
-      ROS_ERROR_STREAM(
-          "FrankaCombinableHW: Invalid claim cartesian interface without joint torque"
-          " interface. Arm:"
-          << arm_id_ << ". Conflict!");
-      return true;
-    }
-  }
-
-  // check for conflicts between joint and cartesian level for each arm.
-  // Valid claims are torque claims on joint level in combination with either
-  // 7 non-torque claims on joint_level or one claim on cartesian level.
-  if (arm_claim_map.find(arm_id_) != arm_claim_map.end()) {
-    if ((arm_claim_map[arm_id_].cartesian_velocity_claims +
-                 arm_claim_map[arm_id_].cartesian_pose_claims >
-             0 &&
-         arm_claim_map[arm_id_].joint_position_claims +
-                 arm_claim_map[arm_id_].joint_velocity_claims >
-             0)) {
-      ROS_ERROR_STREAM("FrankaCombinableHW: Invalid claims on joint AND cartesian level on arm "
-                       << arm_id_ << ". Conflict!");
-      return true;
-    }
-    if ((arm_claim_map[arm_id_].joint_position_claims > 0 &&
-         arm_claim_map[arm_id_].joint_position_claims != 7) ||
-        (arm_claim_map[arm_id_].joint_velocity_claims > 0 &&
-         arm_claim_map[arm_id_].joint_velocity_claims != 7) ||
-        (arm_claim_map[arm_id_].joint_torque_claims > 0 &&
-         arm_claim_map[arm_id_].joint_torque_claims != 7)) {
-      ROS_ERROR_STREAM("FrankaCombinableHW: Non-consistent claims on the joints of "
-                       << arm_id_ << ". Not supported. Conflict!");
-      return true;
-    }
-  }
-  return false;
-}
-
-// doSwitch runs on the main realtime thread
-void FrankaCombinableHW::doSwitch(
-    const std::list<hardware_interface::ControllerInfo>& /* start_list */,
-    const std::list<hardware_interface::ControllerInfo>& /* stop_list */) {
-  if (current_control_mode_ != ControlMode::None) {
-    controller_active_ = true;
-  }
-}
-
-// prepareSwitch runs on the background message handling thread
-bool FrankaCombinableHW::prepareSwitch(
-    const std::list<hardware_interface::ControllerInfo>& start_list,
-    const std::list<hardware_interface::ControllerInfo>& stop_list) {
-  ResourceWithClaimsMap start_resource_map = getResourceMap(start_list);
-  ArmClaimedMap start_arm_claim_map;
-  if (!getArmClaimedMap(start_resource_map, start_arm_claim_map)) {
-    ROS_ERROR("FrankaCombinableHW: Unknown interface claimed for starting!");
-    return false;
-  }
-  ControlMode start_control_mode = getControlMode(arm_id_, start_arm_claim_map);
-
-  ResourceWithClaimsMap stop_resource_map = getResourceMap(stop_list);
-  ArmClaimedMap stop_arm_claim_map;
-  if (!getArmClaimedMap(stop_resource_map, stop_arm_claim_map)) {
-    ROS_ERROR("FrankaCombinableHW: Unknown interface claimed for stopping!");
-    return false;
-  }
-  ControlMode stop_control_mode = getControlMode(arm_id_, stop_arm_claim_map);
-
-  ControlMode requested_control_mode = current_control_mode_;
-  requested_control_mode |= start_control_mode;
-  requested_control_mode &= ~stop_control_mode;
-
-  using std::placeholders::_1;
-  using std::placeholders::_2;
-
-  switch (requested_control_mode) {
-    case ControlMode::None:
-      break;
-    case ControlMode::JointTorque:
-      run_function_ = [this](franka::Robot& robot) {
-        robot.control(std::bind(&FrankaCombinableHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_libfranka_), _1, _2),
-                      limit_rate_);
-      };
-      break;
-    case ControlMode::JointPosition:
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-    case ControlMode::JointVelocity:
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-    case ControlMode::CartesianPose:
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-    case ControlMode::CartesianVelocity:
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-    case (ControlMode::JointTorque | ControlMode::JointPosition):
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-    case (ControlMode::JointTorque | ControlMode::JointVelocity):
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-    case (ControlMode::JointTorque | ControlMode::CartesianPose):
-      run_function_ = [this](franka::Robot& robot) {
-        robot.control(std::bind(&FrankaCombinableHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_libfranka_), _1, _2),
-                      std::bind(&FrankaCombinableHW::controlCallback<franka::CartesianPose>, this,
-                                std::cref(pose_cartesian_command_libfranka_), _1, _2),
-                      limit_rate_);
-      };
-      break;
-    case (ControlMode::JointTorque | ControlMode::CartesianVelocity):
-      run_function_ = [this](franka::Robot& robot) {
-        robot.control(std::bind(&FrankaCombinableHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_libfranka_), _1, _2),
-                      std::bind(&FrankaCombinableHW::controlCallback<franka::CartesianVelocities>,
-                                this, std::cref(velocity_cartesian_command_libfranka_), _1, _2),
-                      limit_rate_);
-      };
-      break;
-    default:
-      ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers.");
-      return false;
-  }
-
-  if (current_control_mode_ != requested_control_mode) {
-    ROS_INFO_STREAM("FrankaCombinableHW: Prepared switching controllers to "
-                    << requested_control_mode);
-    current_control_mode_ = requested_control_mode;
-
-    controller_active_ = false;
-  }
-
-  return true;
-}
-
-void FrankaCombinableHW::read(const ros::Time&,        // NOLINT (readability-identifier-naming)
-                              const ros::Duration&) {  // NOLINT [readability-named-parameter]
-  controller_needs_reset_ = error_recovered_;
-  ros_state_mutex_.lock();
-  libfranka_state_mutex_.lock();
-  robot_state_ros_ = robot_state_libfranka_;
-  libfranka_state_mutex_.unlock();
-  ros_state_mutex_.unlock();
-}
-
-void FrankaCombinableHW::write(const ros::Time&,  // NOLINT (readability-identifier-naming)
-                               const ros::Duration& period) {
-  // if flag `controller_needs_reset_` was updated, then controller_manager. update(...,
-  // reset_controller) must
-  // have been executed to reset the controller.
-  if (controller_needs_reset_ && error_recovered_) {
-    controller_needs_reset_ = false;
-    error_recovered_ = false;
-  }
-  enforceLimits(period);
-  ros_cmd_mutex_.lock();
-  libfranka_cmd_mutex_.lock();
-  effort_joint_command_libfranka_.tau_J =
-      saturateTorqueRate(effort_joint_command_ros_.tau_J, robot_state_libfranka_.tau_J_d);
-  pose_cartesian_command_libfranka_ = pose_cartesian_command_ros_;
-  velocity_cartesian_command_libfranka_ = velocity_cartesian_command_ros_;
-  libfranka_cmd_mutex_.unlock();
-  ros_cmd_mutex_.unlock();
-}
-
-std::array<double, 7> FrankaCombinableHW::saturateTorqueRate(
-    const std::array<double, 7>& tau_d_calculated,
-    const std::array<double, 7>& tau_J_d) {  // NOLINT (readability-identifier-naming)
-  const double kDeltaTauMax = 1.0;
-  std::array<double, 7> tau_d_saturated{};
-  for (size_t i = 0; i < 7; i++) {
-    double difference = tau_d_calculated[i] - tau_J_d[i];
-    tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
-  }
-  return tau_d_saturated;
-}
-
-std::array<double, 7> FrankaCombinableHW::getJointEffortCommand() const noexcept {
-  return effort_joint_command_libfranka_.tau_J;
-}
-
-std::string FrankaCombinableHW::getArmID() {
-  return arm_id_;
-}
-
-void FrankaCombinableHW::triggerError() {
-  has_error_ = true;
-  publishErrorState(has_error_);
-}
-
-bool FrankaCombinableHW::hasError() {
-  return has_error_;
-}
-
-void FrankaCombinableHW::resetError() {
-  robot_->automaticErrorRecovery();
-  // error recovered => reset controller
-  if (has_error_) {
-    error_recovered_ = true;
-  }
-  has_error_ = false;
-  publishErrorState(has_error_);
-}
-
-bool FrankaCombinableHW::controllerNeedsReset() {
-  return controller_needs_reset_;
-}
-
-bool FrankaCombinableHW::commandHasNaN(const franka::Torques& command) {
-  return arrayHasNaN(command.tau_J);
-}
-
-bool FrankaCombinableHW::commandHasNaN(const franka::CartesianPose& command) {
-  return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_T_EE);
-}
-
-bool FrankaCombinableHW::commandHasNaN(const franka::CartesianVelocities& command) {
-  return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_dP_EE);
-}
-
-}  // namespace franka_combinable_hw
-
-PLUGINLIB_EXPORT_CLASS(franka_combinable_hw::FrankaCombinableHW, hardware_interface::RobotHW)
diff --git a/franka_combinable_hw/test/CMakeLists.txt b/franka_combinable_hw/test/CMakeLists.txt
deleted file mode 100644
index ffd3a6afb628895dc26e950485261a9eef396686..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/test/CMakeLists.txt
+++ /dev/null
@@ -1,23 +0,0 @@
-find_package(rostest REQUIRED)
-
-add_rostest_gtest(franka_combinable_hw_test
-  launch/franka_combinable_hw_test.test
-  main.cpp
-  franka_combinable_hw_controller_switching_test.cpp
-)
-
-add_dependencies(franka_combinable_hw_test
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_combinable_hw_test
-  ${catkin_LIBRARIES}
-  Franka::Franka
-  franka_combinable_hw
-)
-
-target_include_directories(franka_combinable_hw_test PUBLIC
-  ${catkin_INCLUDE_DIRS}
-)
-
diff --git a/franka_combinable_hw/test/config/franka_combinable_hw_test_node.yaml b/franka_combinable_hw/test/config/franka_combinable_hw_test_node.yaml
deleted file mode 100644
index 7ce0b485fb47754662062033845799206a4d81b6..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/test/config/franka_combinable_hw_test_node.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-robot_hardware:
-  - panda
-panda:
-  type: franka_combinable_hw/FrankaCombinableHW
-  arm_id: panda
-  joint_names:
-    - panda_joint1
-    - panda_joint2
-    - panda_joint3
-    - panda_joint4
-    - panda_joint5
-    - panda_joint6
-    - panda_joint7
diff --git a/franka_combinable_hw/test/config/ros_console_settings_for_tests.conf b/franka_combinable_hw/test/config/ros_console_settings_for_tests.conf
deleted file mode 100644
index 82c355359b3090d6bf8c1b62e9c73dc6705d0189..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/test/config/ros_console_settings_for_tests.conf
+++ /dev/null
@@ -1,4 +0,0 @@
-# Set the default ros output to warning and higher
-log4j.logger.ros=WARN
-# Override my package to output everything
-log4j.logger.ros.franka_hw=FATAL
diff --git a/franka_combinable_hw/test/franka_combinable_hw_controller_switching_test.cpp b/franka_combinable_hw/test/franka_combinable_hw_controller_switching_test.cpp
deleted file mode 100644
index d6bc475c190529078512f9eca27f8706e32c51ef..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/test/franka_combinable_hw_controller_switching_test.cpp
+++ /dev/null
@@ -1,208 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <array>
-#include <list>
-#include <random>
-#include <set>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#include <hardware_interface/controller_info.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <joint_limits_interface/joint_limits.h>
-#include <joint_limits_interface/joint_limits_urdf.h>
-#include <ros/ros.h>
-#include <urdf/model.h>
-
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_combinable_hw/franka_combinable_hw.h>
-
-using std::string;
-using std::set;
-using hardware_interface::InterfaceResources;
-using hardware_interface::ControllerInfo;
-
-std::string arm_id("panda");
-
-std::array<std::string, 7> joint_names  = {arm_id + "_joint1",
-        arm_id + "_joint2",
-        arm_id + "_joint3",
-        arm_id + "_joint4",
-        arm_id + "_joint5",
-        arm_id + "_joint6",
-        arm_id + "_joint7"};
-
-namespace franka_combinable_hw {
-
-hardware_interface::ControllerInfo newInfo(
-    const std::string& name,
-    const std::string& type,
-    const hardware_interface::InterfaceResources& resource1) {
-  hardware_interface::ControllerInfo info;
-  info.claimed_resources.clear();
-  info.name = name;
-  info.type = type;
-  info.claimed_resources.push_back(resource1);
-  return info;
-}
-
-hardware_interface::ControllerInfo newInfo(
-    const std::string& name,
-    const std::string& type,
-    const hardware_interface::InterfaceResources& resource1,
-    const hardware_interface::InterfaceResources& resource2) {
-  hardware_interface::ControllerInfo info = newInfo(name, type, resource1);
-  info.claimed_resources.push_back(resource2);
-  return info;
-}
-
-hardware_interface::ControllerInfo newInfo(
-    const std::string& name,
-    const std::string& type,
-    const hardware_interface::InterfaceResources& resource1,
-    const hardware_interface::InterfaceResources& resource2,
-    const hardware_interface::InterfaceResources& resource3) {
-  hardware_interface::ControllerInfo info = newInfo(name, type, resource1, resource2);
-  info.claimed_resources.push_back(resource3);
-  return info;
-}
-
-class ControllerConflict :
-        public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > {
-public:
- ControllerConflict() : robot_(new FrankaCombinableHW()) {
-     ros::NodeHandle root_nh;
-     ros::NodeHandle robot_hw_nh("~/" + arm_id);
-     robot_->initROSInterfaces(root_nh, robot_hw_nh);
- }
- bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->checkForConflict(info_list);
- }
- bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->prepareSwitch(info_list, info_list);
- }
- private:
- std::unique_ptr<FrankaCombinableHW>  robot_;
-};
-
-class NoControllerConflict : public
-        ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > {
-public:
- NoControllerConflict() : robot_(new FrankaCombinableHW()) {
-     ros::NodeHandle root_nh;
-     ros::NodeHandle robot_hw_nh("~/" + arm_id);
-     robot_->initROSInterfaces(root_nh, robot_hw_nh);
- }
- bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->checkForConflict(info_list);
- }
- bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->prepareSwitch(info_list, info_list);
- }
- private:
- std::unique_ptr<FrankaCombinableHW> robot_;
-};
-
-string arm_id2("panda2");
-string jp_iface_str("hardware_interface::PositionJointInterface");
-string jv_iface_str("hardware_interface::VelocityJointInterface");
-string jt_iface_str("hardware_interface::EffortJointInterface");
-string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface");
-string cp_iface_str("franka_hw::FrankaPoseCartesianInterface");
-string unknown_iface_str("hardware_interface::UnknownInterface");
-string name_str("some_controller");
-string type_str("SomeControllerClass");
-set<string> joints_set = {joint_names[0],
-                          joint_names[1],
-                          joint_names[2],
-                          joint_names[3],
-                          joint_names[4],
-                          joint_names[5],
-                          joint_names[6]};
-set<string> cartesian_set = {arm_id + "_robot"};
-set<string> cartesian_arm2_set = {arm_id2 + "_robot"};
-set<string> no_id_set = {"joint1"};
-InterfaceResources no_id_res(jp_iface_str, no_id_set);
-InterfaceResources unknown_iface_res(unknown_iface_str, joints_set);
-InterfaceResources jp_res(jp_iface_str, joints_set);
-InterfaceResources jv_res(jv_iface_str, joints_set);
-InterfaceResources jt_res(jt_iface_str, joints_set);
-InterfaceResources cv_res(cv_iface_str, cartesian_set);
-InterfaceResources cp_res(cp_iface_str, cartesian_set);
-InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set);
-ControllerInfo cp_arm2_info = newInfo(name_str, type_str, cp_arm2_res);
-ControllerInfo no_id_info = newInfo(name_str, type_str, no_id_res);
-ControllerInfo unknown_iface_info = newInfo(name_str, type_str, unknown_iface_res);
-ControllerInfo jt_jt_info = newInfo(name_str, type_str, jt_res, jt_res);
-ControllerInfo jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res);
-ControllerInfo jp_jv_info = newInfo(name_str, type_str, jp_res, jv_res);
-ControllerInfo jp_cv_info = newInfo(name_str, type_str, jp_res, cv_res);
-ControllerInfo jp_cp_info = newInfo(name_str, type_str, jp_res, cp_res);
-ControllerInfo jv_jv_info = newInfo(name_str, type_str, jv_res, jv_res);
-ControllerInfo jv_cv_info = newInfo(name_str, type_str, jv_res, cv_res);
-ControllerInfo jv_cp_info = newInfo(name_str, type_str, jv_res, cp_res);
-ControllerInfo cv_cv_info = newInfo(name_str, type_str, cv_res, cv_res);
-ControllerInfo cv_cp_info = newInfo(name_str, type_str, cv_res, cp_res);
-ControllerInfo cp_cp_info = newInfo(name_str, type_str, cp_res, cp_res);
-ControllerInfo jp_jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res, jp_res);
-ControllerInfo jp_info = newInfo(name_str, type_str, jp_res);
-ControllerInfo jv_info = newInfo(name_str, type_str, jv_res);
-ControllerInfo jt_info = newInfo(name_str, type_str, jt_res);
-ControllerInfo cv_info = newInfo(name_str, type_str, cv_res);
-ControllerInfo cp_info = newInfo(name_str, type_str, cp_res);
-ControllerInfo jt_jp_info = newInfo(name_str, type_str, jt_res, jp_res);
-ControllerInfo jt_jv_info = newInfo(name_str, type_str, jt_res, jv_res);
-ControllerInfo jt_cv_info = newInfo(name_str, type_str, jt_res, cv_res);
-ControllerInfo jt_cp_info = newInfo(name_str, type_str, jt_res, cp_res);
-
-INSTANTIATE_TEST_CASE_P(nonAdmissibleRequests,
-                        ControllerConflict,
-                        ::testing::Values(std::list<ControllerInfo>{no_id_info},
-                                          std::list<ControllerInfo>{unknown_iface_info},
-                                          std::list<ControllerInfo>{jp_info},
-                                          std::list<ControllerInfo>{jv_info},
-                                          std::list<ControllerInfo>{cv_info},
-                                          std::list<ControllerInfo>{cp_info},
-                                          std::list<ControllerInfo>{jt_jp_info},
-                                          std::list<ControllerInfo>{jt_jv_info},
-                                          std::list<ControllerInfo>{jt_jt_info},
-                                          std::list<ControllerInfo>{jp_jp_info},
-                                          std::list<ControllerInfo>{jp_jv_info},
-                                          std::list<ControllerInfo>{jp_cv_info},
-                                          std::list<ControllerInfo>{jp_cp_info},
-                                          std::list<ControllerInfo>{jv_jv_info},
-                                          std::list<ControllerInfo>{jv_cv_info},
-                                          std::list<ControllerInfo>{jv_cp_info},
-                                          std::list<ControllerInfo>{cv_cv_info},
-                                          std::list<ControllerInfo>{cv_cp_info},
-                                          std::list<ControllerInfo>{cp_cp_info},
-                                          std::list<ControllerInfo>{jp_jp_jp_info}));
-
-INSTANTIATE_TEST_CASE_P(admissibleRequests,
-                        NoControllerConflict,
-                        ::testing::Values(std::list<ControllerInfo>{jt_info},
-                                          std::list<ControllerInfo>{jt_cv_info},
-                                          std::list<ControllerInfo>{jt_cp_info},
-                                          std::list<ControllerInfo>{jt_info, cp_arm2_info}));
-
-TEST(FrankaCombinableHWTests, CanInitROSInterfaces) {
-  FrankaCombinableHW hw;
-  ros::NodeHandle root_nh;
-  ros::NodeHandle robot_hw_nh("~/" + arm_id);
-  EXPECT_TRUE(hw.initROSInterfaces(root_nh, robot_hw_nh));
-}
-
-TEST_P(ControllerConflict, ConflictsForIncompatibleControllers) {
-  EXPECT_TRUE(callCheckForConflict(GetParam()));
-}
-
-TEST_P(NoControllerConflict, DoesNotConflictForCompatibleControllers) {
-  EXPECT_FALSE(callCheckForConflict(GetParam()));
-}
-
-TEST_P(NoControllerConflict, CanPrepareSwitchForCompatibleControllers) {
-    EXPECT_TRUE(callPrepareSwitch(GetParam()));
-}
-
-}  // namespace franka_combinable_hw
diff --git a/franka_combinable_hw/test/launch/franka_combinable_hw_test.test b/franka_combinable_hw/test/launch/franka_combinable_hw_test.test
deleted file mode 100644
index 4e6553b2f3b61780833e2e2860a33c3c8e306ffa..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/test/launch/franka_combinable_hw_test.test
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
- <env name="ROSCONSOLE_CONFIG_FILE" value="$(find franka_combinable_hw)/test/config/ros_console_settings_for_tests.conf"/>
- <test test-name="franka_combinable_hw_test" pkg="franka_combinable_hw" type="franka_combinable_hw_test">
-   <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" />
-   <rosparam command="load" file="$(find franka_combinable_hw)/test/config/franka_combinable_hw_test_node.yaml" />
- </test>
-</launch>
diff --git a/franka_combinable_hw/test/main.cpp b/franka_combinable_hw/test/main.cpp
deleted file mode 100644
index 8e558b2411e69a76bb72d46f57db0a476b4dcdca..0000000000000000000000000000000000000000
--- a/franka_combinable_hw/test/main.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <gtest/gtest.h>
-#include <ros/ros.h>
-
-int main(int argc, char** argv) {
-  ::testing::InitGoogleTest(&argc, argv);
-  ros::init(argc, argv, "franka_hw_test_node");
-  return RUN_ALL_TESTS();
-}
diff --git a/franka_combined_control/CMakeLists.txt b/franka_combined_control/CMakeLists.txt
deleted file mode 100644
index d300733c783ed7b28b8329b78df17043fe09a662..0000000000000000000000000000000000000000
--- a/franka_combined_control/CMakeLists.txt
+++ /dev/null
@@ -1,63 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_combined_control)
-
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_BUILD_TYPE Release)
-
-find_package(catkin REQUIRED COMPONENTS
-  controller_manager
-  franka_combinable_hw
-  roscpp
-)
-
-find_package(Franka 0.5.0 REQUIRED)
-
-catkin_package(
-  CATKIN_DEPENDS
-    controller_manager
-    franka_combinable_hw
-    roscpp
-)
-
-add_executable(franka_combined_control_node
-    src/franka_combined_control_node.cpp
-)
-
-add_dependencies(franka_combined_control_node
-    ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_combined_control_node
-  ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_combined_control_node SYSTEM PUBLIC
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Installation
-install(TARGETS franka_combined_control_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
-  RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_TOOLS)
-  file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/franka_combined_control_node.cpp)
-  add_format_target(franka_combined_control FILES ${SOURCES})
-  add_tidy_target(franka_combined_control
-    FILES ${SOURCES}
-    DEPENDS franka_combined_control_node
-  )
-endif()
diff --git a/franka_combined_control/config/default_combined_controllers.yaml b/franka_combined_control/config/default_combined_controllers.yaml
deleted file mode 100644
index 9d5d77fe1afa5e12ff2889b29358d3c86ec6c95f..0000000000000000000000000000000000000000
--- a/franka_combined_control/config/default_combined_controllers.yaml
+++ /dev/null
@@ -1,88 +0,0 @@
-effort_joint_trajectory_controller:
-  type: effort_controllers/JointTrajectoryController
-  joints:
-    - panda_left_joint1
-    - panda_left_joint2
-    - panda_left_joint3
-    - panda_left_joint4
-    - panda_left_joint5
-    - panda_left_joint6
-    - panda_left_joint7
-    - panda_right_joint1
-    - panda_right_joint2
-    - panda_right_joint3
-    - panda_right_joint4
-    - panda_right_joint5
-    - panda_right_joint6
-    - panda_right_joint7
-  constraints:
-    goal_time: 0.5
-    panda_left_joint1:
-      goal: 0.05
-    panda_left_joint2:
-      goal: 0.05
-    panda_left_joint3:
-      goal: 0.05
-    panda_left_joint4:
-      goal: 0.05
-    panda_left_joint5:
-      goal: 0.05
-    panda_left_joint6:
-      goal: 0.05
-    panda_left_joint7:
-      goal: 0.05
-    panda_right_joint1:
-      goal: 0.05
-    panda_right_joint2:
-      goal: 0.05
-    panda_right_joint3:
-      goal: 0.05
-    panda_right_joint4:
-      goal: 0.05
-    panda_right_joint5:
-      goal: 0.05
-    panda_right_joint6:
-      goal: 0.05
-    panda_right_joint7:
-      goal: 0.05
-  gains:
-    panda_left_joint1: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_left_joint2: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_left_joint3: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_left_joint4: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_left_joint5: {p: 250, d: 10, i: 0, i_clamp: 1}
-    panda_left_joint6: {p: 150, d: 10, i: 0, i_clamp: 1}
-    panda_left_joint7: {p: 50, d: 5, i: 0, i_clamp: 1}
-    panda_right_joint1: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_right_joint2: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_right_joint3: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_right_joint4: {p: 600, d: 30, i: 0, i_clamp: 1}
-    panda_right_joint5: {p: 250, d: 10, i: 0, i_clamp: 1}
-    panda_right_joint6: {p: 150, d: 10, i: 0, i_clamp: 1}
-    panda_right_joint7: {p: 50, d: 5, i: 0, i_clamp: 1}
-
-panda_left_state_controller:
-  type: franka_control/FrankaStateController
-  arm_id: panda_left
-  joint_names:
-    - panda_left_joint1
-    - panda_left_joint2
-    - panda_left_joint3
-    - panda_left_joint4
-    - panda_left_joint5
-    - panda_left_joint6
-    - panda_left_joint7
-  publish_rate: 30  # [Hz]
-
-panda_right_state_controller:
-  type: franka_control/FrankaStateController
-  arm_id: panda_right
-  joint_names:
-    - panda_right_joint1
-    - panda_right_joint2
-    - panda_right_joint3
-    - panda_right_joint4
-    - panda_right_joint5
-    - panda_right_joint6
-    - panda_right_joint7
-  publish_rate: 30  # [Hz]
diff --git a/franka_combined_control/config/franka_combined_control_node.yaml b/franka_combined_control/config/franka_combined_control_node.yaml
deleted file mode 100644
index 71b012ea4c40c0eb77e91192c3e29f76703afc69..0000000000000000000000000000000000000000
--- a/franka_combined_control/config/franka_combined_control_node.yaml
+++ /dev/null
@@ -1,25 +0,0 @@
-robot_hardware:
-  - panda_left
-  - panda_right
-panda_left:
-  type: franka_combinable_hw/FrankaCombinableHW
-  arm_id: panda_left
-  joint_names:
-    - panda_left_joint1
-    - panda_left_joint2
-    - panda_left_joint3
-    - panda_left_joint4
-    - panda_left_joint5
-    - panda_left_joint6
-    - panda_left_joint7
-panda_right:
-  type: franka_combinable_hw/FrankaCombinableHW
-  arm_id: panda_right
-  joint_names:
-    - panda_right_joint1
-    - panda_right_joint2
-    - panda_right_joint3
-    - panda_right_joint4
-    - panda_right_joint5
-    - panda_right_joint6
-    - panda_right_joint7
diff --git a/franka_combined_control/mainpage.dox b/franka_combined_control/mainpage.dox
deleted file mode 100644
index 941d0bf97714038b58c39c7dcd816e901e22c023..0000000000000000000000000000000000000000
--- a/franka_combined_control/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_combined_control/package.xml b/franka_combined_control/package.xml
deleted file mode 100644
index 883a511d72d52600bf7440dc2fff709d45872ad0..0000000000000000000000000000000000000000
--- a/franka_combined_control/package.xml
+++ /dev/null
@@ -1,27 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>franka_combined_control</name>
-  <version>0.7.0</version>
-  <description>franka_combined_control provides a hardware node to control multiple Panda robots.</description>
-  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
-  <license>Apache 2.0</license>
-
-  <url type="website">http://wiki.ros.org/franka_combined_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 GmbH</author>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <depend>controller_manager</depend>
-  <depend>franka_combinable_hw</depend>
-  <depend>roscpp</depend>
-
-  <exec_depend>franka_description</exec_depend>
-  <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-
-  <build_export_depend>message_runtime</build_export_depend>
-
-</package>
diff --git a/franka_combined_control/rosdoc.yaml b/franka_combined_control/rosdoc.yaml
deleted file mode 100644
index 96ee597ef5cacd0f223f676c738f396f0810b78c..0000000000000000000000000000000000000000
--- a/franka_combined_control/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
-  javadoc_autobrief: YES
diff --git a/franka_combined_example_controllers/CMakeLists.txt b/franka_combined_example_controllers/CMakeLists.txt
deleted file mode 100644
index e9a0f7f62ab9aacc3115cc49e1ceb5853a43d9ce..0000000000000000000000000000000000000000
--- a/franka_combined_example_controllers/CMakeLists.txt
+++ /dev/null
@@ -1,110 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_combined_example_controllers)
-
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_BUILD_TYPE Release)
-
-find_package(catkin REQUIRED COMPONENTS
-  actionlib_msgs
-  controller_interface
-  dynamic_reconfigure
-  eigen_conversions
-  franka_combinable_hw
-  franka_example_controllers
-  franka_hw
-  geometry_msgs
-  hardware_interface
-  tf
-  tf_conversions
-  pluginlib
-  realtime_tools
-  roscpp
-)
-
-find_package(Eigen3 REQUIRED)
-find_package(Franka 0.5.0 REQUIRED)
-
-generate_dynamic_reconfigure_options(
-  cfg/dual_arm_compliance_param.cfg
-)
-
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES franka_combined_example_controllers
-  CATKIN_DEPENDS
-    controller_interface
-    dynamic_reconfigure
-    eigen_conversions
-    franka_combinable_hw
-    franka_example_controllers
-    franka_hw
-    geometry_msgs
-    tf
-    tf_conversions  hardware_interface
-    pluginlib
-    roscpp
-    realtime_tools
-  DEPENDS Franka
-)
-
-add_library(franka_combined_example_controllers
-   src/dual_arm_cartesian_impedance_example_controller.cpp
-)
-
-target_include_directories(franka_combined_example_controllers PUBLIC
-  include
-)
-
-target_include_directories(franka_combined_example_controllers SYSTEM PUBLIC
-  ${catkin_INCLUDE_DIRS}
-  ${Franka_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIRS}
-)
-
-add_dependencies(franka_combined_example_controllers
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  ${PROJECT_NAME}_gencfg
-)
-
-target_link_libraries(franka_combined_example_controllers PUBLIC
-   ${catkin_LIBRARIES}
-   ${Franka_LIBRARIES}
-)
-
-## Installation
-install(TARGETS franka_combined_example_controllers
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY scripts
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(FILES franka_combined_example_controllers_plugin.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
-  RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_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
-  )
-  add_format_target(franka_combined_example_controllers FILES ${SOURCES} ${HEADERS})
-  add_tidy_target(franka_combined_example_controllers
-    FILES ${SOURCES}
-    DEPENDS franka_combined_example_controllers
-  )
-endif()
diff --git a/franka_combined_example_controllers/config/franka_combined_example_controllers.yaml b/franka_combined_example_controllers/config/franka_combined_example_controllers.yaml
deleted file mode 100644
index e9eae02774152578c88ed1bc0ae07c72220db4d8..0000000000000000000000000000000000000000
--- a/franka_combined_example_controllers/config/franka_combined_example_controllers.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-dual_arm_cartesian_impedance_example_controller:
-    type: franka_combined_example_controllers/DualArmCartesianImpedanceExampleController
-    left:
-        arm_id: panda_left
-        joint_names:
-            - panda_left_joint1
-            - panda_left_joint2
-            - panda_left_joint3
-            - panda_left_joint4
-            - panda_left_joint5
-            - panda_left_joint6
-            - panda_left_joint7
-    right:
-        arm_id: panda_right
-        joint_names:
-            - panda_right_joint1
-            - panda_right_joint2
-            - panda_right_joint3
-            - panda_right_joint4
-            - panda_right_joint5
-            - panda_right_joint6
-            - panda_right_joint7
diff --git a/franka_combined_example_controllers/franka_combined_example_controllers_plugin.xml b/franka_combined_example_controllers/franka_combined_example_controllers_plugin.xml
deleted file mode 100644
index 52835eea4e895cf144b281745af47cdadd65d5f7..0000000000000000000000000000000000000000
--- a/franka_combined_example_controllers/franka_combined_example_controllers_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="lib/libfranka_combined_example_controllers">
-  <class name="franka_combined_example_controllers/DualArmCartesianImpedanceExampleController" type="franka_combined_example_controllers::DualArmCartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
-    <description>
-      A controller that renders Cartesian impedance based tracking of separate target poses for 2 panda arms.
-    </description>
-  </class>   
-</library>
diff --git a/franka_combined_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_combined_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
deleted file mode 100644
index 014514f57e761a4899c1e7989bb56e74b70b5786..0000000000000000000000000000000000000000
--- a/franka_combined_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
-  <arg name="robot_id" default="panda_dual" />
-  <arg name="robot_left_ip" default="robot_left.franka.de" />
-  <arg name="robot_right_ip" default="robot_right.franka.de" />
-  <arg name="rviz" default="true" />
-  <arg name="rqt" default="true" />
-
-  <include file="$(find franka_combined_control)/launch/franka_combined_control.launch" >
-    <arg name="robot_id" value="$(arg robot_id)" />
-    <arg name="robot_left_ip" value="$(arg robot_left_ip)" />
-    <arg name="robot_right_ip" value="$(arg robot_right_ip)" />
-  </include>
-
-  <group ns="$(arg robot_id)">
-    <rosparam command="load" file="$(find franka_combined_example_controllers)/config/franka_combined_example_controllers.yaml" />
-    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="dual_arm_cartesian_impedance_example_controller"/>
-    <node name="interactive_marker_left" pkg="franka_combined_example_controllers" type="dual_arm_interactive_marker.py"
-      args="--right_arm_id panda_right --left_arm_id panda_left" required="false" output="screen" />
-    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" if="$(arg rqt)"/>
-    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_combined_example_controllers)/launch/rviz/franka_dual_description_with_marker.rviz" if="$(arg rviz)"/>
-  </group>
-</launch>
diff --git a/franka_combined_example_controllers/package.xml b/franka_combined_example_controllers/package.xml
deleted file mode 100644
index 9ac764edb6f774dd1e1073ea2a823fc79033a664..0000000000000000000000000000000000000000
--- a/franka_combined_example_controllers/package.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>franka_combined_example_controllers</name>
-  <version>0.7.0</version>
-  <description>The franka_combined_example_controllers package</description>
-  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
-  <license>Apache 2.0</license>
-
-  <url type="website">http://wiki.ros.org/franka_combined_control_example_controllers</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 GmbH</author>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>eigen</build_depend>
-
-  <depend>controller_interface</depend>
-  <depend>dynamic_reconfigure</depend>
-  <depend>eigen_conversions</depend>
-  <depend>franka_combinable_hw</depend>
-  <depend>franka_example_controllers</depend>
-  <depend>franka_hw</depend>
-  <depend>geometry_msgs</depend>
-  <depend>hardware_interface</depend>
-  <depend>pluginlib</depend>
-  <depend>tf</depend>
-  <depend>tf_conversions</depend>
-  <depend>libfranka</depend>
-  <depend>realtime_tools</depend>
-  <depend>roscpp</depend>
-
-  <exec_depend>franka_combined_control</exec_depend>
-  <exec_depend>rospy</exec_depend>
-
-  <export>
-    <controller_interface plugin="${prefix}/franka_combined_example_controllers_plugin.xml"/>
-  </export>
-
-</package>
diff --git a/franka_control/CMakeLists.txt b/franka_control/CMakeLists.txt
index 219f2d08cde4b7638be4f0c614493a6162dfb5be..065df6aa3fa39fcd57a5d3135346fcab8a84d0ae 100644
--- a/franka_control/CMakeLists.txt
+++ b/franka_control/CMakeLists.txt
@@ -5,8 +5,6 @@ set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  actionlib_msgs
   controller_interface
   controller_manager
   franka_hw
@@ -23,32 +21,14 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Franka 0.5.0 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 franka_control_services
   CATKIN_DEPENDS
-    actionlib
     controller_interface
     franka_hw
     franka_msgs
     geometry_msgs
-    message_runtime
     pluginlib
     realtime_tools
     roscpp
@@ -80,54 +60,48 @@ target_include_directories(franka_state_controller PUBLIC
   include
 )
 
-## franka_control_services
-add_library(franka_control_services
-  src/services.cpp
+## franka_control_node
+add_executable(franka_control_node
+  src/franka_control_node.cpp
 )
 
-add_dependencies(franka_control_services
+add_dependencies(franka_control_node
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}
 )
 
-target_link_libraries(franka_control_services
+target_link_libraries(franka_control_node
   ${Franka_LIBRARIES}
+  franka_control_services
   ${catkin_LIBRARIES}
 )
 
-target_include_directories(franka_control_services SYSTEM PUBLIC
+target_include_directories(franka_control_node SYSTEM PUBLIC
   ${Franka_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
 )
-target_include_directories(franka_control_services PUBLIC
-  include
-)
 
-## franka_control_node
-add_executable(franka_control_node
-  src/franka_control_node.cpp
+add_executable(franka_combined_control_node
+    src/franka_combined_control_node.cpp
 )
 
-add_dependencies(franka_control_node
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+add_dependencies(franka_combined_control_node
+    ${catkin_EXPORTED_TARGETS}
 )
 
-target_link_libraries(franka_control_node
-  ${Franka_LIBRARIES}
-  franka_control_services
+target_link_libraries(franka_combined_control_node
   ${catkin_LIBRARIES}
 )
 
-target_include_directories(franka_control_node SYSTEM PUBLIC
-  ${Franka_INCLUDE_DIRS}
+target_include_directories(franka_combined_control_node SYSTEM PUBLIC
   ${catkin_INCLUDE_DIRS}
 )
 
+
 ## Installation
 install(TARGETS franka_state_controller
-                franka_control_services
                 franka_control_node
+                franka_combined_control_node
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -158,6 +132,6 @@ if(CLANG_TOOLS)
   add_format_target(franka_control FILES ${SOURCES} ${HEADERS})
   add_tidy_target(franka_control
     FILES ${SOURCES}
-    DEPENDS franka_control_node franka_state_controller
+    DEPENDS franka_control_node franka_combined_control_node franka_state_controller
   )
 endif()
diff --git a/franka_control/config/default_combined_controllers.yaml b/franka_control/config/default_combined_controllers.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e7ef4923f4f38e5d4ab443d1e0d1462b1f29f661
--- /dev/null
+++ b/franka_control/config/default_combined_controllers.yaml
@@ -0,0 +1,88 @@
+effort_joint_trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - panda_2_joint1
+    - panda_2_joint2
+    - panda_2_joint3
+    - panda_2_joint4
+    - panda_2_joint5
+    - panda_2_joint6
+    - panda_2_joint7
+    - panda_1_joint1
+    - panda_1_joint2
+    - panda_1_joint3
+    - panda_1_joint4
+    - panda_1_joint5
+    - panda_1_joint6
+    - panda_1_joint7
+  constraints:
+    goal_time: 0.5
+    panda_2_joint1:
+      goal: 0.05
+    panda_2_joint2:
+      goal: 0.05
+    panda_2_joint3:
+      goal: 0.05
+    panda_2_joint4:
+      goal: 0.05
+    panda_2_joint5:
+      goal: 0.05
+    panda_2_joint6:
+      goal: 0.05
+    panda_2_joint7:
+      goal: 0.05
+    panda_1_joint1:
+      goal: 0.05
+    panda_1_joint2:
+      goal: 0.05
+    panda_1_joint3:
+      goal: 0.05
+    panda_1_joint4:
+      goal: 0.05
+    panda_1_joint5:
+      goal: 0.05
+    panda_1_joint6:
+      goal: 0.05
+    panda_1_joint7:
+      goal: 0.05
+  gains:
+    panda_1_joint1: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_1_joint2: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_1_joint3: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_1_joint4: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_1_joint5: {p: 250, d: 10, i: 0, i_clamp: 1}
+    panda_1_joint6: {p: 150, d: 10, i: 0, i_clamp: 1}
+    panda_1_joint7: {p: 50, d: 5, i: 0, i_clamp: 1}
+    panda_2_joint1: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_2_joint2: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_2_joint3: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_2_joint4: {p: 600, d: 30, i: 0, i_clamp: 1}
+    panda_2_joint5: {p: 250, d: 10, i: 0, i_clamp: 1}
+    panda_2_joint6: {p: 150, d: 10, i: 0, i_clamp: 1}
+    panda_2_joint7: {p: 50, d: 5, i: 0, i_clamp: 1}
+
+panda_1_state_controller:
+  type: franka_control/FrankaStateController
+  arm_id: panda_1
+  joint_names:
+    - panda_1_joint1
+    - panda_1_joint2
+    - panda_1_joint3
+    - panda_1_joint4
+    - panda_1_joint5
+    - panda_1_joint6
+    - panda_1_joint7
+  publish_rate: 30  # [Hz]
+
+panda_2_state_controller:
+  type: franka_control/FrankaStateController
+  arm_id: panda_2
+  joint_names:
+    - panda_2_joint1
+    - panda_2_joint2
+    - panda_2_joint3
+    - panda_2_joint4
+    - panda_2_joint5
+    - panda_2_joint6
+    - panda_2_joint7
+  publish_rate: 30  # [Hz]
diff --git a/franka_control/config/franka_combined_control_node.yaml b/franka_control/config/franka_combined_control_node.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8fd271c34c7ba1dd1b617ee7c017c606530b38fb
--- /dev/null
+++ b/franka_control/config/franka_combined_control_node.yaml
@@ -0,0 +1,63 @@
+robot_hardware:
+  - panda_1
+  - panda_2
+
+panda_1:
+  type: franka_hw/FrankaCombinableHW
+  arm_id: panda_1
+  joint_names:
+    - panda_1_joint1
+    - panda_1_joint2
+    - panda_1_joint3
+    - panda_1_joint4
+    - panda_1_joint5
+    - panda_1_joint6
+    - panda_1_joint7
+  # Configure the threshold angle for printing joint limit warnings.
+  joint_limit_warning_threshold: 0.1 # [rad]
+  # Activate rate limiter? [true|false]
+  rate_limiting: true
+  # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+  cutoff_frequency: 100
+  # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+  internal_controller: joint_impedance
+  # Configure the initial defaults for the collision behavior reflexes.
+  collision_config:
+    lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+
+panda_2:
+  type: franka_hw/FrankaCombinableHW
+  arm_id: panda_2
+  joint_names:
+    - panda_2_joint1
+    - panda_2_joint2
+    - panda_2_joint3
+    - panda_2_joint4
+    - panda_2_joint5
+    - panda_2_joint6
+    - panda_2_joint7
+  # Configure the threshold angle for printing joint limit warnings.
+  joint_limit_warning_threshold: 0.1 # [rad]
+  # Activate rate limiter? [true|false]
+  rate_limiting: true
+  # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+  cutoff_frequency: 100
+  # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+  internal_controller: joint_impedance
+  # Configure the initial defaults for the collision behavior reflexes.
+  collision_config:
+    lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
diff --git a/franka_control/config/franka_control_node.yaml b/franka_control/config/franka_control_node.yaml
index fd2524afc14bc81163d105843f6eddd57436117a..5a0c78c807802643ad90b38ca25c2c6e731dc335 100644
--- a/franka_control/config/franka_control_node.yaml
+++ b/franka_control/config/franka_control_node.yaml
@@ -7,9 +7,21 @@ joint_names:
   - panda_joint6
   - panda_joint7
 arm_id: panda
+# Configure the threshold angle for printing joint limit warnings.
+joint_limit_warning_threshold: 0.1 # [rad]
 # Activate rate limiter? [true|false]
 rate_limiting: true
 # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
 cutoff_frequency: 100
 # Internal controller for motion generators [joint_impedance|cartesian_impedance]
 internal_controller: joint_impedance
+# Configure the initial defaults for the collision behavior reflexes.
+collision_config:
+  lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
diff --git a/franka_control/include/franka_control/services.h b/franka_control/include/franka_control/services.h
deleted file mode 100644
index 5589a15a84fd99a1913254e82aa9fd0997271561..0000000000000000000000000000000000000000
--- a/franka_control/include/franka_control/services.h
+++ /dev/null
@@ -1,72 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#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>
-
-#include <vector>
-
-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;
-      });
-}
-
-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_;
-};
-
-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
diff --git a/franka_combined_control/launch/franka_combined_control.launch b/franka_control/launch/franka_combined_control.launch
similarity index 53%
rename from franka_combined_control/launch/franka_combined_control.launch
rename to franka_control/launch/franka_combined_control.launch
index 8bd89bb4a8daa4407af9e657cd0cb9b71b8d53f3..c17e7b927a4458adcce542cc7cb3804b713028dc 100644
--- a/franka_combined_control/launch/franka_combined_control.launch
+++ b/franka_control/launch/franka_combined_control.launch
@@ -7,31 +7,31 @@
   <!-- Additional XACRO args. Be sure to escape colons and equal signs
        with backslashes, because XACRO use same syntax as roslaunch:
        roslaunch <pkg> <launchfile> robot:=path/to/urdf.xacro args:="foo\:\=bar other\:\=true"  -->
-
   <arg name="args" default="" />
 
-  <arg name="robot_id" default="panda_dual" />
-  <arg name="robot_left_ip" default="robot_left.franka.de" />
-  <arg name="robot_right_ip" default="robot_right.franka.de" />
+  <arg name="robot_id" default="combined_panda" />
+  <arg name="robot_1_ip" default="robot_1.franka.de" />
+  <arg name="robot_2_ip" default="robot_2.franka.de" />
+  <arg name="hw_config_file" default="$(find franka_control)/config/franka_combined_control_node.yaml"/>
 
-  <node name="$(arg robot_id)" pkg="franka_combined_control" type="franka_combined_control_node" output="screen" required="true" >
-    <rosparam command="load" file="$(find franka_combined_control)/config/franka_combined_control_node.yaml" />
-    <param name="panda_left/robot_ip" value="$(arg robot_left_ip)" />
-    <param name="panda_right/robot_ip" value="$(arg robot_right_ip)" />
+  <node name="$(arg robot_id)" pkg="franka_control" type="franka_combined_control_node" output="screen" required="true" >
+    <rosparam command="load" file="$(arg hw_config_file)" />
+    <param name="panda_1/robot_ip" value="$(arg robot_1_ip)" />
+    <param name="panda_2/robot_ip" value="$(arg robot_2_ip)" />
     <param name="robot_description" command="xacro --inorder $(arg robot) $(arg args)" />
   </node>
 
   <group ns="$(arg robot_id)">
-    <rosparam command="load" file="$(find franka_combined_control)/config/default_combined_controllers.yaml" />
+    <rosparam command="load" file="$(find franka_control)/config/default_combined_controllers.yaml" />
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
     <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
-      args="panda_left_state_controller panda_right_state_controller"/>
+      args="panda_1_state_controller panda_2_state_controller"/>
     <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
       <param name="robot_description" command="xacro --inorder $(arg robot) $(arg args)" />
-      <rosparam param="source_list">[panda_left_state_controller/joint_states,
-        panda_right_state_controller/joint_states,
-        panda_left/franka_gripper/joint_states,
-        panda_right/franka_gripper/joint_states]
+      <rosparam param="source_list">[panda_1_state_controller/joint_states,
+        panda_2_state_controller/joint_states,
+        panda_1/franka_gripper/joint_states,
+        panda_2/franka_gripper/joint_states]
       </rosparam>
       <param name="rate" value="30"/>
     </node>
diff --git a/franka_control/package.xml b/franka_control/package.xml
index c01d2bd7726f86affcde02c44a9a56c0beb6a6bf..af060e9eecabd971fb5c79570f57f47e30f4e4d0 100644
--- a/franka_control/package.xml
+++ b/franka_control/package.xml
@@ -13,11 +13,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>message_generation</build_depend>
-
   <depend>libfranka</depend>
-  <depend>actionlib</depend>
-  <depend>actionlib_msgs</depend>
   <depend>controller_interface</depend>
   <depend>controller_manager</depend>
   <depend>franka_hw</depend>
@@ -33,11 +29,8 @@
   <exec_depend>franka_description</exec_depend>
   <exec_depend>franka_gripper</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
 
-  <build_export_depend>message_runtime</build_export_depend>
-
   <export>
     <controller_interface plugin="${prefix}/franka_controller_plugins.xml"/>
   </export>
diff --git a/franka_combined_control/src/franka_combined_control_node.cpp b/franka_control/src/franka_combined_control_node.cpp
similarity index 65%
rename from franka_combined_control/src/franka_combined_control_node.cpp
rename to franka_control/src/franka_combined_control_node.cpp
index 2553c3dc45706a6b9cf75f822e1986004ff6b74c..a5211c767131a3a795039df672adf4229d559220 100644
--- a/franka_combined_control/src/franka_combined_control_node.cpp
+++ b/franka_control/src/franka_combined_control_node.cpp
@@ -2,11 +2,11 @@
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
 
 #include <controller_manager/controller_manager.h>
-#include <franka_combinable_hw/franka_combined_hw.h>
+#include <franka_hw/franka_combined_hw.h>
 #include <ros/ros.h>
 
 #include <sched.h>
-#include <stdexcept>
+#include <string>
 
 int main(int argc, char** argv) {
   ros::init(argc, argv, "franka_combined_control_node");
@@ -14,21 +14,13 @@ int main(int argc, char** argv) {
   ros::AsyncSpinner spinner(4);
   spinner.start();
 
-  ros::NodeHandle nh("~");
-  franka_combinable_hw::FrankaCombinedHW hw;
-  bool init_success = hw.init(nh, nh);
-
-  if (!init_success) {
-    throw std::runtime_error(
-        "franka_combined_control_node:: Initialization of FrankaCombinedHW failed!");
+  ros::NodeHandle private_node_handle("~");
+  franka_hw::FrankaCombinedHW franka_control;
+  if (!franka_control.init(private_node_handle, private_node_handle)) {
+    ROS_ERROR("franka_combined_control_node:: Initialization of FrankaCombinedHW failed!");
     return 1;
   }
 
-  controller_manager::ControllerManager cm(&hw, nh);
-
-  ros::Duration period(0.001);
-  ros::Rate rate(period);
-
   // set current control_loop thread to real-time
   const int kThreadPriority = sched_get_priority_max(SCHED_FIFO);
   if (kThreadPriority == -1) {
@@ -44,11 +36,16 @@ int main(int argc, char** argv) {
     return 1;
   }
 
+  controller_manager::ControllerManager cm(&franka_control, private_node_handle);
+  ros::Duration period(0.001);
+  ros::Rate rate(period);
+
   while (ros::ok()) {
     rate.sleep();
-    hw.read(ros::Time::now(), period);
-    cm.update(ros::Time::now(), period, hw.controllerNeedsReset());
-    hw.write(ros::Time::now(), period);
+    ros::Time now = ros::Time::now();
+    franka_control.read(now, period);
+    cm.update(now, period, franka_control.controllerNeedsReset());
+    franka_control.write(now, period);
   }
 
   return 0;
diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp
index 0b0a2fab92866fa2fdbe83c3243c1d918fdf0e82..fae2f0f6c2c316c8e258dec4b8c46051951c8c18 100644
--- a/franka_control/src/franka_control_node.cpp
+++ b/franka_control/src/franka_control_node.cpp
@@ -11,146 +11,45 @@
 #include <franka/exception.h>
 #include <franka/robot.h>
 #include <franka_hw/franka_hw.h>
+#include <franka_hw/services.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 #include <ros/ros.h>
 
-#include <franka_control/ErrorRecoveryAction.h>
-#include <franka_control/services.h>
-
-using franka_control::ServiceContainer;
+using franka_hw::ServiceContainer;
 
 int main(int argc, char** argv) {
   ros::init(argc, argv, "franka_control_node");
+
   ros::NodeHandle public_node_handle;
   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");
+  franka_hw::FrankaHW franka_control;
+  if (!franka_control.init(public_node_handle, node_handle)) {
+    ROS_ERROR("franka_control_node: Failed to initialize FrankaHW class. Shutting down!");
     return 1;
   }
 
-  std::array<std::string, 7> joint_names;
-  std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
-
-  bool rate_limiting;
-  if (!node_handle.getParamCached("rate_limiting", rate_limiting)) {
-    ROS_ERROR("Invalid or no rate_limiting parameter provided");
-    return 1;
-  }
-
-  double cutoff_frequency;
-  if (!node_handle.getParamCached("cutoff_frequency", cutoff_frequency)) {
-    ROS_ERROR("Invalid or no cutoff_frequency parameter provided");
-    return 1;
-  }
-
-  std::string internal_controller;
-  if (!node_handle.getParam("internal_controller", internal_controller)) {
-    ROS_ERROR("No internal_controller parameter provided");
-    return 1;
-  }
-
-  urdf::Model urdf_model;
-  if (!urdf_model.initParamWithNodeHandle("robot_description", public_node_handle)) {
-    ROS_ERROR("Could not initialize URDF model from robot_description");
-    return 1;
-  }
-
-  std::string robot_ip;
-  if (!node_handle.getParam("robot_ip", robot_ip)) {
-    ROS_ERROR("Invalid or no robot_ip parameter provided");
-    return 1;
-  }
-
-  std::string arm_id;
-  if (!node_handle.getParam("arm_id", arm_id)) {
-    ROS_ERROR("Invalid or no arm_id parameter provided");
-    return 1;
-  }
-  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}});
+  franka::Robot& robot = franka_control.robot();
 
   std::atomic_bool has_error(false);
 
   ServiceContainer services;
-  services
-      .advertiseService<franka_control::SetJointImpedance>(
-          node_handle, "set_joint_impedance",
-          [&robot](auto&& req, auto&& res) {
-            return franka_control::setJointImpedance(robot, req, res);
-          })
-      .advertiseService<franka_control::SetCartesianImpedance>(
-          node_handle, "set_cartesian_impedance",
-          [&robot](auto&& req, auto&& res) {
-            return franka_control::setCartesianImpedance(robot, req, res);
-          })
-      .advertiseService<franka_control::SetEEFrame>(
-          node_handle, "set_EE_frame",
-          [&robot](auto&& req, auto&& res) { return franka_control::setEEFrame(robot, req, res); })
-      .advertiseService<franka_control::SetKFrame>(
-          node_handle, "set_K_frame",
-          [&robot](auto&& req, auto&& res) { return franka_control::setKFrame(robot, req, res); })
-      .advertiseService<franka_control::SetForceTorqueCollisionBehavior>(
-          node_handle, "set_force_torque_collision_behavior",
-          [&robot](auto&& req, auto&& res) {
-            return franka_control::setForceTorqueCollisionBehavior(robot, req, res);
-          })
-      .advertiseService<franka_control::SetFullCollisionBehavior>(
-          node_handle, "set_full_collision_behavior",
-          [&robot](auto&& req, auto&& res) {
-            return franka_control::setFullCollisionBehavior(robot, req, res);
-          })
-      .advertiseService<franka_control::SetLoad>(
-          node_handle, "set_load",
-          [&robot](auto&& req, auto&& res) { return franka_control::setLoad(robot, req, res); });
-
-  actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction> recovery_action_server(
+  franka_hw::setupServices(robot, node_handle, services);
+
+  actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction> recovery_action_server(
       node_handle, "error_recovery",
-      [&](const franka_control::ErrorRecoveryGoalConstPtr&) {
+      [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
         try {
           robot.automaticErrorRecovery();
           has_error = false;
           recovery_action_server.setSucceeded();
           ROS_INFO("Recovered from error");
         } catch (const franka::Exception& ex) {
-          recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what());
+          recovery_action_server.setAborted(franka_msgs::ErrorRecoveryResult(), ex.what());
         }
       },
       false);
 
-  franka::Model model = robot.loadModel();
-  auto get_rate_limiting = [&]() {
-    node_handle.getParamCached("rate_limiting", rate_limiting);
-    return rate_limiting;
-  };
-  auto get_internal_controller = [&]() {
-    node_handle.getParamCached("internal_controller", internal_controller);
-
-    franka::ControllerMode controller_mode;
-    if (internal_controller == "joint_impedance") {
-      controller_mode = franka::ControllerMode::kJointImpedance;
-    } else if (internal_controller == "cartesian_impedance") {
-      controller_mode = franka::ControllerMode::kCartesianImpedance;
-    } else {
-      ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance");
-      controller_mode = franka::ControllerMode::kJointImpedance;
-    }
-
-    return controller_mode;
-  };
-  auto get_cutoff_frequency = [&]() {
-    node_handle.getParamCached("cutoff_frequency", cutoff_frequency);
-    return cutoff_frequency;
-  };
-  franka_hw::FrankaHW franka_control(joint_names, arm_id, urdf_model, model, get_rate_limiting,
-                                     get_cutoff_frequency, get_internal_controller);
-
   // Initialize robot state before loading any controller
   franka_control.update(robot.readOnce());
 
@@ -181,7 +80,7 @@ int main(int argc, char** argv) {
 
     try {
       // Run control loop. Will exit if the controller is switched.
-      franka_control.control(robot, [&](const ros::Time& now, const ros::Duration& period) {
+      franka_control.control([&](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);
diff --git a/franka_description/robots/dual_panda_example.urdf.xacro b/franka_description/robots/dual_panda_example.urdf.xacro
index cd7dbebae17d4d9f66909cf124ff8e4d767854da..f98c133bf96cdfbd61c5d76549a9d2b8f17d494f 100644
--- a/franka_description/robots/dual_panda_example.urdf.xacro
+++ b/franka_description/robots/dual_panda_example.urdf.xacro
@@ -1,8 +1,11 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:arg name="arm_id_1" default="panda_1" />
+  <xacro:arg name="arm_id_2" default="panda_2" />
+
   <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
   <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
-  
+
   <!-- box shaped table as base for the 2 Pandas -->
   <link name="base">
     <visual>
@@ -21,13 +24,13 @@
       </geometry>
     </collision>
   </link>
-  
-  <!-- left arm with gripper -->
-  <xacro:panda_arm arm_id="panda_left" connected_to="base"  xyz="0 0.5 1" safety_distance="0.03"/>
-  <xacro:hand ns="panda_left" rpy="0 0 ${-pi/4}" connected_to="panda_left_link8" safety_distance="0.03"/>
-  
+
   <!-- right arm with gripper -->
-  <xacro:panda_arm arm_id="panda_right" connected_to="base"  xyz="0 -0.5 1" safety_distance="0.03"/>
-  <xacro:hand ns="panda_right" rpy="0 0 ${-pi/4}" connected_to="panda_right_link8" safety_distance="0.03"/>
-  
+  <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base"  xyz="0 -0.5 1" safety_distance="0.03"/>
+  <xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" safety_distance="0.03"/>
+
+  <!-- left arm with gripper -->
+  <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base"  xyz="0 0.5 1" safety_distance="0.03"/>
+  <xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8" safety_distance="0.03"/>
+
 </robot>
diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
index 7942e577ce64f8a9798812e7bfbcde9154c3c3b2..669121caa156141cc3abbc5b36f15dddcd1a2e8c 100644
--- a/franka_example_controllers/CMakeLists.txt
+++ b/franka_example_controllers/CMakeLists.txt
@@ -2,16 +2,18 @@ cmake_minimum_required(VERSION 3.4)
 project(franka_example_controllers)
 
 set(CMAKE_BUILD_TYPE Release)
-
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(catkin REQUIRED COMPONENTS
   controller_interface
   dynamic_reconfigure
+  eigen_conversions
   franka_hw
   geometry_msgs
   hardware_interface
+  tf
+  tf_conversions
   message_generation
   pluginlib
   realtime_tools
@@ -31,6 +33,7 @@ generate_messages()
 generate_dynamic_reconfigure_options(
   cfg/compliance_param.cfg
   cfg/desired_mass_param.cfg
+  cfg/dual_arm_compliance_param.cfg
 )
 
 catkin_package(
@@ -39,9 +42,12 @@ catkin_package(
   CATKIN_DEPENDS
     controller_interface
     dynamic_reconfigure
+    eigen_conversions
     franka_hw
     geometry_msgs
     hardware_interface
+    tf
+    tf_conversions
     message_runtime
     pluginlib
     realtime_tools
@@ -59,6 +65,7 @@ add_library(franka_example_controllers
   src/joint_impedance_example_controller.cpp
   src/cartesian_impedance_example_controller.cpp
   src/force_example_controller.cpp
+  src/dual_arm_cartesian_impedance_example_controller.cpp
 )
 
 add_dependencies(franka_example_controllers
@@ -99,7 +106,7 @@ install(FILES franka_example_controllers_plugin.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 catkin_install_python(
-  PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py
+  PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py scripts/dual_arm_interactive_marker.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
diff --git a/franka_combined_example_controllers/cfg/dual_arm_compliance_param.cfg b/franka_example_controllers/cfg/dual_arm_compliance_param.cfg
similarity index 100%
rename from franka_combined_example_controllers/cfg/dual_arm_compliance_param.cfg
rename to franka_example_controllers/cfg/dual_arm_compliance_param.cfg
diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml
index 4b7707efc53ad63104834189ee45cdd48d510c33..b82b7efa8cebbf5563287afa94962c2ef4ca4e8b 100644
--- a/franka_example_controllers/config/franka_example_controllers.yaml
+++ b/franka_example_controllers/config/franka_example_controllers.yaml
@@ -92,3 +92,26 @@ cartesian_impedance_example_controller:
         - panda_joint5
         - panda_joint6
         - panda_joint7
+
+dual_arm_cartesian_impedance_example_controller:
+    type: franka_example_controllers/DualArmCartesianImpedanceExampleController
+    right:
+        arm_id: panda_1
+        joint_names:
+            - panda_1_joint1
+            - panda_1_joint2
+            - panda_1_joint3
+            - panda_1_joint4
+            - panda_1_joint5
+            - panda_1_joint6
+            - panda_1_joint7
+    left:
+        arm_id: panda_2
+        joint_names:
+            - panda_2_joint1
+            - panda_2_joint2
+            - panda_2_joint3
+            - panda_2_joint4
+            - panda_2_joint5
+            - panda_2_joint6
+            - panda_2_joint7
diff --git a/franka_example_controllers/franka_example_controllers_plugin.xml b/franka_example_controllers/franka_example_controllers_plugin.xml
index 559a9f63eccaa1f924094702ffe1d38ec5dd9789..e770f046880f32432fe13f282b05e3f103cb2b65 100644
--- a/franka_example_controllers/franka_example_controllers_plugin.xml
+++ b/franka_example_controllers/franka_example_controllers_plugin.xml
@@ -36,13 +36,17 @@
   </class>
   <class name="franka_example_controllers/CartesianImpedanceExampleController" type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
     <description>
-      A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. 
+      A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively.
     </description>
   </class>
   <class name="franka_example_controllers/ForceExampleController" type="franka_example_controllers::ForceExampleController" base_class_type="controller_interface::ControllerBase">
     <description>
-      A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure. 
+      A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure.
     </description>
   </class>
-
+  <class name="franka_example_controllers/DualArmCartesianImpedanceExampleController" type="franka_example_controllers::DualArmCartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A controller that renders Cartesian impedance based tracking of separate target poses for 2 panda arms.
+      </description>
+  </class>
 </library>
diff --git a/franka_combined_example_controllers/include/franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
similarity index 97%
rename from franka_combined_example_controllers/include/franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h
rename to franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
index d8d5aeb8a329529ffbbc877e8536aeed7bfdef5b..4af1a6ccc36f34f002b12eeba8b399070f6ede78 100644
--- a/franka_combined_example_controllers/include/franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
@@ -16,12 +16,12 @@
 #include <ros/time.h>
 #include <Eigen/Dense>
 
-#include <franka_combined_example_controllers/dual_arm_compliance_paramConfig.h>
+#include <franka_example_controllers/dual_arm_compliance_paramConfig.h>
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
 #include <franka_hw/trigger_rate.h>
 
-namespace franka_combined_example_controllers {
+namespace franka_example_controllers {
 
 /**
  * This container holds all data and parameters used to control one panda arm with a Cartesian
@@ -179,4 +179,4 @@ class DualArmCartesianImpedanceExampleController
   void publishCenteringPose();
 };
 
-}  // namespace franka_combined_example_controllers
+}  // namespace franka_example_controllers
diff --git a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..186fdcb3d854768d97e99d9b46ad5882c91aab43
--- /dev/null
+++ b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_id" default="panda_dual" />
+  <arg name="robot_right_ip" default="robot_1.franka.de" />
+  <arg name="robot_left_ip" default="robot_2.franka.de" />
+  <arg name="rviz" default="true" />
+  <arg name="rqt" default="true" />
+
+  <include file="$(find franka_control)/launch/franka_combined_control.launch" >
+    <arg name="robot_id" value="$(arg robot_id)" />
+    <arg name="robot_1_ip" value="$(arg robot_right_ip)" />
+    <arg name="robot_2_ip" value="$(arg robot_left_ip)" />
+  </include>
+
+  <group ns="$(arg robot_id)">
+    <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="dual_arm_cartesian_impedance_example_controller"/>
+    <node name="interactive_marker_left" pkg="franka_example_controllers" type="dual_arm_interactive_marker.py"
+      args="--right_arm_id panda_1 --left_arm_id panda_2" required="false" output="screen" />
+    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" if="$(arg rqt)"/>
+    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_dual_description_with_marker.rviz" if="$(arg rviz)"/>
+  </group>
+</launch>
diff --git a/franka_example_controllers/launch/joint_impedance_example_controller.launch b/franka_example_controllers/launch/joint_impedance_example_controller.launch
index 380c5e1f83553942c7426c0c8586e5aa0400940b..326617e1532ef9fd200f688321096e8b6f3afdb4 100644
--- a/franka_example_controllers/launch/joint_impedance_example_controller.launch
+++ b/franka_example_controllers/launch/joint_impedance_example_controller.launch
@@ -9,5 +9,4 @@
 
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_impedance_example_controller"/>
-  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
 </launch>
diff --git a/franka_combined_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
similarity index 92%
rename from franka_combined_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
rename to franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
index 00a20474a596ddf367b67d57ea20eb038e3a9382..bdd4b593a530fb3c89e7423473e06f7e7635fa04 100644
--- a/franka_combined_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
+++ b/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
@@ -6,6 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /RobotModel1
         - /InteractiveMarkers1
       Splitter Ratio: 0.655555546
     Tree Height: 820
@@ -80,122 +81,122 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_hand:
+        panda_1_hand:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_leftfinger:
+        panda_1_leftfinger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link0:
+        panda_1_link0:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link1:
+        panda_1_link1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link2:
+        panda_1_link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link3:
+        panda_1_link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link4:
+        panda_1_link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link5:
+        panda_1_link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link6:
+        panda_1_link6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link7:
+        panda_1_link7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_link8:
+        panda_1_link8:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_left_rightfinger:
+        panda_1_rightfinger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_hand:
+        panda_2_hand:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_leftfinger:
+        panda_2_leftfinger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link0:
+        panda_2_link0:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link1:
+        panda_2_link1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link2:
+        panda_2_link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link3:
+        panda_2_link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link4:
+        panda_2_link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link5:
+        panda_2_link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link6:
+        panda_2_link6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link7:
+        panda_2_link7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_link8:
+        panda_2_link8:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        panda_right_rightfinger:
+        panda_2_rightfinger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
index a382e13c87eb07e8320ed21682350db9d58d0f9b..65636db264699a3852f5d9f3bba7f9311424c0fc 100644
--- a/franka_example_controllers/package.xml
+++ b/franka_example_controllers/package.xml
@@ -20,9 +20,12 @@
 
   <depend>controller_interface</depend>
   <depend>dynamic_reconfigure</depend>
+  <depend>eigen_conversions</depend>
   <depend>franka_hw</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
+  <depend>tf</depend>
+  <depend>tf_conversions</depend>
   <depend>libfranka</depend>
   <depend>pluginlib</depend>
   <depend>realtime_tools</depend>
diff --git a/franka_combined_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
similarity index 90%
rename from franka_combined_example_controllers/scripts/dual_arm_interactive_marker.py
rename to franka_example_controllers/scripts/dual_arm_interactive_marker.py
index 0cb22bbfc1f9d187ad03c31951f9af5f3271be2c..d2e6657c1dc436415202fbeb7b79b26e19f8677b 100755
--- a/franka_combined_example_controllers/scripts/dual_arm_interactive_marker.py
+++ b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
@@ -12,8 +12,8 @@ from interactive_markers.interactive_marker_server import \
     InteractiveMarkerServer, InteractiveMarkerFeedback
 from visualization_msgs.msg import InteractiveMarker, \
     InteractiveMarkerControl, Marker
+from franka_msgs.msg import FrankaState
 from geometry_msgs.msg import PoseStamped
-from std_msgs.msg import Bool
 
 marker_pose = PoseStamped()
 
@@ -53,25 +53,31 @@ def publish_target_pose():
     pose_pub.publish(marker_pose)
 
 
-def left_has_error_callback(msg):
+def left_franka_state_callback(msg):
     """
     This callback function set `has_error` variable to True if the left arm is having an error.
-    :param msg: error msg data
+    :param msg: FrankaState msg data
     :return:  None
     """
-    global has_error, left_has_error, right_has_error
-    left_has_error = msg.data
+    global has_error, left_has_error
+    if msg.robot_mode == FrankaState.ROBOT_MODE_MOVE:
+        left_has_error = False
+    else:
+        left_has_error = True
     has_error = left_has_error or right_has_error
 
 
-def right_has_error_callback(msg):
+def right_franka_state_callback(msg):
     """
     This callback function set `has_error` variable to True if the right arm is having an error.
-    :param msg: error msg data
+    :param msg: FrankaState msg data
     :return:  None
     """
-    global has_error, left_has_error, right_has_error
-    right_has_error = msg.data
+    global has_error, right_has_error
+    if msg.robot_mode == FrankaState.ROBOT_MODE_MOVE:
+        right_has_error = False
+    else:
+        right_has_error = True
     has_error = left_has_error or right_has_error
 
 
@@ -142,10 +148,11 @@ if __name__ == "__main__":
     right_arm_id = args.right_arm_id
 
     # Initialize subscribers for error states of the arms
-    left_error_sub = rospy.Subscriber(left_arm_id + "/has_error", Bool,
-                                      left_has_error_callback)
-    right_error_sub = rospy.Subscriber(right_arm_id + "/has_error", Bool,
-                                       right_has_error_callback)
+    left_state_sub = rospy.Subscriber(left_arm_id + "_state_controller/franka_states",
+                                 FrankaState, left_franka_state_callback)
+
+    right_state_sub = rospy.Subscriber(right_arm_id + "_state_controller/franka_states",
+                                 FrankaState, right_franka_state_callback)
 
     # Set marker pose to be the current "middle pose" of both EEs
     reset_marker_pose_blocking()
diff --git a/franka_combined_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
similarity index 98%
rename from franka_combined_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
rename to franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
index c29e8f77141f1a0a4262686e99ac1b9f613bf957..8bb703c1301258b95a13062983e6f6d42461c6f3 100644
--- a/franka_combined_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
@@ -1,6 +1,6 @@
 // Copyright (c) 2019 Franka Emika GmbH
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h>
+#include <franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h>
 
 #include <cmath>
 #include <functional>
@@ -18,7 +18,7 @@
 #include <tf/transform_listener.h>
 #include <tf_conversions/tf_eigen.h>
 
-namespace franka_combined_example_controllers {
+namespace franka_example_controllers {
 
 bool DualArmCartesianImpedanceExampleController::initArm(
     hardware_interface::RobotHW* robot_hw,
@@ -406,8 +406,7 @@ void DualArmCartesianImpedanceExampleController::publishCenteringPose() {
   }
 }
 
-}  // namespace franka_combined_example_controllers
+}  // namespace franka_example_controllers
 
-PLUGINLIB_EXPORT_CLASS(
-    franka_combined_example_controllers::DualArmCartesianImpedanceExampleController,
-    controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::DualArmCartesianImpedanceExampleController,
+                       controller_interface::ControllerBase)
diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
index db3f0916e77478974fbd3e4cbd2f615471d83665..90b93b8a7db2bf50394e0757988e0837aa07be80 100644
--- a/franka_hw/CMakeLists.txt
+++ b/franka_hw/CMakeLists.txt
@@ -5,11 +5,16 @@ set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(catkin REQUIRED COMPONENTS
+  actionlib
   controller_interface
+  combined_robot_hw
   hardware_interface
   joint_limits_interface
   roscpp
+  pluginlib
   urdf
+  franka_msgs
+  message_generation
 )
 
 find_package(Franka 0.5.0 REQUIRED)
@@ -17,13 +22,25 @@ find_package(Franka 0.5.0 REQUIRED)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES franka_hw
-  CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp urdf
+  CATKIN_DEPENDS
+    actionlib_msgs
+    actionlib
+    controller_interface
+    combined_robot_hw
+    hardware_interface
+    joint_limits_interface
+    roscpp
+    pluginlib
+    urdf
+    franka_msgs
   DEPENDS Franka
 )
 
 add_library(franka_hw
   src/control_mode.cpp
   src/franka_hw.cpp
+  src/franka_combinable_hw.cpp
+  src/franka_combined_hw.cpp
   src/resource_helpers.cpp
   src/trigger_rate.cpp
 )
@@ -36,6 +53,7 @@ add_dependencies(franka_hw
 target_link_libraries(franka_hw
   ${Franka_LIBRARIES}
   ${catkin_LIBRARIES}
+  franka_control_services
 )
 
 target_include_directories(franka_hw SYSTEM PUBLIC
@@ -56,6 +74,29 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
+## franka_control_services
+add_library(franka_control_services
+  src/services.cpp
+)
+
+add_dependencies(franka_control_services
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(franka_control_services
+  ${Franka_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+target_include_directories(franka_control_services SYSTEM PUBLIC
+  ${Franka_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+target_include_directories(franka_control_services PUBLIC
+  include
+)
+
 if(CATKIN_ENABLE_TESTING)
   add_subdirectory(test)
 endif()
diff --git a/franka_hw/franka_combinable_hw_plugin.xml b/franka_hw/franka_combinable_hw_plugin.xml
new file mode 100644
index 0000000000000000000000000000000000000000..12763ff6b121246a221708af40c67016e5cde51b
--- /dev/null
+++ b/franka_hw/franka_combinable_hw_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libfranka_hw">
+  <class name="franka_hw/FrankaCombinableHW" type="franka_hw::FrankaCombinableHW" base_class_type="hardware_interface::RobotHW">
+    <description>
+      A robot hardware interface class for franka robots that is combinable with other FrankaCombinableHW.
+    </description>
+  </class>
+</library>
diff --git a/franka_hw/include/franka_hw/franka_combinable_hw.h b/franka_hw/include/franka_hw/franka_combinable_hw.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d2ed2191c1b09ecf3de9efaee3e2ab7d0084dde
--- /dev/null
+++ b/franka_hw/include/franka_hw/franka_combinable_hw.h
@@ -0,0 +1,172 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <franka/exception.h>
+#include <franka/robot_state.h>
+
+#include <actionlib/server/simple_action_server.h>
+#include <franka_hw/franka_hw.h>
+#include <franka_hw/services.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
+#include <ros/ros.h>
+
+namespace franka_hw {
+
+/**
+ * A hardware class for a Panda robot based on the ros_control framework.
+ * This class is ready to be combined with other hardware classes e.g. to
+ * control mulitple robots from a single controller.
+ * Note: This class allows for torque (effort) control only due to the lack of synchronization
+ * between master controllers of different robots. For more information see the documentation at
+ * https://frankaemika.github.io/docs/franka_ros.html .
+ */
+class FrankaCombinableHW : public FrankaHW {
+ public:
+  /**
+   * Creates an instance of FrankaCombinableHW.
+   */
+  FrankaCombinableHW();
+
+  /**
+   * Initializes the class in terms of ros_control interfaces.
+   * Note: You have to call initParameters beforehand. Use the complete initialization routine
+   * \ref init() method to control robots.
+   *
+   * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware.
+   * @return True if successful, false otherwise.
+   */
+  void initROSInterfaces(ros::NodeHandle& robot_hw_nh) override;
+
+  /**
+   * Runs the currently active controller in a realtime loop. If no controller is active, the
+   * function immediately exits.
+   *
+   * @param[in] ros_callback A callback function that is executed at each time step.
+   *
+   * @throw franka::ControlException if an error related to torque control occured.
+   * @throw franka::InvalidOperationException if a conflicting operation is already running.
+   * @throw franka::NetworkException if the connection is lost, e.g. after a timeout.
+   * @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
+   */
+  void control(const std::function<bool(const ros::Time&, const ros::Duration&)>&
+                   ros_callback =  // NOLINT (google-default-arguments)
+               [](const ros::Time&, const ros::Duration&) {
+                 return true;
+               }) const override;  // NOLINT (google-default-arguments)
+
+  /**
+   * Checks whether a requested controller can be run, based on the resources and interfaces it
+   * claims. Note: FrankaCombinableHW allows torque control only.
+   *
+   * @param[in] info Controllers to be running at the same time.
+   *
+   * @return True in case of a conflict, false in case of valid controllers.
+   */
+  bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override;
+
+  /**
+   * Reads data from the franka robot.
+   *
+   * @param[in] time The current time. Not used in this class.
+   * @param[in] period The time passed since the last call to \ref read. Not used in this class.
+   */
+  void read(const ros::Time& /*time*/, const ros::Duration& /*period*/) override;
+
+  /**
+   * Writes data to the franka robot.
+   *
+   * @param[in] time The current time. Not used in this class.
+   * @param[in] period The time passed since the last call to \ref write.
+   */
+  void write(const ros::Time& /*time*/, const ros::Duration& period) override;
+
+  /**
+   * Getter method for the arm_id which is used to distinguish between mulitple
+   * instances of FrankaCombinableHW.
+   *
+   * @return A copy of the arm_id string indentifying the class instance.
+   */
+  std::string getArmID() const noexcept;
+
+  /**
+   * Triggers a stop of the controlLoop. This interface is used to stop all combined
+   * robots together when at one robot encounters an error.
+   */
+  void triggerError();
+
+  /**
+   * Getter for the error flag of the class.
+   *
+   * @return True in case of an error false otherwise.
+   */
+  bool hasError() const noexcept;
+
+  /**
+   * Recovers the libfranka robot, resets the error flag and publishes the error state.
+   */
+  void resetError();
+
+  /**
+   * Returns whether the controller needs to be reset e.g. after error recovery.
+   *
+   * @return A copy of the controller_needs_reset flag.
+   */
+  bool controllerNeedsReset() const noexcept;
+
+ private:
+  template <typename T>
+  T libfrankaUpdateCallback(const T& command,
+                            const franka::RobotState& robot_state,
+                            franka::Duration time_step) {
+    if (commandHasNaN(command)) {
+      std::string error_message = "FrankaCombinableHW: Got NaN value in command!";
+      ROS_FATAL("%s", error_message.c_str());
+      throw std::invalid_argument(error_message);
+    }
+    checkJointLimits();
+    libfranka_state_mutex_.lock();
+    robot_state_libfranka_ = robot_state;
+    libfranka_state_mutex_.unlock();
+    libfranka_cmd_mutex_.lock();
+    T current_cmd = command;
+    libfranka_cmd_mutex_.unlock();
+    if (has_error_ || !controller_active_) {
+      return franka::MotionFinished(current_cmd);
+    }
+    return current_cmd;
+  }
+
+  void publishErrorState(const bool error);
+
+  void setupServicesAndActionServers(ros::NodeHandle& node_handle);
+
+  void initRobot() override;
+
+  bool setRunFunction(const ControlMode& requested_control_mode,
+                      const bool limit_rate,
+                      const double cutoff_frequency,
+                      const franka::ControllerMode internal_controller) override;
+
+  void controlLoop();
+
+  static std::array<double, 7> saturateTorqueRate(const std::array<double, 7>& tau_d_calculated,
+                                                  const std::array<double, 7>& tau_J_d);
+
+  std::unique_ptr<std::thread> control_loop_thread_;
+  ServiceContainer services_;
+  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
+      recovery_action_server_;
+  std::atomic_bool has_error_{false};
+  ros::Publisher has_error_pub_;
+  std::atomic_bool error_recovered_{false};
+  std::atomic_bool controller_needs_reset_{false};
+};
+
+}  // namespace franka_hw
diff --git a/franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h b/franka_hw/include/franka_hw/franka_combined_hw.h
similarity index 86%
rename from franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h
rename to franka_hw/include/franka_hw/franka_combined_hw.h
index d1a733a956b9920426eafcd3e5dcfc1275effb1f..ea92d54a333bc3b0836da371390f35efd2cf7e0c 100644
--- a/franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h
+++ b/franka_hw/include/franka_hw/franka_combined_hw.h
@@ -3,8 +3,8 @@
 #pragma once
 
 #include <combined_robot_hw/combined_robot_hw.h>
-#include <franka_combinable_hw/franka_combinable_hw.h>
-#include <franka_control/ErrorRecoveryAction.h>
+#include <franka_hw/franka_combinable_hw.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 
 #include <actionlib/server/simple_action_server.h>
 #include <ros/node_handle.h>
@@ -12,7 +12,7 @@
 
 #include <memory>
 
-namespace franka_combinable_hw {
+namespace franka_hw {
 
 class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW {
  public:
@@ -51,7 +51,7 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW {
   bool controllerNeedsReset();
 
  protected:
-  std::unique_ptr<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>
+  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
       combined_recovery_action_server_;
 
  private:
@@ -61,4 +61,4 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW {
   bool is_recovering_{false};
 };
 
-}  // namespace franka_combinable_hw
+}  // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h
index 99f1fc1e64fca8fef17b3b99ff4198b02f323dd3..0140d0228012869a9e8002f009bb8c42e10da53a 100644
--- a/franka_hw/include/franka_hw/franka_hw.h
+++ b/franka_hw/include/franka_hw/franka_hw.h
@@ -4,6 +4,7 @@
 
 #include <array>
 #include <atomic>
+#include <exception>
 #include <functional>
 #include <list>
 #include <string>
@@ -13,11 +14,6 @@
 #include <franka/rate_limiting.h>
 #include <franka/robot.h>
 #include <franka/robot_state.h>
-#include <franka_hw/control_mode.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
-#include <franka_hw/resource_helpers.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
@@ -26,67 +22,75 @@
 #include <ros/time.h>
 #include <urdf/model.h>
 
+#include <franka_hw/control_mode.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+#include <franka_hw/resource_helpers.h>
+
 namespace franka_hw {
 
+/**
+ * This class wraps the functionality of libfranka for controlling Panda robots into the ros_control
+ * framework.
+ */
 class FrankaHW : public hardware_interface::RobotHW {
  public:
-  FrankaHW() = delete;
-
-  /**
-   * Creates an instance of FrankaHW that does not provide a model interface.
-   *
-   * @param[in] joint_names An array of joint names being controlled.
-   * @param[in] arm_id Unique identifier for the robot being controlled.
-   * @param[in] urdf_model A URDF model to initialize joint limits from.
-   * @param[in] get_limit_rate Getter that should return true if the rate limiter
-   * should be used, false otherwise. Defaults to true.
-   * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter.
-   * Defaults to franka::kDefaultCutoffFrequency.
-   * @param[in] get_internal_controller Getter for an internal controller to
-   * be used for control loops using only motion generation. Defaults to joint impedance.
-   */
-  FrankaHW(const std::array<std::string, 7>& joint_names,
-           const std::string& arm_id,
-           const urdf::Model& urdf_model,
-           std::function<bool()> get_limit_rate = []() { return true; },
-           std::function<double()> get_cutoff_frequency =
-               []() { return franka::kDefaultCutoffFrequency; },
-           std::function<franka::ControllerMode()> get_internal_controller =
-               []() { return franka::ControllerMode::kJointImpedance; });
-
-  /**
-   * Creates an instance of FrankaHW that provides a model interface.
-   *
-   * @param[in] joint_names An array of joint names being controlled.
-   * @param[in] arm_id Unique identifier for the robot being controlled.
-   * @param[in] urdf_model A URDF model to initialize joint limits from.
-   * @param[in] model Robot model.
-   * @param[in] get_limit_rate Getter that should return true if the rate limiter
-   * should be used, false otherwise. Defaults to true.
-   * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter.
-   * Defaults to franka::kDefaultCutoffFrequency.
-   * @param[in] get_internal_controller Getter for an internal controller to
-   * be used for control loops using only motion generation. Defaults to joint impedance.
-   */
-  FrankaHW(const std::array<std::string, 7>& joint_names,
-           const std::string& arm_id,
-           const urdf::Model& urdf_model,
-           franka::Model& model,
-           std::function<bool()> get_limit_rate = []() { return true; },
-           std::function<double()> get_cutoff_frequency =
-               []() { return franka::kDefaultCutoffFrequency; },
-           std::function<franka::ControllerMode()> get_internal_controller =
-               []() { return franka::ControllerMode::kJointImpedance; });
+  /**
+   * Default constructor.
+   * Note: Be sure to call the init() method before operation.
+   */
+  FrankaHW();
 
   virtual ~FrankaHW() override = default;
 
+  /**
+   * Initializes the FrankaHW class to be fully operational. This involves parsing required
+   * configurations from the ROS parameter server, connecting to the robot and setting up interfaces
+   * for the ros_control framework.
+   *
+   * @param[in] root_nh A node handle in the root namespace of the control node.
+   * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware.
+   *
+   * @return True if successful, false otherwise.
+   */
+  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
+
+  /**
+   * Reads the parameterizsation of the hardware class from the ROS parameter server
+   * (e.g. arm_id, robot_ip joint_names etc.)
+   *
+   * @param[in] root_nh A node handle in the root namespace of the control node.
+   * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware.
+   *
+   * @return True if successful, false otherwise.
+   */
+  virtual bool initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);
+
+  /**
+   * Initializes the class in terms of ros_control interfaces.
+   * Note: You have to call initParameters beforehand. Use the complete initialization routine
+   * \ref init() method to control robots.
+   *
+   * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware.
+   * @return True if successful, false otherwise.
+   */
+  virtual void initROSInterfaces(ros::NodeHandle& robot_hw_nh);
+
+  /**
+   * Initializes the callbacks for on-the-fly reading the parameters for rate limiting,
+   * internal controllers and cutoff frequency.
+   *
+   * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware.
+   */
+  virtual void setupParameterCallbacks(ros::NodeHandle& robot_hw_nh);
+
   /**
    * Runs the currently active controller in a realtime loop.
    *
    * If no controller is active, the function immediately exits. When running a controller,
    * the function only exits when ros_callback returns false.
    *
-   * @param[in] robot Robot instance.
    * @param[in] ros_callback A callback function that is executed at each time step and runs
    * all ROS-side functionality of the hardware. Execution is stopped if it returns false.
    *
@@ -97,16 +101,12 @@ class FrankaHW : public hardware_interface::RobotHW {
    * @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
    */
   virtual void control(
-      franka::Robot& robot,
-      const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback);
+      const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) const;
 
   /**
    * Updates the controller interfaces from the given robot state.
    *
    * @param[in] robot_state Current robot state.
-   *
-   * @throw franka::InvalidOperationException if a conflicting operation is already running.
-   * @throw franka::NetworkException if the connection is lost.
    */
   virtual void update(const franka::RobotState& robot_state);
 
@@ -131,8 +131,9 @@ class FrankaHW : public hardware_interface::RobotHW {
   /**
    * Performs controller switching (real-time capable).
    */
-  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>&,
-                        const std::list<hardware_interface::ControllerInfo>&) override;
+  virtual void doSwitch(
+      const std::list<hardware_interface::ControllerInfo>& /*start_list*/,
+      const std::list<hardware_interface::ControllerInfo>& /*stop_list*/) override;
 
   /**
    * Prepares switching between controllers (not real-time capable).
@@ -149,28 +150,28 @@ class FrankaHW : public hardware_interface::RobotHW {
   /**
    * Gets the current joint position command.
    *
-   * @return Current joint position command.
+   * @return The current joint position command.
    */
   virtual std::array<double, 7> getJointPositionCommand() const noexcept;
 
   /**
    * Gets the current joint velocity command.
    *
-   * @return Current joint velocity command.
+   * @return The current joint velocity command.
    */
   virtual std::array<double, 7> getJointVelocityCommand() const noexcept;
 
   /**
    * Gets the current joint torque command.
    *
-   * @return Current joint torque command.
+   * @return The current joint torque command.
    */
   virtual std::array<double, 7> getJointEffortCommand() const noexcept;
 
   /**
    * Enforces limits on position, velocity, and torque level.
    *
-   * @param[in] period Duration of the current cycle.
+   * @param[in] period The duration of the current cycle.
    */
   virtual void enforceLimits(const ros::Duration& period);
 
@@ -185,29 +186,137 @@ class FrankaHW : public hardware_interface::RobotHW {
    */
   virtual void checkJointLimits();
 
+  /**
+   * Reads data from the franka robot.
+   *
+   * @param[in] time The current time.
+   * @param[in] period The time passed since the last call to \ref read.
+   */
+  virtual void read(const ros::Time& time, const ros::Duration& period) override;
+
+  /**
+   * Writes data to the franka robot.
+   *
+   * @param[in] time The current time.
+   * @param[in] period The time passed since the last call to \ref write.
+   */
+  virtual void write(const ros::Time& time, const ros::Duration& period) override;
+
+  /**
+   * Getter for the libfranka robot instance.
+   */
+  virtual franka::Robot& robot() const;
+
+  /**
+   * Checks a command for NaN values.
+   *
+   * @param[in] command  The command to check.
+   *
+   * @return True if the command contains NaN, false otherwise.
+   */
+  static bool commandHasNaN(const franka::Torques& command);
+
+  /**
+   * Checks a command for NaN values.
+   *
+   * @param[in] command The command to check.
+   *
+   * @return True if the command contains NaN, false otherwise.
+   */
+  static bool commandHasNaN(const franka::JointPositions& command);
+
+  /**
+   * Checks a command for NaN values.
+   *
+   * @param[in] command The command to check.
+   *
+   * @return True if the command contains NaN, false otherwise.
+   */
+  static bool commandHasNaN(const franka::JointVelocities& command);
+
+  /**
+   * Checks a command for NaN values.
+   *
+   * @param[in] command  The command to check.
+   *
+   * @return True if the command contains NaN, false otherwise.
+   */
+  static bool commandHasNaN(const franka::CartesianPose& command);
+
+  /**
+   * Checks a command for NaN values.
+   *
+   * @param[in] command  The command to check.
+   *
+   * @return True if the command contains NaN, false otherwise.
+   */
+  static bool commandHasNaN(const franka::CartesianVelocities& command);
+
  protected:
+  /**
+   * Checks whether an array of doubles contains NaN values.
+   *
+   * @param[in] command  array The array to check.
+   *
+   * @return True if the array contains NaN values, false otherwise.
+   */
+  template <size_t size>
+  static bool arrayHasNaN(const std::array<double, size> array) {
+    return std::any_of(array.begin(), array.end(), [](const double& e) { return std::isnan(e); });
+  }
+
   using Callback = std::function<bool(const franka::RobotState&, franka::Duration)>;
+
+  /**
+   * Callback for the libfranka control loop. This method is designed to incorporate a
+   * second callback named ros_callback that will be called on each iteration of the callback.
+   *
+   * @param[in] command The datafield containing the command to send to the robot.
+   * @param[in] ros_callback An additional callback that is executed every time step.
+   * @param[in] robot_state The current robot state to compute commands with.
+   * @param[in] time_step Time since last call to the callback.
+   * @throw std::invalid_argument When a command contains NaN values.
+   *
+   * @return The command to be sent to the robot via libfranka.
+   */
   template <typename T>
   T controlCallback(const T& command,
                     Callback ros_callback,
                     const franka::RobotState& robot_state,
                     franka::Duration time_step) {
-    robot_state_ = robot_state;
+    robot_state_libfranka_ = robot_state;
+    ros::Time now = ros::Time(0);
+    read(now, ros::Duration(time_step.toSec()));
+
     if (!controller_active_ || (ros_callback && !ros_callback(robot_state, time_step))) {
       return franka::MotionFinished(command);
     }
+
+    write(now, ros::Duration(time_step.toSec()));
+    if (commandHasNaN(command)) {
+      std::string error_message = "FrankaHW::controlCallback: Got NaN command!";
+      ROS_FATAL("%s", error_message.c_str());
+      throw std::invalid_argument(error_message);
+    }
+
     return command;
   }
 
+  /**
+   * Configures a limit interface to enforce limits on effort, velocity or position level
+   * on joint commands.
+   *
+   * @param[out] limit_interface The limit interface to set up.
+   * @param[out] command_interface The command interface to hook the limit interface to.
+   */
   template <typename T>
-  void setupLimitInterface(const urdf::Model& urdf_model,
-                           joint_limits_interface::JointLimitsInterface<T>& limit_interface,
+  void setupLimitInterface(joint_limits_interface::JointLimitsInterface<T>& limit_interface,
                            hardware_interface::JointCommandInterface& command_interface) {
     joint_limits_interface::SoftJointLimits soft_limits;
     joint_limits_interface::JointLimits joint_limits;
     for (size_t i = 0; i < joint_names_.size(); i++) {
       const std::string& joint_name = joint_names_[i];
-      auto urdf_joint = urdf_model.getJoint(joint_name);
+      auto urdf_joint = urdf_model_.getJoint(joint_name);
       if (!urdf_joint || !urdf_joint->safety || !urdf_joint->limits) {
         ROS_WARN(
             "FrankaHW: Joint %s has incomplete limits and safety specs. Skipping it in the joint "
@@ -236,8 +345,22 @@ class FrankaHW : public hardware_interface::RobotHW {
     }
   }
 
+  /**
+   * Configures and registers the joint state interface in ros_control.
+   *
+   * @param[in] robot_state The data field holding the updated robot state.
+   */
   virtual void setupJointStateInterface(franka::RobotState& robot_state);
 
+  /**
+   * Configures and registers a joint command interface for positions velocities or efforts in
+   * ros_control.
+   *
+   * @param[in] command The data field holding the command to execute.
+   * @param[in] state The data field holding the updated robot state.
+   * @param[in] use_q_d Flag to configure using desired values as joint_states.
+   * @param[out] interface The command interface to configure.
+   */
   template <typename T>
   void setupJointCommandInterface(std::array<double, 7>& command,
                                   franka::RobotState& state,
@@ -258,22 +381,86 @@ class FrankaHW : public hardware_interface::RobotHW {
     registerInterface(&interface);
   }
 
+  /**
+   * Configures and registers the state interface offering the full franka::RobotState in
+   * ros_control.
+   *
+   * @param[in] robot_state The data field holding the updated robot state.
+   */
   virtual void setupFrankaStateInterface(franka::RobotState& robot_state);
 
+  /**
+   * Configures and registers the command interface for Cartesian poses in ros_control.
+   *
+   * @param[in] pose_cartesian_command The data field holding the command to execute.
+   */
   virtual void setupFrankaCartesianPoseInterface(franka::CartesianPose& pose_cartesian_command);
 
+  /**
+   * Configures and registers the command interface for Cartesian velocities in ros_control.
+   *
+   * @param[in] velocity_cartesian_command The data field holding the command to execute.
+   */
   virtual void setupFrankaCartesianVelocityInterface(
       franka::CartesianVelocities& velocity_cartesian_command);
 
-  virtual void setupFrankaModelInterface(franka::Model& model, franka::RobotState& robot_state);
+  /**
+   * Configures and registers the model interface offering kinematics and dynamics in ros_control.
+   *
+   * @param[in] robot_state A reference to the data field storing the current robot state. This
+   * state is used to evaluate model qunatities (by default) at the current state.
+   */
+  virtual void setupFrankaModelInterface(franka::RobotState& robot_state);
 
+  /**
+   * Configures the run function which is used as libfranka control callback based on the requested
+   * control mode.
+   *
+   * @param[in] requested_control_mode The control mode to configure (e.g. torque/position/velocity
+   * etc.)
+   * @param[in] limit_rate Flag to enable/disable rate limiting to smoothen the commands.
+   * @param[in] cutoff_frequency The cutoff frequency applied for command smoothing.
+   * @param[in] internal_controller The internal controller to use when using position or velocity
+   * modes.
+   *
+   * @return True if successful, false otherwise.
+   */
   virtual bool setRunFunction(const ControlMode& requested_control_mode,
                               const bool limit_rate,
                               const double cutoff_frequency,
                               const franka::ControllerMode internal_controller);
+  /**
+   * Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model.
+   */
+  virtual void initRobot();
 
-  virtual void init();
-
+  /**
+   * Parses a set of collision thresholds from the parameter server. The methods returns
+   * the default values if no parameter was found or the size of the array did not match
+   * the defaults dimension.
+   *
+   * @param[in] name The name of the parameter to look for.
+   * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware.
+   * @param[in] defaults A set of default values that also specify the size the parameter must have
+   * to be valid.
+   * @return A set parsed parameters if valid parameters where found, the default values otherwise.
+   */
+  static std::vector<double> getCollisionThresholds(const std::string& name,
+                                                    ros::NodeHandle& robot_hw_nh,
+                                                    const std::vector<double>& defaults);
+
+  struct CollisionConfig {
+    std::array<double, 7> lower_torque_thresholds_acceleration;
+    std::array<double, 7> upper_torque_thresholds_acceleration;
+    std::array<double, 7> lower_torque_thresholds_nominal;
+    std::array<double, 7> upper_torque_thresholds_nominal;
+    std::array<double, 6> lower_force_thresholds_acceleration;
+    std::array<double, 6> upper_force_thresholds_acceleration;
+    std::array<double, 6> lower_force_thresholds_nominal;
+    std::array<double, 6> upper_force_thresholds_nominal;
+  };
+
+  CollisionConfig collision_config_;
   hardware_interface::JointStateInterface joint_state_interface_{};
   FrankaStateInterface franka_state_interface_{};
   hardware_interface::PositionJointInterface position_joint_interface_{};
@@ -286,27 +473,42 @@ class FrankaHW : public hardware_interface::RobotHW {
   joint_limits_interface::VelocityJointSoftLimitsInterface velocity_joint_limit_interface_{};
   joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limit_interface_{};
 
-  franka::RobotState robot_state_{};
+  std::mutex libfranka_state_mutex_;
+  std::mutex ros_state_mutex_;
+  franka::RobotState robot_state_libfranka_{};
+  franka::RobotState robot_state_ros_{};
 
-  std::array<std::string, 7> joint_names_;
-  const std::string arm_id_;
-  std::function<franka::ControllerMode()> get_internal_controller_;
-  std::function<bool()> get_limit_rate_;
-  std::function<double()> get_cutoff_frequency_;
+  std::mutex libfranka_cmd_mutex_;
+  franka::JointPositions position_joint_command_libfranka_;
+  franka::JointVelocities velocity_joint_command_libfranka_;
+  franka::Torques effort_joint_command_libfranka_;
+  franka::CartesianPose pose_cartesian_command_libfranka_;
+  franka::CartesianVelocities velocity_cartesian_command_libfranka_;
 
-  franka::JointPositions position_joint_command_;
-  franka::JointVelocities velocity_joint_command_;
-  franka::Torques effort_joint_command_;
-  franka::CartesianPose pose_cartesian_command_;
-  franka::CartesianVelocities velocity_cartesian_command_;
+  std::mutex ros_cmd_mutex_;
+  franka::JointPositions position_joint_command_ros_;
+  franka::JointVelocities velocity_joint_command_ros_;
+  franka::Torques effort_joint_command_ros_;
+  franka::CartesianPose pose_cartesian_command_ros_;
+  franka::CartesianVelocities velocity_cartesian_command_ros_;
 
-  std::function<void(franka::Robot&, Callback)> run_function_;
+  std::unique_ptr<franka::Robot> robot_;
+  std::unique_ptr<franka::Model> model_;
 
+  std::array<std::string, 7> joint_names_;
+  std::string arm_id_;
+  std::string robot_ip_;
   urdf::Model urdf_model_;
   double joint_limit_warning_threshold_{0.1};
 
+  bool initialized_{false};
   std::atomic_bool controller_active_{false};
   ControlMode current_control_mode_ = ControlMode::None;
+
+  std::function<franka::ControllerMode()> get_internal_controller_;
+  std::function<bool()> get_limit_rate_;
+  std::function<double()> get_cutoff_frequency_;
+  std::function<void(franka::Robot&, Callback)> run_function_;
 };
 
 }  // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/resource_helpers.h b/franka_hw/include/franka_hw/resource_helpers.h
index 4f2b2440bd7453469d96fbe4ca1796cb86d3fcd4..f2231ba58d3d3867f2942b6e22224d19087e6e46 100644
--- a/franka_hw/include/franka_hw/resource_helpers.h
+++ b/franka_hw/include/franka_hw/resource_helpers.h
@@ -40,4 +40,6 @@ bool hasConflictingJointAndCartesianClaim(const ArmClaimedMap& arm_claim_map,
 
 bool partiallyClaimsArmJoints(const ArmClaimedMap& arm_claim_map, const std::string& arm_id);
 
+bool hasTrajectoryClaim(const ArmClaimedMap& arm_claim_map, const std::string& arm_id);
+
 }  // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/services.h b/franka_hw/include/franka_hw/services.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7b3c55e4e0e722bbc37e5df5b7e0305397d8e70
--- /dev/null
+++ b/franka_hw/include/franka_hw/services.h
@@ -0,0 +1,161 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#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_msgs/SetCartesianImpedance.h>
+#include <franka_msgs/SetEEFrame.h>
+#include <franka_msgs/SetForceTorqueCollisionBehavior.h>
+#include <franka_msgs/SetFullCollisionBehavior.h>
+#include <franka_msgs/SetJointImpedance.h>
+#include <franka_msgs/SetKFrame.h>
+#include <franka_msgs/SetLoad.h>
+
+#include <vector>
+
+namespace franka_hw {
+
+/**
+ * Advertises a service that acts according to the handler function which is used in the service
+ * callback.
+ *
+ * @param[in] node_handle The NodeHandle in the namespace at which to advertise the service.
+ * @param[in] name The name of the service.
+ * @param[in] handler The callback method for the service.
+ * @return The service server.
+ */
+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;
+      });
+}
+
+/**
+ * This class serves as container that gathers all possible service interfaces to a libfranka robot
+ * instance.
+ */
+class ServiceContainer {
+ public:
+  /**
+   * Advertises and adds a service to the container class.
+   *
+   * @return A reference to the service container to allow a fluent API.
+   */
+  template <typename T, typename... TArgs>
+  ServiceContainer& advertiseService(TArgs&&... args) {
+    ros::ServiceServer server = franka_hw::advertiseService<T>(std::forward<TArgs>(args)...);
+    services_.push_back(server);
+    return *this;
+  }
+
+ private:
+  std::vector<ros::ServiceServer> services_;
+};
+
+/**
+ * Sets up all services relevant for a libfranka robot inside a service container.
+ *
+ * @param[in] robot The libfranka robot for which to set up services interfaces.
+ * @param[in] node_handle The NodeHandle in the namespace at which to advertise the services.
+ * @param[in] services The container to store the service servers.
+ */
+void setupServices(franka::Robot& robot, ros::NodeHandle& node_handle, ServiceContainer& services);
+
+/**
+ * Callback for the service interface to franka::robot::setCartesianImpedance.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setCartesianImpedance(franka::Robot& robot,
+                           const franka_msgs::SetCartesianImpedance::Request& req,
+                           franka_msgs::SetCartesianImpedance::Response& res);
+
+/**
+ * Callback for the service interface to franka::robot::setJointImpedance.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setJointImpedance(franka::Robot& robot,
+                       const franka_msgs::SetJointImpedance::Request& req,
+                       franka_msgs::SetJointImpedance::Response& res);
+
+/**
+ * Callback for the service interface to franka::robot::setEEFrame.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setEEFrame(franka::Robot& robot,
+                const franka_msgs::SetEEFrame::Request& req,
+                franka_msgs::SetEEFrame::Response& res);
+
+/**
+ * Callback for the service interface to franka::robot::setKFrame.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setKFrame(franka::Robot& robot,
+               const franka_msgs::SetKFrame::Request& req,
+               franka_msgs::SetKFrame::Response& res);
+
+/**
+ * Callback for the service interface to franka::robot::setForceTorqueCollisionBehavior.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setForceTorqueCollisionBehavior(
+    franka::Robot& robot,
+    const franka_msgs::SetForceTorqueCollisionBehavior::Request& req,
+    franka_msgs::SetForceTorqueCollisionBehavior::Response& res);
+
+/**
+ * Callback for the service interface to franka::robot::setFullCollisionBehavior.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setFullCollisionBehavior(franka::Robot& robot,
+                              const franka_msgs::SetFullCollisionBehavior::Request& req,
+                              franka_msgs::SetFullCollisionBehavior::Response& res);
+
+/**
+ * Callback for the service interface to franka::robot::setLoad.
+ *
+ * @param[in] robot The libfranka robot for which to set up the service.
+ * @param[in] req The service request.
+ * @param[out] res The service response.
+ */
+void setLoad(franka::Robot& robot,
+             const franka_msgs::SetLoad::Request& req,
+             franka_msgs::SetLoad::Response& res);
+
+}  // namespace franka_hw
diff --git a/franka_hw/package.xml b/franka_hw/package.xml
index 0ff25b93cf76894735bb1950e059774ef0a374d6..a61cec93a6301996d8073a7fe3c23025c099bc52 100644
--- a/franka_hw/package.xml
+++ b/franka_hw/package.xml
@@ -13,14 +13,25 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>message_generation</build_depend>
+
+  <depend>actionlib_msgs</depend>
+  <depend>actionlib</depend>
   <depend>controller_interface</depend>
+  <depend>combined_robot_hw</depend>
   <depend>hardware_interface</depend>
   <depend>joint_limits_interface</depend>
   <depend>libfranka</depend>
   <depend>roscpp</depend>
   <depend>urdf</depend>
+  <depend>pluginlib</depend>
+  <depend>franka_msgs</depend>
 
   <test_depend>franka_description</test_depend>
   <test_depend>gtest</test_depend>
   <test_depend>rostest</test_depend>
+
+  <export>
+      <hardware_interface plugin="${prefix}/franka_combinable_hw_plugin.xml"/>
+  </export>
 </package>
diff --git a/franka_hw/src/franka_combinable_hw.cpp b/franka_hw/src/franka_combinable_hw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1451b244deb1f39e7165591c1bea912343076e95
--- /dev/null
+++ b/franka_hw/src/franka_combinable_hw.cpp
@@ -0,0 +1,226 @@
+
+#include <hardware_interface/joint_command_interface.h>
+#include <joint_limits_interface/joint_limits_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <std_msgs/Bool.h>
+
+#include <franka_hw/franka_combinable_hw.h>
+#include <franka_hw/services.h>
+
+namespace franka_hw {
+
+FrankaCombinableHW::FrankaCombinableHW() : has_error_(false), error_recovered_(false) {}
+
+void FrankaCombinableHW::initROSInterfaces(ros::NodeHandle& robot_hw_nh) {
+  setupJointStateInterface(robot_state_ros_);
+  setupJointCommandInterface(effort_joint_command_ros_.tau_J, robot_state_ros_, false,
+                             effort_joint_interface_);
+  setupLimitInterface<joint_limits_interface::EffortJointSoftLimitsHandle>(
+      effort_joint_limit_interface_, effort_joint_interface_);
+  setupFrankaStateInterface(robot_state_ros_);
+  setupFrankaModelInterface(robot_state_ros_);
+
+  has_error_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("has_error", 1, true);
+  publishErrorState(has_error_);
+
+  setupServicesAndActionServers(robot_hw_nh);
+}
+
+void FrankaCombinableHW::initRobot() {
+  FrankaHW::initRobot();
+  control_loop_thread_ = std::make_unique<std::thread>(&FrankaCombinableHW::controlLoop, this);
+}
+
+void FrankaCombinableHW::publishErrorState(const bool error) {
+  std_msgs::Bool msg;
+  msg.data = static_cast<int>(error);
+  has_error_pub_.publish(msg);
+}
+
+void FrankaCombinableHW::controlLoop() {
+  while (ros::ok()) {
+    ros::Time last_time = ros::Time::now();
+
+    // Wait until controller has been activated or error has been recovered
+    while (!controllerActive() || has_error_) {
+      if (!controllerActive()) {
+        ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): controller is not active.",
+                           arm_id_.c_str());
+      }
+      if (has_error_) {
+        ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): an error has occured.",
+                           arm_id_.c_str());
+      }
+
+      checkJointLimits();
+
+      ros_state_mutex_.lock();
+      libfranka_state_mutex_.lock();
+      robot_state_libfranka_ = robot_->readOnce();
+      robot_state_ros_ = robot_->readOnce();
+      libfranka_state_mutex_.unlock();
+      ros_state_mutex_.unlock();
+      if (!ros::ok()) {
+        return;
+      }
+    }
+    ROS_INFO("FrankaCombinableHW::%s::control_loop(): controller is active.", arm_id_.c_str());
+
+    // Reset commands
+    libfranka_cmd_mutex_.lock();
+    effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.});
+    libfranka_cmd_mutex_.unlock();
+
+    try {
+      control();
+    } catch (const franka::ControlException& e) {
+      // Reflex could be caught and it needs to wait for automatic error recovery
+      ROS_ERROR("%s", e.what());
+      has_error_ = true;
+      publishErrorState(has_error_);
+    }
+  }
+}
+
+void FrankaCombinableHW::setupServicesAndActionServers(ros::NodeHandle& node_handle) {
+  setupServices(*robot_, node_handle, services_);
+  recovery_action_server_ =
+      std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>(
+          node_handle, "error_recovery",
+          [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
+            try {
+              robot_->automaticErrorRecovery();
+              // error recovered => reset controller
+              if (has_error_) {
+                error_recovered_ = true;
+              }
+              has_error_ = false;
+              publishErrorState(has_error_);
+              recovery_action_server_->setSucceeded();
+            } catch (const franka::Exception& ex) {
+              recovery_action_server_->setAborted(franka_msgs::ErrorRecoveryResult(), ex.what());
+            }
+          },
+          false);
+  recovery_action_server_->start();
+}
+
+void FrankaCombinableHW::control(  // NOLINT (google-default-arguments)
+    const std::function<bool(const ros::Time&, const ros::Duration&)>& /*ros_callback*/) const {
+  if (!controller_active_) {
+    return;
+  }
+  auto empty_method = [](const franka::RobotState&, franka::Duration) { return true; };
+  run_function_(*robot_, empty_method);
+}
+
+bool FrankaCombinableHW::checkForConflict(
+    const std::list<hardware_interface::ControllerInfo>& info) const {
+  ResourceWithClaimsMap resource_map = getResourceMap(info);
+
+  if (hasConflictingMultiClaim(resource_map)) {
+    return true;
+  }
+
+  ArmClaimedMap arm_claim_map;
+  if (!getArmClaimedMap(resource_map, arm_claim_map)) {
+    ROS_ERROR("FrankaCombinableHW: Unknown interface claimed. Conflict!");
+    return true;
+  }
+
+  // check for any claim to trajectory interfaces (non-torque) which are not supported.
+  if (hasTrajectoryClaim(arm_claim_map, arm_id_)) {
+    ROS_ERROR_STREAM("FrankaCombinableHW: Invalid claim joint position or velocity interface."
+                     << "Note: joint position and joint velocity interfaces are not supported"
+                     << " in FrankaCombinableHW. Arm:" << arm_id_ << ". Conflict!");
+    return true;
+  }
+
+  return partiallyClaimsArmJoints(arm_claim_map, arm_id_);
+}
+
+void FrankaCombinableHW::read(const ros::Time& time, const ros::Duration& period) {
+  controller_needs_reset_ = bool(error_recovered_);
+  FrankaHW::read(time, period);
+}
+
+void FrankaCombinableHW::write(const ros::Time& time, const ros::Duration& period) {
+  // if flag `controller_needs_reset_` was updated, then controller_manager. update(...,
+  // reset_controller) must
+  // have been executed to reset the controller.
+  if (controller_needs_reset_ && error_recovered_) {
+    controller_needs_reset_ = false;
+    error_recovered_ = false;
+  }
+
+  enforceLimits(period);
+
+  FrankaHW::write(time, period);
+}
+
+std::array<double, 7> FrankaCombinableHW::saturateTorqueRate(
+    const std::array<double, 7>& tau_d_calculated,
+    const std::array<double, 7>& tau_J_d) {  // NOLINT (readability-identifier-naming)
+  const double kDeltaTauMax = 1.0;
+  std::array<double, 7> tau_d_saturated{};
+  for (size_t i = 0; i < 7; i++) {
+    double difference = tau_d_calculated[i] - tau_J_d[i];
+    tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
+  }
+  return tau_d_saturated;
+}
+
+std::string FrankaCombinableHW::getArmID() const noexcept {
+  return arm_id_;
+}
+
+void FrankaCombinableHW::triggerError() {
+  has_error_ = true;
+  publishErrorState(has_error_);
+}
+
+bool FrankaCombinableHW::hasError() const noexcept {
+  return has_error_;
+}
+
+void FrankaCombinableHW::resetError() {
+  robot_->automaticErrorRecovery();
+  // error recovered => reset controller
+  if (has_error_) {
+    error_recovered_ = true;
+  }
+  has_error_ = false;
+  publishErrorState(has_error_);
+}
+
+bool FrankaCombinableHW::controllerNeedsReset() const noexcept {
+  return controller_needs_reset_;
+}
+
+bool FrankaCombinableHW::setRunFunction(const ControlMode& requested_control_mode,
+                                        const bool limit_rate,
+                                        const double cutoff_frequency,
+                                        const franka::ControllerMode /*internal_controller*/) {
+  using Callback = std::function<bool(const franka::RobotState&, franka::Duration)>;
+
+  if (requested_control_mode == ControlMode::None) {
+    return true;
+  }
+  if (requested_control_mode == ControlMode::JointTorque) {
+    run_function_ = [this, limit_rate, cutoff_frequency](franka::Robot& robot,
+                                                         Callback /*callback*/) {
+      robot.control(std::bind(&FrankaCombinableHW::libfrankaUpdateCallback<franka::Torques>, this,
+                              std::cref(effort_joint_command_libfranka_), std::placeholders::_1,
+                              std::placeholders::_2),
+                    limit_rate, cutoff_frequency);
+    };
+    return true;
+  }
+
+  ROS_ERROR("FrankaCombinableHW: No valid control mode selected; cannot set run function.");
+  return false;
+}
+
+}  // namespace franka_hw
+
+PLUGINLIB_EXPORT_CLASS(franka_hw::FrankaCombinableHW, hardware_interface::RobotHW)
diff --git a/franka_combinable_hw/src/franka_combined_hw.cpp b/franka_hw/src/franka_combined_hw.cpp
similarity index 78%
rename from franka_combinable_hw/src/franka_combined_hw.cpp
rename to franka_hw/src/franka_combined_hw.cpp
index 42921fb54e78db1af40f72e9f27cf77d515fe939..a2fce7257ae2baa4edea30cebe9b94ec6e499d71 100644
--- a/franka_combinable_hw/src/franka_combined_hw.cpp
+++ b/franka_hw/src/franka_combined_hw.cpp
@@ -1,10 +1,10 @@
 // Copyright (c) 2019 Franka Emika GmbH
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_combinable_hw/franka_combinable_hw.h>
+#include <franka_hw/franka_combinable_hw.h>
 
-#include <franka_combinable_hw/franka_combined_hw.h>
-#include <franka_control/ErrorRecoveryAction.h>
+#include <franka_hw/franka_combined_hw.h>
 #include <franka_hw/franka_hw.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 
 #include <actionlib/server/simple_action_server.h>
 #include <ros/node_handle.h>
@@ -13,7 +13,7 @@
 #include <algorithm>
 #include <memory>
 
-namespace franka_combinable_hw {
+namespace franka_hw {
 
 FrankaCombinedHW::FrankaCombinedHW() = default;
 
@@ -21,14 +21,14 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_
   bool success = CombinedRobotHW::init(root_nh, robot_hw_nh);
   // Error recovery server for all FrankaHWs
   combined_recovery_action_server_ =
-      std::make_unique<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>(
+      std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>(
           robot_hw_nh, "error_recovery",
-          [&](const franka_control::ErrorRecoveryGoalConstPtr&) {
+          [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
             try {
               is_recovering_ = true;
               for (const auto& robot_hw : robot_hw_list_) {
                 auto* franka_combinable_hw_ptr =
-                    dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get());
+                    dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get());
                 if (franka_combinable_hw_ptr != nullptr) {
                   franka_combinable_hw_ptr->resetError();
                 } else {
@@ -36,7 +36,7 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_
                       "FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed.");
                   is_recovering_ = false;
                   combined_recovery_action_server_->setAborted(
-                      franka_control::ErrorRecoveryResult(),
+                      franka_msgs::ErrorRecoveryResult(),
                       "dynamic_cast from RobotHW to FrankaCombinableHW failed");
                   return;
                 }
@@ -45,7 +45,7 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_
               combined_recovery_action_server_->setSucceeded();
             } catch (const franka::Exception& ex) {
               is_recovering_ = false;
-              combined_recovery_action_server_->setAborted(franka_control::ErrorRecoveryResult(),
+              combined_recovery_action_server_->setAborted(franka_msgs::ErrorRecoveryResult(),
                                                            ex.what());
             }
           },
@@ -64,8 +64,7 @@ bool FrankaCombinedHW::controllerNeedsReset() {
   // Check if any of the RobotHW object needs a controller reset
   bool controller_reset = false;
   for (const auto& robot_hw : robot_hw_list_) {
-    auto* franka_combinable_hw_ptr =
-        dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get());
+    auto* franka_combinable_hw_ptr = dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get());
     if (franka_combinable_hw_ptr != nullptr) {
       controller_reset = controller_reset || franka_combinable_hw_ptr->controllerNeedsReset();
     } else {
@@ -86,8 +85,7 @@ void FrankaCombinedHW::handleError() {
 bool FrankaCombinedHW::hasError() {
   bool has_error = false;
   for (const auto& robot_hw : robot_hw_list_) {
-    auto* franka_combinable_hw_ptr =
-        dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get());
+    auto* franka_combinable_hw_ptr = dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get());
     if (franka_combinable_hw_ptr != nullptr) {
       has_error = has_error || franka_combinable_hw_ptr->hasError();
     } else {
@@ -101,8 +99,7 @@ bool FrankaCombinedHW::hasError() {
 void FrankaCombinedHW::triggerError() {
   // Trigger error state of all RobotHW objects.
   for (const auto& robot_hw : robot_hw_list_) {
-    auto* franka_combinable_hw_ptr =
-        dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get());
+    auto* franka_combinable_hw_ptr = dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get());
     if (franka_combinable_hw_ptr != nullptr) {
       franka_combinable_hw_ptr->triggerError();
     } else {
@@ -111,4 +108,4 @@ void FrankaCombinedHW::triggerError() {
   }
 }
 
-}  // namespace franka_combinable_hw
+}  // namespace franka_hw
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index fe66d59f654c1f5e4f4d2ad2027320c7641574ff..c4cce12ab6c5bb9a7815e5d982152b39bfe8495e 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -5,8 +5,10 @@
 
 #include <array>
 #include <cstdint>
+#include <exception>
 #include <functional>
 #include <list>
+#include <mutex>
 #include <ostream>
 #include <sstream>
 #include <string>
@@ -40,45 +42,133 @@ std::string toStringWithPrecision(const double value, const size_t precision = 6
 
 }  // anonymous namespace
 
-FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
-                   const std::string& arm_id,
-                   const urdf::Model& urdf_model,
-                   std::function<bool()> get_limit_rate,
-                   std::function<double()> get_cutoff_frequency,
-                   std::function<franka::ControllerMode()> get_internal_controller)
-    : joint_names_(joint_names),
-      arm_id_(arm_id),
-      get_internal_controller_(std::move(get_internal_controller)),
-      get_limit_rate_(std::move(get_limit_rate)),
-      get_cutoff_frequency_(std::move(get_cutoff_frequency)),
-      position_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      velocity_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      effort_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      pose_cartesian_command_(
+FrankaHW::FrankaHW()
+    : position_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      position_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      velocity_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      velocity_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      effort_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      effort_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      pose_cartesian_command_ros_(
           {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
-      velocity_cartesian_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
-      urdf_model_(urdf_model) {
-  init();
-}
-
-FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
-                   const std::string& arm_id,
-                   const urdf::Model& urdf_model,
-                   franka::Model& model,
-                   std::function<bool()> get_limit_rate,
-                   std::function<double()> get_cutoff_frequency,
-                   std::function<franka::ControllerMode()> get_internal_controller)
-    : FrankaHW(joint_names,
-               arm_id,
-               urdf_model,
-               std::move(get_limit_rate),
-               std::move(get_cutoff_frequency),
-               std::move(get_internal_controller)) {
-  setupFrankaModelInterface(model, robot_state_);
+      pose_cartesian_command_libfranka_(
+          {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
+      velocity_cartesian_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
+      velocity_cartesian_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) {}
+
+bool FrankaHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
+  if (initialized_) {
+    ROS_ERROR("FrankaHW: Cannot be initialized twice.");
+    return false;
+  }
+
+  if (!initParameters(root_nh, robot_hw_nh)) {
+    ROS_ERROR("FrankaHW: Failed to parse all required parameters.");
+    return false;
+  }
+  try {
+    initRobot();
+  } catch (const std::runtime_error& error) {
+    ROS_ERROR("FrankaHW: Failed to initialize libfranka robot. %s", error.what());
+    return false;
+  }
+  initROSInterfaces(robot_hw_nh);
+  setupParameterCallbacks(robot_hw_nh);
+
+  initialized_ = true;
+  return true;
+}
+
+bool FrankaHW::initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
+  std::vector<std::string> joint_names_vector;
+  if (!robot_hw_nh.getParam("joint_names", joint_names_vector) || joint_names_vector.size() != 7) {
+    ROS_ERROR("Invalid or no joint_names parameters provided");
+    return false;
+  }
+  std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names_.begin());
+
+  bool rate_limiting;
+  if (!robot_hw_nh.getParamCached("rate_limiting", rate_limiting)) {
+    ROS_ERROR("Invalid or no rate_limiting parameter provided");
+    return false;
+  }
+
+  double cutoff_frequency;
+  if (!robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency)) {
+    ROS_ERROR("Invalid or no cutoff_frequency parameter provided");
+    return false;
+  }
+
+  std::string internal_controller;
+  if (!robot_hw_nh.getParam("internal_controller", internal_controller)) {
+    ROS_ERROR("No internal_controller parameter provided");
+    return false;
+  }
+
+  if (!robot_hw_nh.getParam("arm_id", arm_id_)) {
+    ROS_ERROR("Invalid or no arm_id parameter provided");
+    return false;
+  }
+
+  if (!urdf_model_.initParamWithNodeHandle("robot_description", root_nh)) {
+    ROS_ERROR("Could not initialize URDF model from robot_description");
+    return false;
+  }
+
+  if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) {
+    ROS_ERROR("Invalid or no robot_ip parameter provided");
+    return false;
+  }
+
+  if (!robot_hw_nh.getParam("joint_limit_warning_threshold", joint_limit_warning_threshold_)) {
+    ROS_INFO(
+        "No parameter joint_limit_warning_threshold is found, using default "
+        "value %f",
+        joint_limit_warning_threshold_);
+  }
+
+  // Get full collision behavior config from the parameter server.
+  std::vector<double> thresholds =
+      getCollisionThresholds("lower_torque_thresholds_acceleration", robot_hw_nh,
+                             {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.lower_torque_thresholds_acceleration.begin());
+  thresholds = getCollisionThresholds("upper_torque_thresholds_acceleration", robot_hw_nh,
+                                      {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.upper_torque_thresholds_acceleration.begin());
+  thresholds = getCollisionThresholds("lower_torque_thresholds_nominal", robot_hw_nh,
+                                      {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.lower_torque_thresholds_nominal.begin());
+  thresholds = getCollisionThresholds("upper_torque_thresholds_nominal", robot_hw_nh,
+                                      {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.upper_torque_thresholds_nominal.begin());
+  thresholds.resize(6);
+  thresholds = getCollisionThresholds("lower_force_thresholds_acceleration", robot_hw_nh,
+                                      {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.lower_force_thresholds_acceleration.begin());
+  thresholds = getCollisionThresholds("upper_force_thresholds_acceleration", robot_hw_nh,
+                                      {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.upper_force_thresholds_acceleration.begin());
+  thresholds = getCollisionThresholds("lower_force_thresholds_nominal", robot_hw_nh,
+                                      {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.lower_force_thresholds_nominal.begin());
+  thresholds = getCollisionThresholds("upper_force_thresholds_nominal", robot_hw_nh,
+                                      {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
+  std::copy(thresholds.begin(), thresholds.end(),
+            collision_config_.upper_force_thresholds_nominal.begin());
+
+  return true;
 }
 
 void FrankaHW::update(const franka::RobotState& robot_state) {
-  robot_state_ = robot_state;
+  std::lock_guard<std::mutex> ros_lock(ros_state_mutex_);
+  robot_state_ros_ = robot_state;
 }
 
 bool FrankaHW::controllerActive() const noexcept {
@@ -86,18 +176,22 @@ bool FrankaHW::controllerActive() const noexcept {
 }
 
 void FrankaHW::control(
-    franka::Robot& robot,
-    const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) {
+    const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) const {
+  if (!initialized_) {
+    ROS_ERROR("FrankaHW: Call to control before initialization!");
+    return;
+  }
   if (!controller_active_) {
     return;
   }
 
-  franka::Duration last_time = robot_state_.time;
+  franka::Duration last_time = robot_state_ros_.time;
 
-  run_function_(robot, [this, ros_callback, &last_time](const franka::RobotState& robot_state,
-                                                        franka::Duration time_step) {
+  run_function_(*robot_, [this, ros_callback, &last_time](const franka::RobotState& robot_state,
+                                                          franka::Duration time_step) {
     if (last_time != robot_state.time) {
       last_time = robot_state.time;
+
       return ros_callback(ros::Time::now(), ros::Duration(time_step.toSec()));
     }
     return true;
@@ -144,6 +238,7 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
     ROS_ERROR("FrankaHW: Unknown interface claimed for starting!");
     return false;
   }
+
   ControlMode start_control_mode = getControlMode(arm_id_, start_arm_claim_map);
 
   ResourceWithClaimsMap stop_resource_map = getResourceMap(stop_list);
@@ -162,6 +257,7 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
                       get_internal_controller_())) {
     return false;
   }
+
   if (current_control_mode_ != requested_control_mode) {
     ROS_INFO_STREAM("FrankaHW: Prepared switching controllers to "
                     << requested_control_mode << " with parameters "
@@ -176,15 +272,15 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
 }
 
 std::array<double, 7> FrankaHW::getJointPositionCommand() const noexcept {
-  return position_joint_command_.q;
+  return position_joint_command_ros_.q;
 }
 
 std::array<double, 7> FrankaHW::getJointVelocityCommand() const noexcept {
-  return velocity_joint_command_.dq;
+  return velocity_joint_command_ros_.dq;
 }
 
 std::array<double, 7> FrankaHW::getJointEffortCommand() const noexcept {
-  return effort_joint_command_.tau_J;
+  return effort_joint_command_ros_.tau_J;
 }
 
 void FrankaHW::reset() {
@@ -225,6 +321,31 @@ void FrankaHW::checkJointLimits() {
   }
 }
 
+franka::Robot& FrankaHW::robot() const {
+  if (!initialized_ || !robot_) {
+    std::string error_message = "FrankaHW: Atempt to access robot before initialization!";
+    ROS_ERROR("%s", error_message.c_str());
+    throw std::logic_error(error_message);
+  }
+  return *robot_;
+}
+
+void FrankaHW::read(const ros::Time& /*time*/, const ros::Duration& /*period*/) {
+  std::lock_guard<std::mutex> ros_lock(ros_state_mutex_);
+  std::lock_guard<std::mutex> libfranka_lock(libfranka_state_mutex_);
+  robot_state_ros_ = robot_state_libfranka_;
+}
+
+void FrankaHW::write(const ros::Time& /*time*/, const ros::Duration& /*period*/) {
+  std::lock_guard<std::mutex> ros_lock(ros_cmd_mutex_);
+  std::lock_guard<std::mutex> libfranka_lock(libfranka_cmd_mutex_);
+  pose_cartesian_command_libfranka_ = pose_cartesian_command_ros_;
+  velocity_cartesian_command_libfranka_ = velocity_cartesian_command_ros_;
+  effort_joint_command_libfranka_ = effort_joint_command_ros_;
+  position_joint_command_libfranka_ = position_joint_command_ros_;
+  velocity_joint_command_libfranka_ = velocity_joint_command_ros_;
+}
+
 void FrankaHW::setupJointStateInterface(franka::RobotState& robot_state) {
   for (size_t i = 0; i < joint_names_.size(); i++) {
     hardware_interface::JointStateHandle joint_handle_q(joint_names_[i], &robot_state.q[i],
@@ -257,10 +378,12 @@ void FrankaHW::setupFrankaCartesianVelocityInterface(
   registerInterface(&franka_velocity_cartesian_interface_);
 }
 
-void FrankaHW::setupFrankaModelInterface(franka::Model& model, franka::RobotState& robot_state) {
-  franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", model, robot_state);
-  franka_model_interface_.registerHandle(model_handle);
-  registerInterface(&franka_model_interface_);
+void FrankaHW::setupFrankaModelInterface(franka::RobotState& robot_state) {
+  if (model_) {
+    franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", *model_, robot_state);
+    franka_model_interface_.registerHandle(model_handle);
+    registerInterface(&franka_model_interface_);
+  }
 }
 
 bool FrankaHW::setRunFunction(const ControlMode& requested_control_mode,
@@ -277,72 +400,74 @@ bool FrankaHW::setRunFunction(const ControlMode& requested_control_mode,
     case ControlMode::JointTorque:
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                                std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2),
                       limit_rate, cutoff_frequency);
       };
       break;
     case ControlMode::JointPosition:
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
-                                std::cref(position_joint_command_), ros_callback, _1, _2),
+                                std::cref(position_joint_command_libfranka_), ros_callback, _1, _2),
                       internal_controller, limit_rate, cutoff_frequency);
       };
       break;
     case ControlMode::JointVelocity:
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
-                                std::cref(velocity_joint_command_), ros_callback, _1, _2),
+                                std::cref(velocity_joint_command_libfranka_), ros_callback, _1, _2),
                       internal_controller, limit_rate, cutoff_frequency);
       };
       break;
     case ControlMode::CartesianPose:
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
-                                std::cref(pose_cartesian_command_), ros_callback, _1, _2),
+                                std::cref(pose_cartesian_command_libfranka_), ros_callback, _1, _2),
                       internal_controller, limit_rate, cutoff_frequency);
       };
       break;
     case ControlMode::CartesianVelocity:
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
-        robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
-                                std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
-                      internal_controller, limit_rate, cutoff_frequency);
+        robot.control(
+            std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
+                      std::cref(velocity_cartesian_command_libfranka_), ros_callback, _1, _2),
+            internal_controller, limit_rate, cutoff_frequency);
       };
       break;
     case (ControlMode::JointTorque | ControlMode::JointPosition):
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                                std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2),
                       std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
-                                std::cref(position_joint_command_), ros_callback, _1, _2),
+                                std::cref(position_joint_command_libfranka_), ros_callback, _1, _2),
                       limit_rate, cutoff_frequency);
       };
       break;
     case (ControlMode::JointTorque | ControlMode::JointVelocity):
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                                std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2),
                       std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
-                                std::cref(velocity_joint_command_), ros_callback, _1, _2),
+                                std::cref(velocity_joint_command_libfranka_), ros_callback, _1, _2),
                       limit_rate, cutoff_frequency);
       };
       break;
     case (ControlMode::JointTorque | ControlMode::CartesianPose):
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
         robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                                std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2),
                       std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
-                                std::cref(pose_cartesian_command_), ros_callback, _1, _2),
+                                std::cref(pose_cartesian_command_libfranka_), ros_callback, _1, _2),
                       limit_rate, cutoff_frequency);
       };
       break;
     case (ControlMode::JointTorque | ControlMode::CartesianVelocity):
       run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
-        robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2),
-                      std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
-                                std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
-                      limit_rate, cutoff_frequency);
+        robot.control(
+            std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
+                      std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2),
+            std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
+                      std::cref(velocity_cartesian_command_libfranka_), ros_callback, _1, _2),
+            limit_rate, cutoff_frequency);
       };
       break;
     default:
@@ -352,23 +477,104 @@ bool FrankaHW::setRunFunction(const ControlMode& requested_control_mode,
   return true;
 }
 
-void FrankaHW::init() {
-  setupJointStateInterface(robot_state_);
-  setupJointCommandInterface(position_joint_command_.q, robot_state_, true,
+void FrankaHW::initROSInterfaces(ros::NodeHandle& /*robot_hw_nh*/) {
+  setupJointStateInterface(robot_state_ros_);
+  setupJointCommandInterface(position_joint_command_ros_.q, robot_state_ros_, true,
                              position_joint_interface_);
-  setupJointCommandInterface(velocity_joint_command_.dq, robot_state_, true,
+  setupJointCommandInterface(velocity_joint_command_ros_.dq, robot_state_ros_, true,
                              velocity_joint_interface_);
-  setupJointCommandInterface(effort_joint_command_.tau_J, robot_state_, false,
+  setupJointCommandInterface(effort_joint_command_ros_.tau_J, robot_state_ros_, false,
                              effort_joint_interface_);
   setupLimitInterface<joint_limits_interface::PositionJointSoftLimitsHandle>(
-      urdf_model_, position_joint_limit_interface_, position_joint_interface_);
+      position_joint_limit_interface_, position_joint_interface_);
   setupLimitInterface<joint_limits_interface::VelocityJointSoftLimitsHandle>(
-      urdf_model_, velocity_joint_limit_interface_, velocity_joint_interface_);
+      velocity_joint_limit_interface_, velocity_joint_interface_);
   setupLimitInterface<joint_limits_interface::EffortJointSoftLimitsHandle>(
-      urdf_model_, effort_joint_limit_interface_, effort_joint_interface_);
-  setupFrankaStateInterface(robot_state_);
-  setupFrankaCartesianPoseInterface(pose_cartesian_command_);
-  setupFrankaCartesianVelocityInterface(velocity_cartesian_command_);
+      effort_joint_limit_interface_, effort_joint_interface_);
+  setupFrankaStateInterface(robot_state_ros_);
+  setupFrankaCartesianPoseInterface(pose_cartesian_command_ros_);
+  setupFrankaCartesianVelocityInterface(velocity_cartesian_command_ros_);
+  setupFrankaModelInterface(robot_state_ros_);
+}
+
+void FrankaHW::initRobot() {
+  robot_ = std::make_unique<franka::Robot>(robot_ip_);
+  model_ = std::make_unique<franka::Model>(robot_->loadModel());
+  robot_->setCollisionBehavior(collision_config_.lower_torque_thresholds_acceleration,
+                               collision_config_.upper_torque_thresholds_acceleration,
+                               collision_config_.lower_torque_thresholds_nominal,
+                               collision_config_.upper_torque_thresholds_nominal,
+                               collision_config_.lower_force_thresholds_acceleration,
+                               collision_config_.upper_force_thresholds_acceleration,
+                               collision_config_.lower_force_thresholds_nominal,
+                               collision_config_.upper_force_thresholds_nominal);
+  update(robot_->readOnce());
+}
+
+void FrankaHW::setupParameterCallbacks(ros::NodeHandle& robot_hw_nh) {
+  get_limit_rate_ = [robot_hw_nh]() {
+    bool rate_limiting;
+    robot_hw_nh.getParamCached("rate_limiting", rate_limiting);
+    return rate_limiting;
+  };
+
+  get_internal_controller_ = [robot_hw_nh]() {
+    std::string internal_controller;
+    robot_hw_nh.getParamCached("internal_controller", internal_controller);
+    franka::ControllerMode controller_mode;
+    if (internal_controller == "joint_impedance") {
+      controller_mode = franka::ControllerMode::kJointImpedance;
+    } else if (internal_controller == "cartesian_impedance") {
+      controller_mode = franka::ControllerMode::kCartesianImpedance;
+    } else {
+      ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance");
+      controller_mode = franka::ControllerMode::kJointImpedance;
+    }
+    return controller_mode;
+  };
+
+  get_cutoff_frequency_ = [robot_hw_nh]() {
+    double cutoff_frequency;
+    robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency);
+    return cutoff_frequency;
+  };
+}
+
+bool FrankaHW::commandHasNaN(const franka::Torques& command) {
+  return arrayHasNaN(command.tau_J);
+}
+
+bool FrankaHW::commandHasNaN(const franka::JointPositions& command) {
+  return arrayHasNaN(command.q);
+}
+
+bool FrankaHW::commandHasNaN(const franka::JointVelocities& command) {
+  return arrayHasNaN(command.dq);
+}
+
+bool FrankaHW::commandHasNaN(const franka::CartesianPose& command) {
+  return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_T_EE);
+}
+
+bool FrankaHW::commandHasNaN(const franka::CartesianVelocities& command) {
+  return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_dP_EE);
+}
+
+std::vector<double> FrankaHW::getCollisionThresholds(const std::string& name,
+                                                     ros::NodeHandle& robot_hw_nh,
+                                                     const std::vector<double>& defaults) {
+  std::vector<double> thresholds;
+  if (!robot_hw_nh.getParam("collision_config/" + name, thresholds) ||
+      thresholds.size() != defaults.size()) {
+    std::string message;
+    for (const double& threshold : defaults) {
+      message += std::to_string(threshold);
+      message += " ";
+    }
+    ROS_INFO("No parameter %s found, using default values: %s", name.c_str(), message.c_str());
+    return defaults;
+  }
+  return thresholds;
 }
 
 }  // namespace franka_hw
diff --git a/franka_hw/src/resource_helpers.cpp b/franka_hw/src/resource_helpers.cpp
index 91c386b7d1108717067a06080e947f05903d2fb0..cf22c58126be252100a3afcff35bd2a1c590d93c 100644
--- a/franka_hw/src/resource_helpers.cpp
+++ b/franka_hw/src/resource_helpers.cpp
@@ -200,4 +200,17 @@ bool partiallyClaimsArmJoints(const ArmClaimedMap& arm_claim_map, const std::str
   return false;
 }
 
+bool hasTrajectoryClaim(const ArmClaimedMap& arm_claim_map, const std::string& arm_id) {
+  if (arm_claim_map.find(arm_id) != arm_claim_map.end()) {
+    if (arm_claim_map.at(arm_id).joint_position_claims +
+            arm_claim_map.at(arm_id).joint_velocity_claims +
+            arm_claim_map.at(arm_id).cartesian_velocity_claims +
+            arm_claim_map.at(arm_id).cartesian_pose_claims >
+        0) {
+      return true;
+    }
+  }
+  return false;
+}
+
 }  // namespace franka_hw
diff --git a/franka_control/src/services.cpp b/franka_hw/src/services.cpp
similarity index 61%
rename from franka_control/src/services.cpp
rename to franka_hw/src/services.cpp
index c7a47c91b6f8a98d5e2a76cf86b07e5a4c9cafcf..355797756959febc2350cdaf7f304dcd9ef42acd 100644
--- a/franka_control/src/services.cpp
+++ b/franka_hw/src/services.cpp
@@ -1,12 +1,45 @@
 // Copyright (c) 2017 Franka Emika GmbH
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_control/services.h>
+#include <franka_hw/services.h>
 
-namespace franka_control {
+namespace franka_hw {
+
+void setupServices(franka::Robot& robot, ros::NodeHandle& node_handle, ServiceContainer& services) {
+  services
+      .advertiseService<franka_msgs::SetJointImpedance>(node_handle, "set_joint_impedance",
+                                                        [&robot](auto&& req, auto&& res) {
+                                                          return franka_hw::setJointImpedance(
+                                                              robot, req, res);
+                                                        })
+      .advertiseService<franka_msgs::SetCartesianImpedance>(
+          node_handle, "set_cartesian_impedance",
+          [&robot](auto&& req, auto&& res) {
+            return franka_hw::setCartesianImpedance(robot, req, res);
+          })
+      .advertiseService<franka_msgs::SetEEFrame>(
+          node_handle, "set_EE_frame",
+          [&robot](auto&& req, auto&& res) { return franka_hw::setEEFrame(robot, req, res); })
+      .advertiseService<franka_msgs::SetKFrame>(
+          node_handle, "set_K_frame",
+          [&robot](auto&& req, auto&& res) { return franka_hw::setKFrame(robot, req, res); })
+      .advertiseService<franka_msgs::SetForceTorqueCollisionBehavior>(
+          node_handle, "set_force_torque_collision_behavior",
+          [&robot](auto&& req, auto&& res) {
+            return franka_hw::setForceTorqueCollisionBehavior(robot, req, res);
+          })
+      .advertiseService<franka_msgs::SetFullCollisionBehavior>(
+          node_handle, "set_full_collision_behavior",
+          [&robot](auto&& req, auto&& res) {
+            return franka_hw::setFullCollisionBehavior(robot, req, res);
+          })
+      .advertiseService<franka_msgs::SetLoad>(
+          node_handle, "set_load",
+          [&robot](auto&& req, auto&& res) { return franka_hw::setLoad(robot, req, res); });
+}
 
 void setCartesianImpedance(franka::Robot& robot,
-                           const SetCartesianImpedance::Request& req,
-                           SetCartesianImpedance::Response& /* res */) {
+                           const franka_msgs::SetCartesianImpedance::Request& req,
+                           franka_msgs::SetCartesianImpedance::Response& /* res */) {
   std::array<double, 6> cartesian_stiffness;
   std::copy(req.cartesian_stiffness.cbegin(), req.cartesian_stiffness.cend(),
             cartesian_stiffness.begin());
@@ -14,32 +47,33 @@ void setCartesianImpedance(franka::Robot& robot,
 }
 
 void setJointImpedance(franka::Robot& robot,
-                       const SetJointImpedance::Request& req,
-                       SetJointImpedance::Response& /* res */) {
+                       const franka_msgs::SetJointImpedance::Request& req,
+                       franka_msgs::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 */) {
+                const franka_msgs::SetEEFrame::Request& req,
+                franka_msgs::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 */) {
+               const franka_msgs::SetKFrame::Request& req,
+               franka_msgs::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 */
+void setForceTorqueCollisionBehavior(
+    franka::Robot& robot,
+    const franka_msgs::SetForceTorqueCollisionBehavior::Request& req,
+    franka_msgs::SetForceTorqueCollisionBehavior::Response& /* res */
 ) {
   std::array<double, 7> lower_torque_thresholds_nominal;
   std::copy(req.lower_torque_thresholds_nominal.cbegin(),
@@ -59,8 +93,8 @@ void setForceTorqueCollisionBehavior(franka::Robot& robot,
 }
 
 void setFullCollisionBehavior(franka::Robot& robot,
-                              const SetFullCollisionBehavior::Request& req,
-                              SetFullCollisionBehavior::Response& /* res */) {
+                              const franka_msgs::SetFullCollisionBehavior::Request& req,
+                              franka_msgs::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(),
@@ -96,7 +130,9 @@ void setFullCollisionBehavior(franka::Robot& robot,
                              upper_force_thresholds_nominal);
 }
 
-void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& /* res */) {
+void setLoad(franka::Robot& robot,
+             const franka_msgs::SetLoad::Request& req,
+             franka_msgs::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());
@@ -106,4 +142,4 @@ void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Respons
   robot.setLoad(mass, F_x_center_load, load_inertia);
 }
 
-}  // namespace franka_control
+}  // namespace franka_hw
diff --git a/franka_hw/test/CMakeLists.txt b/franka_hw/test/CMakeLists.txt
index d14888ffa734fc0d23b25caa70a84dd4ce49192b..3601d19e9f2a5e7780d338b2ae5cdbadc76ad3d3 100644
--- a/franka_hw/test/CMakeLists.txt
+++ b/franka_hw/test/CMakeLists.txt
@@ -5,6 +5,7 @@ add_rostest_gtest(franka_hw_test
   main.cpp
   franka_hw_controller_switching_test.cpp
   franka_hw_interfaces_test.cpp
+  franka_combinable_hw_controller_switching_test.cpp
 )
 
 add_dependencies(franka_hw_test
@@ -21,4 +22,3 @@ target_link_libraries(franka_hw_test
 target_include_directories(franka_hw_test PUBLIC
   ${catkin_INCLUDE_DIRS}
 )
-
diff --git a/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp b/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f2837fb301f2b4e897ecdd4d48b4526b8649dff
--- /dev/null
+++ b/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp
@@ -0,0 +1,198 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <array>
+#include <list>
+#include <memory>
+#include <random>
+#include <set>
+#include <string>
+
+#include <gtest/gtest.h>
+#include <hardware_interface/controller_info.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <joint_limits_interface/joint_limits.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
+#include <ros/ros.h>
+#include <urdf/model.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_combinable_hw.h>
+
+extern std::string arm_id;
+extern std::array<std::string, 7> joint_names;
+
+hardware_interface::ControllerInfo newInfo(
+    const std::string& name,
+    const std::string& type,
+    const hardware_interface::InterfaceResources& resource1) {
+  hardware_interface::ControllerInfo info;
+  info.claimed_resources.clear();
+  info.name = name;
+  info.type = type;
+  info.claimed_resources.push_back(resource1);
+  return info;
+}
+
+hardware_interface::ControllerInfo newInfo(
+    const std::string& name,
+    const std::string& type,
+    const hardware_interface::InterfaceResources& resource1,
+    const hardware_interface::InterfaceResources& resource2) {
+  hardware_interface::ControllerInfo info = newInfo(name, type, resource1);
+  info.claimed_resources.push_back(resource2);
+  return info;
+}
+
+hardware_interface::ControllerInfo newInfo(
+    const std::string& name,
+    const std::string& type,
+    const hardware_interface::InterfaceResources& resource1,
+    const hardware_interface::InterfaceResources& resource2,
+    const hardware_interface::InterfaceResources& resource3) {
+  hardware_interface::ControllerInfo info = newInfo(name, type, resource1, resource2);
+  info.claimed_resources.push_back(resource3);
+  return info;
+}
+
+class CombinableControllerConflict
+    : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
+ public:
+  CombinableControllerConflict() : robot_(std::make_unique<franka_hw::FrankaCombinableHW>()) {
+    ros::NodeHandle root_nh;
+    ros::NodeHandle robot_hw_nh("~");
+    robot_->initParameters(root_nh, robot_hw_nh);
+    robot_->initROSInterfaces(robot_hw_nh);
+    robot_->setupParameterCallbacks(robot_hw_nh);
+  }
+  bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->checkForConflict(info_list);
+  }
+  bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->prepareSwitch(info_list, info_list);
+  }
+
+ private:
+  std::unique_ptr<franka_hw::FrankaCombinableHW> robot_;
+};
+
+class CombinableNoControllerConflict
+    : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
+ public:
+  CombinableNoControllerConflict() : robot_(std::make_unique<franka_hw::FrankaCombinableHW>()) {
+    ros::NodeHandle root_nh;
+    ros::NodeHandle robot_hw_nh("~");
+    robot_->initParameters(root_nh, robot_hw_nh);
+    robot_->initROSInterfaces(robot_hw_nh);
+    robot_->setupParameterCallbacks(robot_hw_nh);
+  }
+  bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->checkForConflict(info_list);
+  }
+  bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->prepareSwitch(info_list, info_list);
+  }
+
+ private:
+  std::unique_ptr<franka_hw::FrankaCombinableHW> robot_;
+};
+
+std::string arm_id2("panda2");
+std::string jp_iface_str("hardware_interface::PositionJointInterface");
+std::string jv_iface_str("hardware_interface::VelocityJointInterface");
+std::string jt_iface_str("hardware_interface::EffortJointInterface");
+std::string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface");
+std::string cp_iface_str("franka_hw::FrankaPoseCartesianInterface");
+std::string unknown_iface_str("hardware_interface::UnknownInterface");
+std::string name_str("some_controller");
+std::string type_str("SomeControllerClass");
+std::set<std::string> joints_set = {joint_names[0], joint_names[1], joint_names[2], joint_names[3],
+                                    joint_names[4], joint_names[5], joint_names[6]};
+std::set<std::string> cartesian_set = {arm_id + "_robot"};
+std::set<std::string> cartesian_arm2_set = {arm_id2 + "_robot"};
+std::set<std::string> no_id_set = {"joint1"};
+hardware_interface::InterfaceResources no_id_res(jp_iface_str, no_id_set);
+hardware_interface::InterfaceResources unknown_iface_res(unknown_iface_str, joints_set);
+hardware_interface::InterfaceResources jp_res(jp_iface_str, joints_set);
+hardware_interface::InterfaceResources jv_res(jv_iface_str, joints_set);
+hardware_interface::InterfaceResources jt_res(jt_iface_str, joints_set);
+hardware_interface::InterfaceResources cv_res(cv_iface_str, cartesian_set);
+hardware_interface::InterfaceResources cp_res(cp_iface_str, cartesian_set);
+hardware_interface::InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set);
+hardware_interface::ControllerInfo cp_arm2_info = newInfo(name_str, type_str, cp_arm2_res);
+hardware_interface::ControllerInfo no_id_info = newInfo(name_str, type_str, no_id_res);
+hardware_interface::ControllerInfo unknown_iface_info =
+    newInfo(name_str, type_str, unknown_iface_res);
+hardware_interface::ControllerInfo jt_jt_info = newInfo(name_str, type_str, jt_res, jt_res);
+hardware_interface::ControllerInfo jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res);
+hardware_interface::ControllerInfo jp_jv_info = newInfo(name_str, type_str, jp_res, jv_res);
+hardware_interface::ControllerInfo jp_cv_info = newInfo(name_str, type_str, jp_res, cv_res);
+hardware_interface::ControllerInfo jp_cp_info = newInfo(name_str, type_str, jp_res, cp_res);
+hardware_interface::ControllerInfo jv_jv_info = newInfo(name_str, type_str, jv_res, jv_res);
+hardware_interface::ControllerInfo jv_cv_info = newInfo(name_str, type_str, jv_res, cv_res);
+hardware_interface::ControllerInfo jv_cp_info = newInfo(name_str, type_str, jv_res, cp_res);
+hardware_interface::ControllerInfo cv_cv_info = newInfo(name_str, type_str, cv_res, cv_res);
+hardware_interface::ControllerInfo cv_cp_info = newInfo(name_str, type_str, cv_res, cp_res);
+hardware_interface::ControllerInfo cp_cp_info = newInfo(name_str, type_str, cp_res, cp_res);
+hardware_interface::ControllerInfo jp_jp_jp_info =
+    newInfo(name_str, type_str, jp_res, jp_res, jp_res);
+hardware_interface::ControllerInfo jp_info = newInfo(name_str, type_str, jp_res);
+hardware_interface::ControllerInfo jv_info = newInfo(name_str, type_str, jv_res);
+hardware_interface::ControllerInfo jt_info = newInfo(name_str, type_str, jt_res);
+hardware_interface::ControllerInfo cv_info = newInfo(name_str, type_str, cv_res);
+hardware_interface::ControllerInfo cp_info = newInfo(name_str, type_str, cp_res);
+hardware_interface::ControllerInfo jt_jp_info = newInfo(name_str, type_str, jt_res, jp_res);
+hardware_interface::ControllerInfo jt_jv_info = newInfo(name_str, type_str, jt_res, jv_res);
+hardware_interface::ControllerInfo jt_cv_info = newInfo(name_str, type_str, jt_res, cv_res);
+hardware_interface::ControllerInfo jt_cp_info = newInfo(name_str, type_str, jt_res, cp_res);
+
+INSTANTIATE_TEST_CASE_P(
+    combinableNonAdmissibleRequests,
+    CombinableControllerConflict,
+    ::testing::Values(std::list<hardware_interface::ControllerInfo>{no_id_info},
+                      std::list<hardware_interface::ControllerInfo>{unknown_iface_info},
+                      std::list<hardware_interface::ControllerInfo>{jp_info},
+                      std::list<hardware_interface::ControllerInfo>{jv_info},
+                      std::list<hardware_interface::ControllerInfo>{cv_info},
+                      std::list<hardware_interface::ControllerInfo>{cp_info},
+                      std::list<hardware_interface::ControllerInfo>{jt_jp_info},
+                      std::list<hardware_interface::ControllerInfo>{jt_jv_info},
+                      std::list<hardware_interface::ControllerInfo>{jt_jt_info},
+                      std::list<hardware_interface::ControllerInfo>{jp_jp_info},
+                      std::list<hardware_interface::ControllerInfo>{jp_jv_info},
+                      std::list<hardware_interface::ControllerInfo>{jp_cv_info},
+                      std::list<hardware_interface::ControllerInfo>{jp_cp_info},
+                      std::list<hardware_interface::ControllerInfo>{jv_jv_info},
+                      std::list<hardware_interface::ControllerInfo>{jv_cv_info},
+                      std::list<hardware_interface::ControllerInfo>{jv_cp_info},
+                      std::list<hardware_interface::ControllerInfo>{cv_cv_info},
+                      std::list<hardware_interface::ControllerInfo>{cv_cp_info},
+                      std::list<hardware_interface::ControllerInfo>{cp_cp_info},
+                      std::list<hardware_interface::ControllerInfo>{jp_jp_jp_info},
+                      std::list<hardware_interface::ControllerInfo>{jt_cv_info},
+                      std::list<hardware_interface::ControllerInfo>{jt_cp_info}));
+
+INSTANTIATE_TEST_CASE_P(combinableAdmissibleRequests,
+                        CombinableNoControllerConflict,
+                        ::testing::Values(std::list<hardware_interface::ControllerInfo>{jt_info},
+                                          std::list<hardware_interface::ControllerInfo>{
+                                              jt_info, cp_arm2_info}));
+
+TEST(FrankaCombinableHWTests, CanInitROSInterfaces) {
+  franka_hw::FrankaCombinableHW hw;
+  ros::NodeHandle root_nh;
+  ros::NodeHandle robot_hw_nh("~");
+  EXPECT_TRUE(hw.initParameters(root_nh, robot_hw_nh));
+  EXPECT_NO_THROW(hw.initROSInterfaces(robot_hw_nh));
+}
+
+TEST_P(CombinableControllerConflict, ConflictsForIncompatibleControllers) {
+  EXPECT_TRUE(callCheckForConflict(GetParam()));
+}
+
+TEST_P(CombinableNoControllerConflict, DoesNotConflictForCompatibleControllers) {
+  EXPECT_FALSE(callCheckForConflict(GetParam()));
+}
+
+TEST_P(CombinableNoControllerConflict, CanPrepareSwitchForCompatibleControllers) {
+  EXPECT_TRUE(callPrepareSwitch(GetParam()));
+}
diff --git a/franka_hw/test/franka_hw_controller_switching_test.cpp b/franka_hw/test/franka_hw_controller_switching_test.cpp
index 58639da26544821748e43a479ebf8e04d27e3c78..59646b7c4468cbf69887c907de3afa837d7502c2 100644
--- a/franka_hw/test/franka_hw_controller_switching_test.cpp
+++ b/franka_hw/test/franka_hw_controller_switching_test.cpp
@@ -24,13 +24,9 @@ using hardware_interface::InterfaceResources;
 using hardware_interface::ControllerInfo;
 
 std::string arm_id("panda");
-std::array<std::string, 7> joint_names  = {arm_id + "_joint1",
-        arm_id + "_joint2",
-        arm_id + "_joint3",
-        arm_id + "_joint4",
-        arm_id + "_joint5",
-        arm_id + "_joint6",
-        arm_id + "_joint7"};
+std::array<std::string, 7> joint_names = {
+    arm_id + "_joint1", arm_id + "_joint2", arm_id + "_joint3", arm_id + "_joint4",
+    arm_id + "_joint5", arm_id + "_joint6", arm_id + "_joint7"};
 
 namespace franka_hw {
 
@@ -67,32 +63,45 @@ hardware_interface::ControllerInfo newInfo(
   return info;
 }
 
-class ControllerConflict :
-        public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > {
-public:
- ControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, urdf::Model())) {}
- bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->checkForConflict(info_list);
- }
- bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->prepareSwitch(info_list, info_list);
- }
+class ControllerConflict
+    : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
+ public:
+  ControllerConflict() : robot_(std::make_unique<FrankaHW>()) {
+    ros::NodeHandle private_nh("~");
+    ros::NodeHandle root_nh;
+    EXPECT_TRUE(robot_->initParameters(root_nh, private_nh));
+    robot_->initROSInterfaces(private_nh);
+  }
+  bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->checkForConflict(info_list);
+  }
+  bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->prepareSwitch(info_list, info_list);
+  }
+
  private:
- std::unique_ptr<FrankaHW>  robot_;
+  std::unique_ptr<FrankaHW> robot_;
 };
 
-class NoControllerConflict : public
-        ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > {
-public:
- NoControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, urdf::Model())) {}
- bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->checkForConflict(info_list);
- }
- bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
-     return robot_->prepareSwitch(info_list, info_list);
- }
+class NoControllerConflict
+    : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
+ public:
+  NoControllerConflict() : robot_(std::make_unique<FrankaHW>()) {
+    ros::NodeHandle private_nh("~");
+    ros::NodeHandle root_nh;
+    EXPECT_TRUE(robot_->initParameters(root_nh, private_nh));
+    robot_->initROSInterfaces(private_nh);
+    robot_->setupParameterCallbacks(private_nh);
+  }
+  bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->checkForConflict(info_list);
+  }
+  bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
+    return robot_->prepareSwitch(info_list, info_list);
+  }
+
  private:
- std::unique_ptr<FrankaHW>  robot_;
+  std::unique_ptr<FrankaHW> robot_;
 };
 
 string arm_id2("panda2");
@@ -104,13 +113,8 @@ string cp_iface_str("franka_hw::FrankaPoseCartesianInterface");
 string unknown_iface_str("hardware_interface::UnknownInterface");
 string name_str("some_controller");
 string type_str("SomeControllerClass");
-set<string> joints_set = {joint_names[0],
-                          joint_names[1],
-                          joint_names[2],
-                          joint_names[3],
-                          joint_names[4],
-                          joint_names[5],
-                          joint_names[6]};
+set<string> joints_set = {joint_names[0], joint_names[1], joint_names[2], joint_names[3],
+                          joint_names[4], joint_names[5], joint_names[6]};
 set<string> cartesian_set = {arm_id + "_robot"};
 set<string> cartesian_arm2_set = {arm_id2 + "_robot"};
 set<string> no_id_set = {"joint1"};
@@ -186,7 +190,7 @@ TEST_P(NoControllerConflict, DoesNotConflictForCompatibleControllers) {
 }
 
 TEST_P(NoControllerConflict, CanPrepareSwitchForCompatibleControllers) {
-    EXPECT_TRUE(callPrepareSwitch(GetParam()));
+  EXPECT_TRUE(callPrepareSwitch(GetParam()));
 }
 
 }  // namespace franka_hw
diff --git a/franka_hw/test/franka_hw_interfaces_test.cpp b/franka_hw/test/franka_hw_interfaces_test.cpp
index 7984e1e2f71dfb3f43a00c06b42ceaa05c928a5a..7087632c21ef527817e3461ddb5e745e013106ed 100644
--- a/franka_hw/test/franka_hw_interfaces_test.cpp
+++ b/franka_hw/test/franka_hw_interfaces_test.cpp
@@ -23,21 +23,24 @@ extern std::array<std::string, 7> joint_names;
 namespace franka_hw {
 
 TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) {
-  std::unique_ptr<FrankaHW> robotptr(new FrankaHW(joint_names, arm_id, urdf::Model()));
+  auto robot_ptr = std::make_unique<FrankaHW>();
+  ros::NodeHandle private_nh("~");
+  ros::NodeHandle root_nh;
+  EXPECT_TRUE(robot_ptr->initParameters(root_nh, private_nh));
+  robot_ptr->initROSInterfaces(private_nh);
+
   hardware_interface::JointStateInterface* js_interface =
-      robotptr->get<hardware_interface::JointStateInterface>();
+      robot_ptr->get<hardware_interface::JointStateInterface>();
   hardware_interface::PositionJointInterface* pj_interface =
-      robotptr->get<hardware_interface::PositionJointInterface>();
+      robot_ptr->get<hardware_interface::PositionJointInterface>();
   hardware_interface::VelocityJointInterface* vj_interface =
-      robotptr->get<hardware_interface::VelocityJointInterface>();
+      robot_ptr->get<hardware_interface::VelocityJointInterface>();
   hardware_interface::EffortJointInterface* ej_interface =
-      robotptr->get<hardware_interface::EffortJointInterface>();
-  FrankaPoseCartesianInterface* fpc_interface =
-      robotptr->get<FrankaPoseCartesianInterface>();
+      robot_ptr->get<hardware_interface::EffortJointInterface>();
+  FrankaPoseCartesianInterface* fpc_interface = robot_ptr->get<FrankaPoseCartesianInterface>();
   FrankaVelocityCartesianInterface* fvc_interface =
-      robotptr->get<FrankaVelocityCartesianInterface>();
-  FrankaStateInterface* fs_interface =
-      robotptr->get<FrankaStateInterface>();
+      robot_ptr->get<FrankaVelocityCartesianInterface>();
+  FrankaStateInterface* fs_interface = robot_ptr->get<FrankaStateInterface>();
 
   ASSERT_NE(nullptr, js_interface);
   ASSERT_NE(nullptr, pj_interface);
@@ -48,23 +51,20 @@ TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) {
   ASSERT_NE(nullptr, fs_interface);
 
   // Model interface not available with this signature
-  FrankaModelInterface* fm_interface = robotptr->get<FrankaModelInterface>();
+  FrankaModelInterface* fm_interface = robot_ptr->get<FrankaModelInterface>();
   ASSERT_EQ(nullptr, fm_interface);
 
   EXPECT_NO_THROW(FrankaCartesianPoseHandle fpc_handle =
-      fpc_interface->getHandle(arm_id + "_robot"));
-  FrankaCartesianPoseHandle fpc_handle =
-        fpc_interface->getHandle(arm_id + "_robot");
-  std::array<double, 16> pose_command = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
-                                         1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
-                                         1.0, 1.0, 1.0, 1.0};
+                      fpc_interface->getHandle(arm_id + "_robot"));
+  FrankaCartesianPoseHandle fpc_handle = fpc_interface->getHandle(arm_id + "_robot");
+  std::array<double, 16> pose_command = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                         1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
   fpc_handle.setCommand(pose_command);
   EXPECT_EQ(pose_command, fpc_handle.getCommand());
 
   EXPECT_NO_THROW(FrankaCartesianVelocityHandle fvc_handle =
-      fvc_interface->getHandle(arm_id + "_robot"));
-  FrankaCartesianVelocityHandle fvc_handle =
-        fvc_interface->getHandle(arm_id + "_robot");
+                      fvc_interface->getHandle(arm_id + "_robot"));
+  FrankaCartesianVelocityHandle fvc_handle = fvc_interface->getHandle(arm_id + "_robot");
   std::array<double, 6> vel_command = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
   fvc_handle.setCommand(vel_command);
   EXPECT_EQ(vel_command, fvc_handle.getCommand());
@@ -73,10 +73,10 @@ TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) {
 }
 
 TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
-  urdf::Model urdf_model;
-  ASSERT_TRUE(urdf_model.initParamWithNodeHandle("robot_description"));
-
-  std::unique_ptr<FrankaHW> robot_ptr(new FrankaHW(joint_names, arm_id, urdf_model));
+  auto robot_ptr = std::make_unique<FrankaHW>();
+  ros::NodeHandle nh("~");
+  ASSERT_TRUE(robot_ptr->initParameters(nh, nh));
+  robot_ptr->initROSInterfaces(nh);
 
   hardware_interface::PositionJointInterface* pj_interface =
       robot_ptr->get<hardware_interface::PositionJointInterface>();
@@ -93,16 +93,15 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
   std::vector<hardware_interface::JointHandle> velocity_handles(7);
   std::vector<hardware_interface::JointHandle> effort_handles(7);
 
+  urdf::Model urdf_model;
+  ASSERT_TRUE(urdf_model.initParamWithNodeHandle("robot_description"));
+
   for (size_t i = 0; i < joint_names.size(); ++i) {
     auto urdf_joint = urdf_model.getJoint(joint_names[i]);
-    ASSERT_TRUE(
-        joint_limits_interface::getJointLimits(urdf_joint, joint_limits[i]));
-    ASSERT_NO_THROW(position_handles[i] =
-                        pj_interface->getHandle(joint_names[i]));
-    ASSERT_NO_THROW(velocity_handles[i] =
-                        vj_interface->getHandle(joint_names[i]));
-    ASSERT_NO_THROW(effort_handles[i] =
-                        ej_interface->getHandle(joint_names[i]));
+    ASSERT_TRUE(joint_limits_interface::getJointLimits(urdf_joint, joint_limits[i]));
+    ASSERT_NO_THROW(position_handles[i] = pj_interface->getHandle(joint_names[i]));
+    ASSERT_NO_THROW(velocity_handles[i] = vj_interface->getHandle(joint_names[i]));
+    ASSERT_NO_THROW(effort_handles[i] = ej_interface->getHandle(joint_names[i]));
   }
 
   std::uniform_real_distribution<double> uniform_distribution(0.1, 3.0);
@@ -113,13 +112,12 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
                                    uniform_distribution(random_engine));
     velocity_handles[i].setCommand(joint_limits[i].max_velocity +
                                    uniform_distribution(random_engine));
-    effort_handles[i].setCommand(joint_limits[i].max_effort +
-                                 uniform_distribution(random_engine));
+    effort_handles[i].setCommand(joint_limits[i].max_effort + uniform_distribution(random_engine));
   }
   robot_ptr->enforceLimits(ros::Duration(0.001));
   for (size_t i = 0; i < joint_names.size(); ++i) {
     EXPECT_LE(position_handles[i].getCommand(), joint_limits[i].max_position);
-    EXPECT_GE(position_handles[i].getCommand(),joint_limits[i].min_position);
+    EXPECT_GE(position_handles[i].getCommand(), joint_limits[i].min_position);
     EXPECT_LE(velocity_handles[i].getCommand(), joint_limits[i].max_velocity);
     EXPECT_GE(velocity_handles[i].getCommand(), -joint_limits[i].max_velocity);
     EXPECT_LE(effort_handles[i].getCommand(), joint_limits[i].max_effort);
@@ -131,18 +129,17 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
                                    uniform_distribution(random_engine));
     velocity_handles[i].setCommand(-joint_limits[i].max_velocity -
                                    uniform_distribution(random_engine));
-    effort_handles[i].setCommand(-joint_limits[i].max_effort -
-                                 uniform_distribution(random_engine));
+    effort_handles[i].setCommand(-joint_limits[i].max_effort - uniform_distribution(random_engine));
   }
   robot_ptr->enforceLimits(ros::Duration(0.001));
   for (size_t i = 0; i < joint_names.size(); ++i) {
-      EXPECT_LE(position_handles[i].getCommand(), joint_limits[i].max_position);
-      EXPECT_GE(position_handles[i].getCommand(),joint_limits[i].min_position);
-      EXPECT_LE(velocity_handles[i].getCommand(), joint_limits[i].max_velocity);
-      EXPECT_GE(velocity_handles[i].getCommand(), -joint_limits[i].max_velocity);
-      EXPECT_LE(effort_handles[i].getCommand(), joint_limits[i].max_effort);
-      EXPECT_GE(effort_handles[i].getCommand(), -joint_limits[i].max_effort); }
+    EXPECT_LE(position_handles[i].getCommand(), joint_limits[i].max_position);
+    EXPECT_GE(position_handles[i].getCommand(), joint_limits[i].min_position);
+    EXPECT_LE(velocity_handles[i].getCommand(), joint_limits[i].max_velocity);
+    EXPECT_GE(velocity_handles[i].getCommand(), -joint_limits[i].max_velocity);
+    EXPECT_LE(effort_handles[i].getCommand(), joint_limits[i].max_effort);
+    EXPECT_GE(effort_handles[i].getCommand(), -joint_limits[i].max_effort);
+  }
 }
 
 }  // namespace franka_hw
-
diff --git a/franka_hw/test/launch/franka_hw_test.test b/franka_hw/test/launch/franka_hw_test.test
index 53dfa12ed4015436ad3dfc2257c2197e90fa92bb..64ffded744f2e8ff753e3d4464efbc2f3221c5c2 100644
--- a/franka_hw/test/launch/franka_hw_test.test
+++ b/franka_hw/test/launch/franka_hw_test.test
@@ -1,5 +1,8 @@
 <launch>
  <env name="ROSCONSOLE_CONFIG_FILE" value="$(find franka_hw)/test/config/ros_console_settings_for_tests.conf"/>
  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" />
- <test test-name="franka_hw_test" pkg="franka_hw" type="franka_hw_test" />
+ <test test-name="franka_hw_test" pkg="franka_hw" type="franka_hw_test" >
+   <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
+   <param name="robot_ip" value="unused_dummy_ip" />
+ </test>
 </launch>
diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt
index 49816727b832637c32d3075794185a542341495e..1f6f926597e947e268865a2a7827ea0e30570d8e 100644
--- a/franka_msgs/CMakeLists.txt
+++ b/franka_msgs/CMakeLists.txt
@@ -1,8 +1,22 @@
 cmake_minimum_required(VERSION 3.4)
 project(franka_msgs)
 
-find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_msgs)
 
 add_message_files(FILES Errors.msg FrankaState.msg)
-generate_messages(DEPENDENCIES std_msgs)
-catkin_package(CATKIN_DEPENDS message_runtime std_msgs)
+
+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 std_msgs actionlib_msgs)
+
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs actionlib_msgs)
diff --git a/franka_control/action/ErrorRecovery.action b/franka_msgs/action/ErrorRecovery.action
similarity index 100%
rename from franka_control/action/ErrorRecovery.action
rename to franka_msgs/action/ErrorRecovery.action
diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml
index e5b2e8dc330ee85bec6bd1b14fbb50ec117cba46..c8bcef0cbad6707c4b4867f2b36792de5e0f7bb1 100644
--- a/franka_msgs/package.xml
+++ b/franka_msgs/package.xml
@@ -13,9 +13,13 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <depend>std_msgs</depend>
   <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+  <depend>actionlib_msgs</depend>
+
   <exec_depend>message_runtime</exec_depend>
+
   <build_export_depend>message_runtime</build_export_depend>
 
   <export>
diff --git a/franka_control/srv/SetCartesianImpedance.srv b/franka_msgs/srv/SetCartesianImpedance.srv
similarity index 100%
rename from franka_control/srv/SetCartesianImpedance.srv
rename to franka_msgs/srv/SetCartesianImpedance.srv
diff --git a/franka_control/srv/SetEEFrame.srv b/franka_msgs/srv/SetEEFrame.srv
similarity index 100%
rename from franka_control/srv/SetEEFrame.srv
rename to franka_msgs/srv/SetEEFrame.srv
diff --git a/franka_control/srv/SetForceTorqueCollisionBehavior.srv b/franka_msgs/srv/SetForceTorqueCollisionBehavior.srv
similarity index 100%
rename from franka_control/srv/SetForceTorqueCollisionBehavior.srv
rename to franka_msgs/srv/SetForceTorqueCollisionBehavior.srv
diff --git a/franka_control/srv/SetFullCollisionBehavior.srv b/franka_msgs/srv/SetFullCollisionBehavior.srv
similarity index 100%
rename from franka_control/srv/SetFullCollisionBehavior.srv
rename to franka_msgs/srv/SetFullCollisionBehavior.srv
diff --git a/franka_control/srv/SetJointImpedance.srv b/franka_msgs/srv/SetJointImpedance.srv
similarity index 100%
rename from franka_control/srv/SetJointImpedance.srv
rename to franka_msgs/srv/SetJointImpedance.srv
diff --git a/franka_control/srv/SetKFrame.srv b/franka_msgs/srv/SetKFrame.srv
similarity index 100%
rename from franka_control/srv/SetKFrame.srv
rename to franka_msgs/srv/SetKFrame.srv
diff --git a/franka_control/srv/SetLoad.srv b/franka_msgs/srv/SetLoad.srv
similarity index 100%
rename from franka_control/srv/SetLoad.srv
rename to franka_msgs/srv/SetLoad.srv