diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
index a85969abdc2ba71dd3ab785a0762f1910733e157..8774cde8cc1a895ad3c019b754ff28c15020c870 100644
--- a/franka_hw/CMakeLists.txt
+++ b/franka_hw/CMakeLists.txt
@@ -7,81 +7,26 @@ set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(catkin REQUIRED COMPONENTS
- actionlib_msgs
controller_interface
- controller_manager
franka_description
- geometry_msgs
hardware_interface
joint_limits_interface
- message_generation
- pluginlib
- realtime_tools
roscpp
- sensor_msgs
- std_msgs
- tf
- tf2_msgs
- urdf
- xmlrpcpp
)
find_package(Franka REQUIRED)
-add_message_files(FILES
- Errors.msg
- FrankaState.msg
-)
-
-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
- std_msgs
-)
-
catkin_package(
INCLUDE_DIRS include
- LIBRARIES franka_hw franka_state_controller
- CATKIN_DEPENDS actionlib_msgs controller_interface pluginlib
- hardware_interface message_runtime realtime_tools roscpp
+ LIBRARIES franka_hw
+ CATKIN_DEPENDS controller_interface hardware_interface roscpp
DEPENDS Franka
)
-add_library(franka_hw_services src/services.cpp)
-
-add_dependencies(franka_hw_services
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- ${PROJECT_NAME}_generate_messages_cpp
-)
-
-target_link_libraries(franka_hw_services
- ${catkin_LIBRARIES}
- Franka::Franka
-)
-
-target_include_directories(franka_hw_services PUBLIC
- ${catkin_INCLUDE_DIRS}
- include
-)
-
add_library(franka_hw
+ src/control_mode.cpp
src/franka_hw.cpp
- src/franka_controller_switching_types.cpp
- src/franka_hw_helper_functions.cpp
+ src/resource_helpers.cpp
src/trigger_rate.cpp
)
@@ -100,46 +45,6 @@ target_include_directories(franka_hw PUBLIC
include
)
-add_library(franka_state_controller
- src/franka_state_controller.cpp
-)
-
-add_dependencies(franka_state_controller
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- ${PROJECT_NAME}_generate_messages_cpp
-)
-
-target_link_libraries(franka_state_controller
- ${catkin_LIBRARIES}
- Franka::Franka
-)
-
-target_include_directories(franka_state_controller PUBLIC
- ${catkin_INCLUDE_DIRS}
- include
-)
-
-add_executable(franka_hw_node
- src/franka_hw_node.cpp
-)
-
-add_dependencies(franka_hw_node
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- franka_hw_services
-)
-
-target_link_libraries(franka_hw_node
- ${catkin_LIBRARIES}
- franka_hw
- franka_hw_services
-)
-
-target_include_directories(franka_hw_node PUBLIC
- ${catkin_INCLUDE_DIRS}
-)
-
if(CATKIN_ENABLE_TESTING)
add_subdirectory(test)
endif()
@@ -154,5 +59,5 @@ include(ClangTools)
add_format_target(franka_hw FILES ${SOURCES} ${HEADERS})
add_tidy_target(franka_hw
FILES ${SOURCES}
- DEPENDS franka_hw franka_hw_node
+ DEPENDS franka_hw
)
diff --git a/franka_hw/action/ErrorRecovery.action b/franka_hw/action/ErrorRecovery.action
deleted file mode 100644
index a845151cc840034a50f5fd010515d61e661f79ee..0000000000000000000000000000000000000000
--- a/franka_hw/action/ErrorRecovery.action
+++ /dev/null
@@ -1,2 +0,0 @@
----
----
diff --git a/franka_hw/config/default_controllers.yaml b/franka_hw/config/default_controllers.yaml
deleted file mode 100644
index 7122c978ca317c84bd25aa02e542dfc063ec2e2b..0000000000000000000000000000000000000000
--- a/franka_hw/config/default_controllers.yaml
+++ /dev/null
@@ -1,66 +0,0 @@
-position_joint_trajectory_controller:
- type: "position_controllers/JointTrajectoryController"
- joints:
- - franka_emika_joint1
- - franka_emika_joint2
- - franka_emika_joint3
- - franka_emika_joint4
- - franka_emika_joint5
- - franka_emika_joint6
- - franka_emika_joint7
- constraints:
- goal_time: 0.5
- franka_emika_joint1:
- goal: 0.05
- franka_emika_joint2:
- goal: 0.05
- franka_emika_joint3:
- goal: 0.05
- franka_emika_joint4:
- goal: 0.05
- franka_emika_joint5:
- goal: 0.05
- franka_emika_joint6:
- goal: 0.05
- franka_emika_joint7:
- goal: 0.05
-
-effort_joint_trajectory_controller:
- type: "effort_controllers/JointTrajectoryController"
- joints:
- - franka_emika_joint1
- - franka_emika_joint2
- - franka_emika_joint3
- - franka_emika_joint4
- - franka_emika_joint5
- - franka_emika_joint6
- - franka_emika_joint7
- constraints:
- goal_time: 0.5
- franka_emika_joint1:
- goal: 0.05
- franka_emika_joint2:
- goal: 0.05
- franka_emika_joint3:
- goal: 0.05
- franka_emika_joint4:
- goal: 0.05
- franka_emika_joint5:
- goal: 0.05
- franka_emika_joint6:
- goal: 0.05
- franka_emika_joint7:
- goal: 0.05
- gains:
- franka_emika_joint1: {p: 100, d: 10, i: 0, i_clamp: 1}
- franka_emika_joint2: {p: 100, d: 10, i: 0, i_clamp: 1}
- franka_emika_joint3: {p: 100, d: 10, i: 0, i_clamp: 1}
- franka_emika_joint4: {p: 100, d: 10, i: 0, i_clamp: 1}
- franka_emika_joint5: {p: 100, d: 10, i: 0, i_clamp: 1}
- franka_emika_joint6: {p: 100, d: 10, i: 0, i_clamp: 1}
- franka_emika_joint7: {p: 100, d: 10, i: 0, i_clamp: 1}
-
-franka_state_controller:
- type: franka_hw/FrankaStateController
- publish_rate: 30 # [Hz]
-
diff --git a/franka_hw/config/franka_hw.yaml b/franka_hw/config/franka_hw.yaml
deleted file mode 100644
index b8f7795bb7d087f97916988a161ab22d7cf687a8..0000000000000000000000000000000000000000
--- a/franka_hw/config/franka_hw.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-joint_names:
- - franka_emika_joint1
- - franka_emika_joint2
- - franka_emika_joint3
- - franka_emika_joint4
- - franka_emika_joint5
- - franka_emika_joint6
- - franka_emika_joint7
-
-arm_id: franka_emika
-robot_ip: 127.0.0.1
diff --git a/franka_hw/franka_hw_controller_plugin.xml b/franka_hw/franka_hw_controller_plugin.xml
deleted file mode 100644
index e4e6fe6066546b793022bc0097aa8932ba92e380..0000000000000000000000000000000000000000
--- a/franka_hw/franka_hw_controller_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="lib/libfranka_state_controller">
- <class name="franka_hw/FrankaStateController" type="franka_hw::FrankaStateController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that publishes all relevant franka robot states
- </description>
- </class>
-</library>
diff --git a/franka_hw/include/franka_hw/franka_controller_switching_types.h b/franka_hw/include/franka_hw/control_mode.h
similarity index 77%
rename from franka_hw/include/franka_hw/franka_controller_switching_types.h
rename to franka_hw/include/franka_hw/control_mode.h
index b81d88722de11788e253a4d84b759856e6b8b3f8..3e83ee73a3f80b642d3adb24b8a954e3b2befb90 100644
--- a/franka_hw/include/franka_hw/franka_controller_switching_types.h
+++ b/franka_hw/include/franka_hw/control_mode.h
@@ -1,25 +1,9 @@
#pragma once
-#include <cstdint>
-#include <map>
#include <ostream>
-#include <string>
-#include <vector>
namespace franka_hw {
-using ResourceWithClaimsMap = std::map<std::string, std::vector<std::vector<std::string>>>;
-
-struct ResourceClaims {
- uint8_t joint_position_claims = 0;
- uint8_t joint_velocity_claims = 0;
- uint8_t joint_torque_claims = 0;
- uint8_t cartesian_velocity_claims = 0;
- uint8_t cartesian_pose_claims = 0;
-};
-
-using ArmClaimedMap = std::map<std::string, ResourceClaims>;
-
enum class ControlMode {
None = 0,
JointTorque = (1 << 0),
diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h
index 364f82d0c4c274e46440ec57cc28b14fc867401f..20fd6d2eb0a1baecc317dbabafd44c4fcf547de3 100644
--- a/franka_hw/include/franka_hw/franka_hw.h
+++ b/franka_hw/include/franka_hw/franka_hw.h
@@ -16,8 +16,8 @@
#include <ros/node_handle.h>
#include <ros/time.h>
+#include <franka_hw/control_mode.h>
#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_controller_switching_types.h>
#include <franka_hw/franka_model_interface.h>
#include <franka_hw/franka_state_interface.h>
diff --git a/franka_hw/include/franka_hw/franka_state_controller.h b/franka_hw/include/franka_hw/franka_state_controller.h
deleted file mode 100644
index 903c6853e20896f8f640f7a551de32125ecf688f..0000000000000000000000000000000000000000
--- a/franka_hw/include/franka_hw/franka_state_controller.h
+++ /dev/null
@@ -1,91 +0,0 @@
-#pragma once
-
-#include <cstdint>
-#include <memory>
-#include <vector>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <franka_hw/FrankaState.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/WrenchStamped.h>
-#include <hardware_interface/robot_hw.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <sensor_msgs/JointState.h>
-#include <tf2_msgs/TFMessage.h>
-
-#include <franka_hw/FrankaState.h>
-#include <franka_hw/franka_state_interface.h>
-#include <franka_hw/trigger_rate.h>
-
-namespace franka_hw {
-
-class FrankaStateController
- : public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface> {
- public:
- FrankaStateController();
-
- /**
- * Initializes the controller with interfaces and publishers
- *
- * @param[in] hardware Pointer to the robot hardware
- * @param[in] root_node_handle Nodehandle on root level passed from HW node
- * @param[in] controller_node_handle Nodehandle in the controller namespace
- * passed from HW node
- */
- bool init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& controller_node_handle) override;
-
- /**
- * Reads a new franka robot state and publishes it
- *
- * @param[in] time Current ros time
- */
- void update(const ros::Time& time, const ros::Duration&) override;
-
- private:
- /**
- * Publishes all relevant data received from the Franka arm
- *
- * @param[in] time Current ros time
- */
- void publishFrankaStates(const ros::Time& time);
-
- /**
- * Publishes the joint states of the Franka arm
- *
- * @param[in] time Current ros time
- */
- void publishJointStates(const ros::Time& time);
-
- /**
- * Publishes the transforms for EE and K frame which define the end-effector
- * (EE) and the Cartesian impedance reference frame (K)
- *
- * @param[in] time Current ros time
- */
- void publishTransforms(const ros::Time& time);
-
- /**
- * Publishes the estimated external wrench felt by the Franka
- *
- * @param[in] time Current ros time
- */
- void publishExternalWrench(const ros::Time& time);
-
- std::string arm_id_;
-
- franka_hw::FrankaStateInterface* franka_state_interface_;
- std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
-
- realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
- realtime_tools::RealtimePublisher<franka_hw::FrankaState> publisher_franka_states_;
- realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_;
- realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> publisher_external_wrench_;
- franka_hw::TriggerRate trigger_publish_;
- franka::RobotState robot_state_;
- uint64_t sequence_number_ = 0;
- std::vector<std::string> joint_names_;
-};
-
-} // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/services.h b/franka_hw/include/franka_hw/services.h
deleted file mode 100644
index 5179109136709b185bd3a88426f413a79dd715e9..0000000000000000000000000000000000000000
--- a/franka_hw/include/franka_hw/services.h
+++ /dev/null
@@ -1,57 +0,0 @@
-#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_hw/SetCartesianImpedance.h>
-#include <franka_hw/SetEEFrame.h>
-#include <franka_hw/SetForceTorqueCollisionBehavior.h>
-#include <franka_hw/SetFullCollisionBehavior.h>
-#include <franka_hw/SetJointImpedance.h>
-#include <franka_hw/SetKFrame.h>
-#include <franka_hw/SetLoad.h>
-
-namespace franka_hw {
-namespace services {
-
-template <typename T>
-ros::ServiceServer advertiseService(
- ros::NodeHandle& node_handle,
- const std::string& name,
- std::function<void(typename T::Request&, typename T::Response&)> handler) {
- return node_handle.advertiseService<typename T::Request, typename T::Response>(
- name, [name, handler](typename T::Request& request, typename T::Response& response) {
- try {
- handler(request, response);
- response.success = true;
- ROS_DEBUG_STREAM(name << " succeeded.");
- } catch (const franka::Exception& ex) {
- ROS_ERROR_STREAM(name << " failed: " << ex.what());
- response.success = false;
- response.error = ex.what();
- }
- return true;
- });
-}
-
-void setCartesianImpedance(franka::Robot& robot,
- const SetCartesianImpedance::Request& req,
- SetCartesianImpedance::Response& res);
-void setJointImpedance(franka::Robot& robot,
- const SetJointImpedance::Request& req,
- SetJointImpedance::Response& res);
-void setEEFrame(franka::Robot& robot, const SetEEFrame::Request& req, SetEEFrame::Response& res);
-void setKFrame(franka::Robot& robot, const SetKFrame::Request& req, SetKFrame::Response& res);
-void setForceTorqueCollisionBehavior(franka::Robot& robot,
- const SetForceTorqueCollisionBehavior::Request& req,
- SetForceTorqueCollisionBehavior::Response& res);
-void setFullCollisionBehavior(franka::Robot& robot,
- const SetFullCollisionBehavior::Request& req,
- SetFullCollisionBehavior::Response& res);
-void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& res);
-
-} // namespace services
-} // namespace franka_hw
diff --git a/franka_hw/launch/franka_hw.launch b/franka_hw/launch/franka_hw.launch
deleted file mode 100644
index eb1b352d628a8c25c69330fc76d763711a270707..0000000000000000000000000000000000000000
--- a/franka_hw/launch/franka_hw.launch
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" default="robot.franka.de" />
- <arg name="arm_id" default="franka_emika" />
- <arg name="load_gripper" default="true" />
-
- <node name="$(arg arm_id)" pkg="franka_hw" type="franka_hw_node" output="screen" required="true">
- <rosparam command="load" file="$(find franka_hw)/config/franka_hw.yaml" />
- <param name="robot_ip" value="$(arg robot_ip)" />
- <param name="arm_id" value="$(arg arm_id)" />
- <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01_Gripper.urdf.xacro'" if="$(arg load_gripper)" />
- <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01.urdf.xacro'" unless="$(arg load_gripper)" />
- </node>
-
- <group ns="$(arg arm_id)">
- <rosparam command="load" file="$(find franka_hw)/config/default_controllers.yaml" />
- <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller --shutdown-timeout 2"/>
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
- <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
- <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01_Gripper.urdf.xacro'" />
- <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper_node/joint_states] </rosparam>
- <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
- <param name="rate" value="30"/>
- </node>
- </group>
-</launch>
diff --git a/franka_hw/msg/Errors.msg b/franka_hw/msg/Errors.msg
deleted file mode 100644
index cddeed88f7e33a89a1a968150725803482744025..0000000000000000000000000000000000000000
--- a/franka_hw/msg/Errors.msg
+++ /dev/null
@@ -1,33 +0,0 @@
-bool joint_position_limits_violation
-bool cartesian_position_limits_violation
-bool self_collision_avoidance_violation
-bool joint_velocity_violation
-bool cartesian_velocity_violation
-bool force_control_safety_violation
-bool joint_reflex
-bool cartesian_reflex
-bool max_goal_pose_deviation_violation
-bool max_path_pose_deviation_violation
-bool cartesian_velocity_profile_safety_violation
-bool joint_position_motion_generator_start_pose_invalid
-bool joint_motion_generator_position_limits_violation
-bool joint_motion_generator_velocity_limits_violation
-bool joint_motion_generator_velocity_discontinuity
-bool joint_motion_generator_acceleration_discontinuity
-bool cartesian_position_motion_generator_start_pose_invalid
-bool cartesian_motion_generator_elbow_limit_violation
-bool cartesian_motion_generator_velocity_limits_violation
-bool cartesian_motion_generator_velocity_discontinuity
-bool cartesian_motion_generator_acceleration_discontinuity
-bool cartesian_motion_generator_elbow_sign_inconsistent
-bool cartesian_motion_generator_start_elbow_invalid
-bool cartesian_motion_generator_joint_position_limits_violation
-bool cartesian_motion_generator_joint_velocity_limits_violation
-bool cartesian_motion_generator_joint_velocity_discontinuity
-bool cartesian_motion_generator_joint_acceleration_discontinuity
-bool cartesian_position_motion_generator_invalid_frame
-bool force_controller_desired_force_tolerance_violation
-bool controller_torque_discontinuity
-bool start_elbow_sign_inconsistent
-bool communication_constraints_violation
-bool power_limit_violation
diff --git a/franka_hw/msg/FrankaState.msg b/franka_hw/msg/FrankaState.msg
deleted file mode 100644
index d909396eed6d5fc57fb184263da39537c8967922..0000000000000000000000000000000000000000
--- a/franka_hw/msg/FrankaState.msg
+++ /dev/null
@@ -1,26 +0,0 @@
-std_msgs/Header header
-float64[6] cartesian_collision
-float64[6] cartesian_contact
-float64[7] q
-float64[7] q_d
-float64[7] dq
-float64[7] dq_d
-float64[7] tau_J
-float64[7] dtau_J
-float64[6] K_F_ext_hat_K
-float64[2] elbow
-float64[2] elbow_d
-float64[7] joint_collision
-float64[7] joint_contact
-float64[6] O_F_ext_hat_K
-float64[7] tau_ext_hat_filtered
-float64 m_load
-float64[3] F_x_Cload
-float64[9] I_load
-float64[16] O_T_EE
-float64[16] O_T_EE_d
-float64[16] F_T_EE
-float64[16] EE_T_K
-float64 time
-franka_hw/Errors current_errors
-franka_hw/Errors last_motion_errors
diff --git a/franka_hw/package.xml b/franka_hw/package.xml
index 78cfa758e7467466feea000894820f4b0d38885f..afbc4db47b1ee577e0743c7b66bb4de13fd3d2a0 100644
--- a/franka_hw/package.xml
+++ b/franka_hw/package.xml
@@ -2,10 +2,10 @@
<package format="2">
<name>franka_hw</name>
<version>0.0.1</version>
- <description>This package implements the franka_hw of Franka Emika for the use with ros_control</description>
+ <description>franka_hw provides hardware interfaces for using FRANKA EMIKA research robots with ros_control</description>
<maintainer email="info@franka.de">FRANKA EMIKA Research Interface Team</maintainer>
<license>NONE</license>
-
+
<url type="website">http://wiki.ros.org/franka_hw</url>
<url type="repository">https://github.com/frankaemika/franka_ros</url>
<url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
@@ -13,32 +13,12 @@
<buildtool_depend>catkin</buildtool_depend>
- <build_depend>message_generation</build_depend>
-
<depend>Franka</depend>
- <depend>actionlib</depend>
- <depend>actionlib_msgs</depend>
<depend>controller_interface</depend>
- <depend>controller_manager</depend>
- <depend>geometry_msgs</depend>
<depend>hardware_interface</depend>
<depend>joint_limits_interface</depend>
- <depend>pluginlib</depend>
- <depend>realtime_tools</depend>
<depend>roscpp</depend>
- <depend>sensor_msgs</depend>
- <depend>std_msgs</depend>
- <depend>tf2_msgs</depend>
- <depend>tf</depend>
- <depend>urdf</depend>
- <depend>xmlrpcpp</depend>
-
- <exec_depend>message_runtime</exec_depend>
<test_depend>gtest</test_depend>
<test_depend>rostest</test_depend>
-
- <export>
- <controller_interface plugin="${prefix}/franka_hw_controller_plugin.xml"/>
- </export>
</package>
diff --git a/franka_hw/src/control_mode.cpp b/franka_hw/src/control_mode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b60c1cf33f89ff6c699903598db7d67118fa46df
--- /dev/null
+++ b/franka_hw/src/control_mode.cpp
@@ -0,0 +1,34 @@
+#include <franka_hw/control_mode.h>
+
+#include <algorithm>
+#include <iterator>
+
+namespace franka_hw {
+
+std::ostream& operator<<(std::ostream& ostream, ControlMode mode) {
+ if (mode == ControlMode::None) {
+ ostream << "<none>";
+ } else {
+ std::vector<std::string> names;
+ if ((mode & ControlMode::JointTorque) != ControlMode::None) {
+ names.emplace_back("JointTorque");
+ }
+ if ((mode & ControlMode::JointPosition) != ControlMode::None) {
+ names.emplace_back("JointPosition");
+ }
+ if ((mode & ControlMode::JointVelocity) != ControlMode::None) {
+ names.emplace_back("JointVelocity");
+ }
+ if ((mode & ControlMode::CartesianVelocity) != ControlMode::None) {
+ names.emplace_back("CartesianVelocity");
+ }
+ if ((mode & ControlMode::CartesianPose) != ControlMode::None) {
+ names.emplace_back("CartesianPose");
+ }
+ std::copy(names.cbegin(), names.cend() - 1, std::ostream_iterator<std::string>(ostream, ", "));
+ ostream << names.back();
+ }
+ return ostream;
+}
+
+} // namespace franka_hw
diff --git a/franka_hw/src/franka_controller_switching_types.cpp b/franka_hw/src/franka_controller_switching_types.cpp
index a00e478b3f5705c10ad68d5bcd4a8b7408bf16d3..0d2cd8a6cf7ecae857ac4b870a3338f4ea9b00d0 100644
--- a/franka_hw/src/franka_controller_switching_types.cpp
+++ b/franka_hw/src/franka_controller_switching_types.cpp
@@ -1,4 +1,4 @@
-#include <franka_hw/franka_controller_switching_types.h>
+#include "franka_hw/control_mode.h"
#include <algorithm>
#include <iterator>
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index a9f3f18f03b785fef007a4b4f9db9eac46250fb7..34dd36b68910e01fb8ff56f9cbf31e1060540f5d 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -5,7 +5,7 @@
#include <joint_limits_interface/joint_limits_urdf.h>
#include <urdf/model.h>
-#include "franka_hw_helper_functions.h"
+#include "resource_helpers.h"
namespace franka_hw {
diff --git a/franka_hw/src/franka_state_controller.cpp b/franka_hw/src/franka_state_controller.cpp
deleted file mode 100644
index 80f67dd6a28627cb3878d02705f17a0716ad4b28..0000000000000000000000000000000000000000
--- a/franka_hw/src/franka_state_controller.cpp
+++ /dev/null
@@ -1,365 +0,0 @@
-#include <franka_hw/franka_state_controller.h>
-
-#include <cmath>
-#include <mutex>
-#include <string>
-
-#include <controller_interface/controller_base.h>
-#include <hardware_interface/hardware_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-#include <tf/tf.h>
-#include <tf/transform_datatypes.h>
-#include <xmlrpcpp/XmlRpcValue.h>
-
-#include <franka_hw/Errors.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-
-namespace {
-tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
- tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],
- transform[9], transform[2], transform[6], transform[10]);
- tf::Vector3 translation(transform[12], transform[13], transform[14]);
- return tf::Transform(rotation, translation);
-}
-
-franka_hw::Errors errorsToMessage(const franka::Errors& error) {
- franka_hw::Errors message;
- message.joint_position_limits_violation =
- static_cast<decltype(message.joint_position_limits_violation)>(
- error.joint_position_limits_violation);
- message.cartesian_position_limits_violation =
- static_cast<decltype(message.cartesian_position_limits_violation)>(
- error.cartesian_position_limits_violation);
- message.self_collision_avoidance_violation =
- static_cast<decltype(message.self_collision_avoidance_violation)>(
- error.self_collision_avoidance_violation);
- message.joint_velocity_violation =
- static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
- message.cartesian_velocity_violation =
- static_cast<decltype(message.cartesian_velocity_violation)>(
- error.cartesian_velocity_violation);
- message.force_control_safety_violation =
- static_cast<decltype(message.force_control_safety_violation)>(
- error.force_control_safety_violation);
- message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
- message.cartesian_reflex =
- static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
- message.max_goal_pose_deviation_violation =
- static_cast<decltype(message.max_goal_pose_deviation_violation)>(
- error.max_goal_pose_deviation_violation);
- message.max_path_pose_deviation_violation =
- static_cast<decltype(message.max_path_pose_deviation_violation)>(
- error.max_path_pose_deviation_violation);
- message.cartesian_velocity_profile_safety_violation =
- static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
- error.cartesian_velocity_profile_safety_violation);
- message.joint_position_motion_generator_start_pose_invalid =
- static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
- error.joint_position_motion_generator_start_pose_invalid);
- message.joint_motion_generator_position_limits_violation =
- static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
- error.joint_motion_generator_position_limits_violation);
- message.joint_motion_generator_velocity_limits_violation =
- static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
- error.joint_motion_generator_velocity_limits_violation);
- message.joint_motion_generator_velocity_discontinuity =
- static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
- error.joint_motion_generator_velocity_discontinuity);
- message.joint_motion_generator_acceleration_discontinuity =
- static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
- error.joint_motion_generator_acceleration_discontinuity);
- message.cartesian_position_motion_generator_start_pose_invalid =
- static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
- error.cartesian_position_motion_generator_start_pose_invalid);
- message.cartesian_motion_generator_elbow_limit_violation =
- static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
- error.cartesian_motion_generator_elbow_limit_violation);
- message.cartesian_motion_generator_velocity_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
- error.cartesian_motion_generator_velocity_limits_violation);
- message.cartesian_motion_generator_velocity_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
- error.cartesian_motion_generator_velocity_discontinuity);
- message.cartesian_motion_generator_acceleration_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
- error.cartesian_motion_generator_acceleration_discontinuity);
- message.cartesian_motion_generator_elbow_sign_inconsistent =
- static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
- error.cartesian_motion_generator_elbow_sign_inconsistent);
- message.cartesian_motion_generator_start_elbow_invalid =
- static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
- error.cartesian_motion_generator_start_elbow_invalid);
- message.cartesian_motion_generator_joint_position_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
- error.cartesian_motion_generator_joint_position_limits_violation);
- message.cartesian_motion_generator_joint_velocity_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
- error.cartesian_motion_generator_joint_velocity_limits_violation);
- message.cartesian_motion_generator_joint_velocity_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
- error.cartesian_motion_generator_joint_velocity_discontinuity);
- message.cartesian_motion_generator_joint_acceleration_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
- error.cartesian_motion_generator_joint_acceleration_discontinuity);
- message.cartesian_position_motion_generator_invalid_frame =
- static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
- error.cartesian_position_motion_generator_invalid_frame);
- message.force_controller_desired_force_tolerance_violation =
- static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
- error.force_controller_desired_force_tolerance_violation);
- message.controller_torque_discontinuity =
- static_cast<decltype(message.controller_torque_discontinuity)>(
- error.controller_torque_discontinuity);
- message.start_elbow_sign_inconsistent =
- static_cast<decltype(message.start_elbow_sign_inconsistent)>(
- error.start_elbow_sign_inconsistent);
- message.communication_constraints_violation =
- static_cast<decltype(message.communication_constraints_violation)>(
- error.communication_constraints_violation);
- message.power_limit_violation =
- static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
- return message;
-}
-
-} // anonymous namespace
-
-namespace franka_hw {
-
-FrankaStateController::FrankaStateController()
- : franka_state_interface_(nullptr),
- franka_state_handle_(nullptr),
- publisher_transforms_(),
- publisher_franka_states_(),
- publisher_joint_states_(),
- publisher_external_wrench_(),
- trigger_publish_(30.0) {}
-
-bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& controller_node_handle) {
- franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
- if (franka_state_interface_ == nullptr) {
- ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
- return false;
- }
- if (!root_node_handle.getParam("arm_id", arm_id_)) {
- ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
- return false;
- }
- double publish_rate(30.0);
- if (controller_node_handle.getParam("publish_rate", publish_rate)) {
- trigger_publish_ = franka_hw::TriggerRate(publish_rate);
- } else {
- ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
- << publish_rate << " [Hz].");
- }
-
- if (!root_node_handle.getParam("joint_names", joint_names_) ||
- joint_names_.size() != robot_state_.q.size()) {
- ROS_ERROR(
- "FrankaStateController: Invalid or no joint_names parameters provided, aborting "
- "controller init!");
- return false;
- }
-
- try {
- franka_state_handle_.reset(
- new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot")));
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM("FrankaStateController: Exception getting cartesian handle: " << ex.what());
- return false;
- }
-
- publisher_transforms_.init(root_node_handle, "/tf", 1);
- publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
- publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
- publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
-
- {
- std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > lock(
- publisher_joint_states_);
- publisher_joint_states_.msg_.name.resize(joint_names_.size());
- publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
- publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
- publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
- }
- {
- std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> > lock(
- publisher_transforms_);
- publisher_transforms_.msg_.transforms.resize(2);
- tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
- tf::Vector3 translation(0.0, 0.0, 0.05);
- tf::Transform transform(quaternion, translation);
- tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
- arm_id_ + "_EE");
- geometry_msgs::TransformStamped transform_message;
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[0] = transform_message;
- translation = tf::Vector3(0.0, 0.0, 0.0);
- transform = tf::Transform(quaternion, translation);
- stamped_transform =
- tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[1] = transform_message;
- }
- {
- std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> > lock(
- publisher_external_wrench_);
- publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
- publisher_external_wrench_.msg_.wrench.force.x = 0.0;
- publisher_external_wrench_.msg_.wrench.force.y = 0.0;
- publisher_external_wrench_.msg_.wrench.force.z = 0.0;
- publisher_external_wrench_.msg_.wrench.torque.x = 0.0;
- publisher_external_wrench_.msg_.wrench.torque.y = 0.0;
- publisher_external_wrench_.msg_.wrench.torque.z = 0.0;
- }
- return true;
-}
-
-void FrankaStateController::update(const ros::Time& time, const ros::Duration& /*period*/) {
- if (trigger_publish_()) {
- robot_state_ = franka_state_handle_->getRobotState();
- publishFrankaStates(time);
- publishTransforms(time);
- publishExternalWrench(time);
- publishJointStates(time);
- sequence_number_++;
- }
-}
-
-void FrankaStateController::publishFrankaStates(const ros::Time& time) {
- if (publisher_franka_states_.trylock()) {
- static_assert(
- sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.K_F_ext_hat_K),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_F_ext_hat_K),
- "Robot state Cartesian members do not have same size");
- for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
- publisher_franka_states_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
- publisher_franka_states_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
- publisher_franka_states_.msg_.K_F_ext_hat_K[i] = robot_state_.K_F_ext_hat_K[i];
- publisher_franka_states_.msg_.O_F_ext_hat_K[i] = robot_state_.O_F_ext_hat_K[i];
- }
-
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
- "Robot state joint members do not have same size");
- for (size_t i = 0; i < robot_state_.q.size(); i++) {
- publisher_franka_states_.msg_.q[i] = robot_state_.q[i];
- publisher_franka_states_.msg_.q_d[i] = robot_state_.q_d[i];
- publisher_franka_states_.msg_.dq[i] = robot_state_.dq[i];
- publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
- publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
- publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
- publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
- publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
- publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
- }
-
- for (size_t i = 0; i < robot_state_.elbow.size(); i++) {
- publisher_franka_states_.msg_.elbow[i] = robot_state_.elbow[i];
- }
-
- for (size_t i = 0; i < robot_state_.elbow_d.size(); i++) {
- publisher_franka_states_.msg_.elbow_d[i] = robot_state_.elbow_d[i];
- }
-
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
- "Robot state transforms do not have same size");
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
- "Robot state transforms do not have same size");
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
- "Robot state transforms do not have same size");
- for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {
- publisher_franka_states_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];
- publisher_franka_states_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];
- publisher_franka_states_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];
- publisher_franka_states_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];
- }
- publisher_franka_states_.msg_.m_load = robot_state_.m_load;
-
- for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
- publisher_franka_states_.msg_.I_load[i] = robot_state_.I_load[i];
- }
-
- for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
- publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
- }
-
- publisher_franka_states_.msg_.time = robot_state_.time.toSec();
- publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
- publisher_franka_states_.msg_.last_motion_errors =
- errorsToMessage(robot_state_.last_motion_errors);
-
- publisher_franka_states_.msg_.header.seq = sequence_number_;
- publisher_franka_states_.msg_.header.stamp = time;
- publisher_franka_states_.unlockAndPublish();
- }
-}
-
-void FrankaStateController::publishJointStates(const ros::Time& time) {
- if (publisher_joint_states_.trylock()) {
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
- "Robot state joint members do not have same size");
- for (size_t i = 0; i < robot_state_.q.size(); i++) {
- publisher_joint_states_.msg_.name[i] = joint_names_[i];
- publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
- publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
- publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];
- }
- publisher_joint_states_.msg_.header.stamp = time;
- publisher_joint_states_.msg_.header.seq = sequence_number_;
- publisher_joint_states_.unlockAndPublish();
- }
-}
-
-void FrankaStateController::publishTransforms(const ros::Time& time) {
- if (publisher_transforms_.trylock()) {
- tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_EE), time,
- arm_id_ + "_link8", arm_id_ + "_EE");
- geometry_msgs::TransformStamped transform_message;
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[0] = transform_message;
- stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,
- arm_id_ + "_EE", arm_id_ + "_K");
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[1] = transform_message;
- publisher_transforms_.unlockAndPublish();
- }
-}
-
-void FrankaStateController::publishExternalWrench(const ros::Time& time) {
- if (publisher_external_wrench_.trylock()) {
- publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
- publisher_external_wrench_.msg_.header.stamp = time;
- publisher_external_wrench_.msg_.wrench.force.x = robot_state_.K_F_ext_hat_K[0];
- publisher_external_wrench_.msg_.wrench.force.y = robot_state_.K_F_ext_hat_K[1];
- publisher_external_wrench_.msg_.wrench.force.z = robot_state_.K_F_ext_hat_K[2];
- publisher_external_wrench_.msg_.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];
- publisher_external_wrench_.msg_.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];
- publisher_external_wrench_.msg_.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];
- publisher_external_wrench_.unlockAndPublish();
- }
-}
-
-} // namespace franka_hw
-
-PLUGINLIB_EXPORT_CLASS(franka_hw::FrankaStateController, controller_interface::ControllerBase)
diff --git a/franka_hw/src/franka_hw_helper_functions.cpp b/franka_hw/src/resource_helpers.cpp
similarity index 99%
rename from franka_hw/src/franka_hw_helper_functions.cpp
rename to franka_hw/src/resource_helpers.cpp
index 11637d33d31c4171253ada8914fedc81d0a22c95..b0ecb250029fcf816d6ae1b1bb08b4a55ca269e9 100644
--- a/franka_hw/src/franka_hw_helper_functions.cpp
+++ b/franka_hw/src/resource_helpers.cpp
@@ -1,4 +1,4 @@
-#include "franka_hw_helper_functions.h"
+#include "resource_helpers.h"
#include <ros/console.h>
diff --git a/franka_hw/src/franka_hw_helper_functions.h b/franka_hw/src/resource_helpers.h
similarity index 53%
rename from franka_hw/src/franka_hw_helper_functions.h
rename to franka_hw/src/resource_helpers.h
index abfe69c1a043dc6ac6674e493c856341e74ff894..1260a9da074eace1e6d073ba2089d07b51d8e850 100644
--- a/franka_hw/src/franka_hw_helper_functions.h
+++ b/franka_hw/src/resource_helpers.h
@@ -1,13 +1,28 @@
#pragma once
#include <list>
+#include <map>
#include <string>
+#include <vector>
-#include <franka_hw/franka_controller_switching_types.h>
#include <hardware_interface/controller_info.h>
+#include <franka_hw/control_mode.h>
+
namespace franka_hw {
+using ResourceWithClaimsMap = std::map<std::string, std::vector<std::vector<std::string>>>;
+
+struct ResourceClaims {
+ uint8_t joint_position_claims = 0;
+ uint8_t joint_velocity_claims = 0;
+ uint8_t joint_torque_claims = 0;
+ uint8_t cartesian_velocity_claims = 0;
+ uint8_t cartesian_pose_claims = 0;
+};
+
+using ArmClaimedMap = std::map<std::string, ResourceClaims>;
+
bool findArmIdInResourceId(const std::string& resource_id, std::string* arm_id);
ResourceWithClaimsMap getResourceMap(const std::list<hardware_interface::ControllerInfo>& info);
diff --git a/franka_hw/srv/SetCartesianImpedance.srv b/franka_hw/srv/SetCartesianImpedance.srv
deleted file mode 100644
index 3a763bae79967532c22ba674055e2b8a87383904..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetCartesianImpedance.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[6] cartesian_stiffness
----
-bool success
-string error
-
diff --git a/franka_hw/srv/SetEEFrame.srv b/franka_hw/srv/SetEEFrame.srv
deleted file mode 100644
index bb7d0fdb7a7a1ecb8d16b533eb64fd5e40b48f50..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetEEFrame.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[16] F_T_EE
----
-bool success
-string error
-
diff --git a/franka_hw/srv/SetForceTorqueCollisionBehavior.srv b/franka_hw/srv/SetForceTorqueCollisionBehavior.srv
deleted file mode 100644
index f6aeaba666c0c4c2f914726480214d91df0171af..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetForceTorqueCollisionBehavior.srv
+++ /dev/null
@@ -1,8 +0,0 @@
-float64[7] lower_torque_thresholds_nominal
-float64[7] upper_torque_thresholds_nominal
-float64[6] lower_force_thresholds_nominal
-float64[6] upper_force_thresholds_nominal
----
-bool success
-string error
-
diff --git a/franka_hw/srv/SetFullCollisionBehavior.srv b/franka_hw/srv/SetFullCollisionBehavior.srv
deleted file mode 100644
index c4d2b2dd00dc94816fb0e1e6d823f10e075da005..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetFullCollisionBehavior.srv
+++ /dev/null
@@ -1,12 +0,0 @@
-float64[7] lower_torque_thresholds_acceleration
-float64[7] upper_torque_thresholds_acceleration
-float64[7] lower_torque_thresholds_nominal
-float64[7] upper_torque_thresholds_nominal
-float64[6] lower_force_thresholds_acceleration
-float64[6] upper_force_thresholds_acceleration
-float64[6] lower_force_thresholds_nominal
-float64[6] upper_force_thresholds_nominal
----
-bool success
-string error
-
diff --git a/franka_hw/srv/SetJointImpedance.srv b/franka_hw/srv/SetJointImpedance.srv
deleted file mode 100644
index c966398ebb8229e501ae6a2ec66036478c880e91..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetJointImpedance.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[7] joint_stiffness
----
-bool success
-string error
-
diff --git a/franka_hw/srv/SetKFrame.srv b/franka_hw/srv/SetKFrame.srv
deleted file mode 100644
index 774c373620be31407c36a000685a56fd30528ebc..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetKFrame.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[16] EE_T_K
----
-bool success
-string error
-
diff --git a/franka_hw/srv/SetLoad.srv b/franka_hw/srv/SetLoad.srv
deleted file mode 100644
index b24211a841dd9f483f0e6a7166eae66619006cdc..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetLoad.srv
+++ /dev/null
@@ -1,7 +0,0 @@
-float64 mass
-float64[3] F_x_center_load
-float64[9] load_inertia
----
-bool success
-string error
-