diff --git a/.clang-tidy b/.clang-tidy
index c4d64102cea300627de8e8b13d42dbea3d4587f7..7fac802e36b9425ad1c0e1a5fb616322f4c83f70 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -1,4 +1,4 @@
-Checks: 'clang-diagnostic-*,clang-analyzer-*,-clang-analyzer-alpha*,google-*,misc-*,readability-*,modernize-*,performance-*,-clang-diagnostic-deprecated-declarations,-modernize-pass-by-value,-clang-diagnostic-reinterpret-base-class,-clang-diagnostic-return-type,-clang-diagnostic-switch'
+Checks: 'clang-diagnostic-*,clang-analyzer-*,-clang-analyzer-optin.cplusplus.VirtualCall,-clang-analyzer-alpha*,google-*,misc-*,readability-*,modernize-*,performance-*,-clang-diagnostic-deprecated-declarations,-modernize-pass-by-value,-clang-diagnostic-reinterpret-base-class,-clang-diagnostic-return-type,-clang-diagnostic-switch'
HeaderFilterRegex: '^franka_.*'
CheckOptions:
# Classes, structs, ...
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5527d9b141f0e7abbc9be5d908215d140f549435..e610347a86fd394e379879effaa652830b2a73aa 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -2,18 +2,30 @@
## 0.7.0 - UNRELEASED
-Requires `libfranka` >= 0.5.0
+Requires `libfranka` >= 0.7.0
- * `franka_control`:
- - Publish whole `libfranka` `franka::RobotState` in `franka_state_controller`.
- * `franka_description`
+ * **BREAKING** moved services and action from `franka_control` to `franka_msgs`.
+ * **BREAKING** moved Service container from `franka_control` to `franka_hw`.
+ * `franka_example_controllers` : Added example for dual-arm control based on franka_combinable_hw.
+ * `franka_description` :
+ - Added an example urdf with two panda arms.
- **BREAKING** Updated collision volumes.
- Removed invalid `axis` for `joint8`.
+ * `franka_hw`:
+ - Added hardware classes to support torque-controlling multiple robots from one controller.
+ - Refactored FrankaHW class to serve as base class (e.g. for FrankaCombinableHW).
+ - Added joint limits checking to FrankaHW which means parameterized warning prints when approaching limits.
+ - Made initial collision behavior a parameter.
+ - Added default constructor and init method to FrankaHW.
+ - **BREAKING** moved parsing of parameters from control node to FrankaHW::init.
+ - **BREAKING** made libfranka robot a member of FrankaHW.
+ - Added missing return value to `franka::ControllerMode` stream operator function.
+ * `franka_control`:
+ - Added control node that can runs a `FrankaCombinedHW` to control mulitple Pandas.
+ - Publish whole `libfranka` `franka::RobotState` in `franka_state_controller`.
* `franka_example_controllers`:
- Cartesian impedance example controller: Interpolate desired orientations with slerp and change orientation error
to quaternion.
- * `franka_hw`:
- - Added missing return value to `franka::ControllerMode` stream operator function.
* **BREAKING** Moved `panda_moveit_config` to [`ros-planning`](https://github.com/ros-planning/panda_moveit_config).
* Added support for ROS Melodic Morenia.
* Raised minimum CMake version to 3.4 to match `libfranka`.
@@ -122,4 +134,3 @@ Requires `libfranka` >= 0.2.0
## 0.1.0 - 2017-09-15
* Initial release
-
diff --git a/cmake/ClangTools.cmake b/cmake/ClangTools.cmake
index 4a356d0296392723adf33d118d41c7111bb1c263..fbc80d6a37b1b59370fe56ea65c70d52b38438d5 100644
--- a/cmake/ClangTools.cmake
+++ b/cmake/ClangTools.cmake
@@ -31,7 +31,7 @@ function(add_format_target _target)
add_dependencies(format format-${_target})
add_custom_target(check-format-${_target}
- COMMAND scripts/format-check.sh ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES}
+ COMMAND ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES} | grep "<replacement " > /dev/null && exit 1 || exit 0
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
COMMENT "Checking ${_target} code formatting with clang-format"
VERBATIM
@@ -55,7 +55,7 @@ function(add_tidy_target _target)
add_dependencies(tidy tidy-${_target})
add_custom_target(check-tidy-${_target}
- COMMAND scripts/fail-on-output.sh ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
+ COMMAND ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES} | grep . && exit 1 || exit 0
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
DEPENDS ${ARG_DEPENDS}
COMMENT "Running clang-tidy for ${_target}"
diff --git a/franka_control/CMakeLists.txt b/franka_control/CMakeLists.txt
index 219f2d08cde4b7638be4f0c614493a6162dfb5be..2abf7e50c5d508a5bc941e01eb2038d94fec3b63 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
@@ -21,34 +19,16 @@ find_package(catkin REQUIRED COMPONENTS
tf2_msgs
)
-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)
+find_package(Franka 0.7.0 REQUIRED)
catkin_package(
INCLUDE_DIRS include
- LIBRARIES franka_state_controller franka_control_services
+ LIBRARIES franka_state_controller
CATKIN_DEPENDS
- actionlib
controller_interface
franka_hw
franka_msgs
geometry_msgs
- message_runtime
pluginlib
realtime_tools
roscpp
@@ -80,54 +60,47 @@ 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}
${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 +131,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..fa103fabc97615ed6f0e37f686e53ba7cdc3d338
--- /dev/null
+++ b/franka_control/config/franka_combined_control_node.yaml
@@ -0,0 +1,67 @@
+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: 1000
+ # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+ internal_controller: joint_impedance
+ # Used to decide whether to enforce realtime mode [enforce|ignore]
+ realtime_config: enforce
+ # 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: 1000
+ # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+ internal_controller: joint_impedance
+ # Used to decide whether to enforce realtime mode [enforce|ignore]
+ realtime_config: enforce
+ # 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 333c390abb729f53bc337ebe99e4aa3d36a9078e..b27d5706d75d7b869de8c1a3d91622e88c0a56f2 100644
--- a/franka_control/config/franka_control_node.yaml
+++ b/franka_control/config/franka_control_node.yaml
@@ -7,6 +7,8 @@ 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.
@@ -15,3 +17,13 @@ cutoff_frequency: 100
internal_controller: joint_impedance
# Used to decide whether to enforce realtime mode [enforce|ignore]
realtime_config: enforce
+# 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 26814b9652d640425f8745d9008980df9b058cea..0000000000000000000000000000000000000000
--- a/franka_control/include/franka_control/services.h
+++ /dev/null
@@ -1,57 +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>
-
-namespace franka_control {
-
-template <typename T>
-ros::ServiceServer advertiseService(
- ros::NodeHandle& node_handle,
- const std::string& name,
- std::function<void(typename T::Request&, typename T::Response&)> handler) {
- return node_handle.advertiseService<typename T::Request, typename T::Response>(
- name, [name, handler](typename T::Request& request, typename T::Response& response) {
- try {
- handler(request, response);
- response.success = true;
- ROS_DEBUG_STREAM(name << " succeeded.");
- } catch (const franka::Exception& ex) {
- ROS_ERROR_STREAM(name << " failed: " << ex.what());
- response.success = false;
- response.error = ex.what();
- }
- return true;
- });
-}
-
-void setCartesianImpedance(franka::Robot& robot,
- const SetCartesianImpedance::Request& req,
- SetCartesianImpedance::Response& res);
-void setJointImpedance(franka::Robot& robot,
- const SetJointImpedance::Request& req,
- SetJointImpedance::Response& res);
-void setEEFrame(franka::Robot& robot, const SetEEFrame::Request& req, SetEEFrame::Response& res);
-void setKFrame(franka::Robot& robot, const SetKFrame::Request& req, SetKFrame::Response& res);
-void setForceTorqueCollisionBehavior(franka::Robot& robot,
- const SetForceTorqueCollisionBehavior::Request& req,
- SetForceTorqueCollisionBehavior::Response& res);
-void setFullCollisionBehavior(franka::Robot& robot,
- const SetFullCollisionBehavior::Request& req,
- SetFullCollisionBehavior::Response& res);
-void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& res);
-
-} // namespace franka_control
diff --git a/franka_control/launch/franka_combined_control.launch b/franka_control/launch/franka_combined_control.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bfdf67336a144b64800d5c199443eb3282dba7a0
--- /dev/null
+++ b/franka_control/launch/franka_combined_control.launch
@@ -0,0 +1,53 @@
+<?xml version="1.0" ?>
+<launch>
+
+ <!-- The path to a robot description (xacro) file to control -->
+ <arg name="robot" default="$(find franka_description)/robots/dual_panda_example.urdf.xacro" />
+
+ <!-- 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="" />
+
+ <!-- The name of the combined robot, used as control node name -->
+ <arg name="robot_id" default="combined_panda" />
+
+ <!-- The config file containing all parameters for the combined hardware node like the IPs of the robots etc. -->
+ <arg name="hw_config_file" default="$(find franka_control)/config/franka_combined_control_node.yaml"/>
+
+ <!-- Optional arg to manually configure the ips of all robots, overwriting ips that are configured in hw_config_file -->
+ <!-- The ips must be set as dictionary like {<arm_id_1>/robot_ip: <my_ip_1>, <arm_id_2>/robot_ip: <my_ip_2>} -->
+ <arg name="robot_ips" />
+
+ <!-- The config file containing the parameterization for all controllers to start with this launch file -->
+ <arg name="controllers_file" default="$(find franka_control)/config/default_combined_controllers.yaml" />
+
+ <!-- The space separated list of controllers to start in this launch files. The controllers should be known from the controllers_file -->
+ <arg name="controllers_to_start" default="panda_1_state_controller panda_2_state_controller"/>
+
+ <!-- The list of the joint_states topics of all combined robots to fuse to a complete topic -->
+ <arg name="joint_states_source_list" default="[panda_1_state_controller/joint_states, panda_2_state_controller/joint_states, panda_1/franka_gripper/joint_states, panda_2/franka_gripper/joint_states]"/>
+
+ <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)" />
+ <!-- Add or overwrite manually configured ips -->
+ <rosparam subst_value="True">$(arg robot_ips)</rosparam>
+ <param name="robot_description" command="xacro --inorder $(arg robot) $(arg args)" />
+ </node>
+
+ <group ns="$(arg robot_id)">
+
+ <rosparam command="load" file="$(arg controllers_file)" />
+ <node name="hw_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
+ args="$(arg controllers_to_start)"/>
+
+ <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="xacro --inorder $(arg robot) $(arg args)" />
+ <rosparam param="source_list" subst_value="true">$(arg joint_states_source_list)</rosparam>
+ <param name="rate" value="30"/>
+ </node>
+
+ </group>
+
+</launch>
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_control/src/franka_combined_control_node.cpp b/franka_control/src/franka_combined_control_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dfcb9ba4f6839b8fb0e37dc27b9bcf9693c28e98
--- /dev/null
+++ b/franka_control/src/franka_combined_control_node.cpp
@@ -0,0 +1,46 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+
+#include <controller_manager/controller_manager.h>
+#include <franka_hw/franka_combined_hw.h>
+#include <ros/ros.h>
+
+#include <franka/control_tools.h>
+#include <sched.h>
+#include <string>
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "franka_combined_control_node");
+
+ ros::AsyncSpinner spinner(4);
+ spinner.start();
+
+ 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;
+ }
+
+ // set current thread to real-time priority
+ std::string error_message;
+ if (!franka::setCurrentThreadToHighestSchedulerPriority(&error_message)) {
+ ROS_ERROR("franka_combined_control_node: Failed to set thread priority to real-time. Error: %s",
+ error_message.c_str());
+ 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();
+ 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 f6fbd71140625f2559571b451ba9e86494c8f062..9562a1a657d5d74731085ce575588a4bd1c4a362 100644
--- a/franka_control/src/franka_control_node.cpp
+++ b/franka_control/src/franka_control_node.cpp
@@ -1,180 +1,52 @@
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <algorithm>
-#include <array>
#include <atomic>
-#include <string>
-#include <utility>
#include <actionlib/server/simple_action_server.h>
#include <controller_manager/controller_manager.h>
#include <franka/exception.h>
#include <franka/robot.h>
#include <franka_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>
-
-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_;
-};
+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");
- 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");
+ 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::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("Invalid or no arm_id parameter provided");
- return 1;
- }
-
- std::string realtime_config_param = node_handle.param("realtime_config", std::string("enforce"));
- franka::RealtimeConfig realtime_config;
- if (realtime_config_param == "enforce") {
- realtime_config = franka::RealtimeConfig::kEnforce;
- } else if (realtime_config_param == "ignore") {
- realtime_config = franka::RealtimeConfig::kIgnore;
- } else {
- ROS_ERROR("Invalid realtime_config parameter provided. Valid values are 'enforce', 'ignore'.");
- return 1;
- }
- franka::Robot robot(robot_ip, realtime_config);
-
- // 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. Valid "
- "values are: 'joint_impedance', 'cartesian_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());
@@ -195,6 +67,7 @@ int main(int argc, char** argv) {
ros::Time now = ros::Time::now();
control_manager.update(now, now - last_time);
+ franka_control.checkJointLimits();
last_time = now;
if (!ros::ok()) {
@@ -204,13 +77,15 @@ 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);
+ franka_control.checkJointLimits();
franka_control.reset();
} else {
control_manager.update(now, period);
+ franka_control.checkJointLimits();
franka_control.enforceLimits(period);
}
return ros::ok();
diff --git a/franka_description/robots/dual_panda_example.urdf.xacro b/franka_description/robots/dual_panda_example.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..f98c133bf96cdfbd61c5d76549a9d2b8f17d494f
--- /dev/null
+++ b/franka_description/robots/dual_panda_example.urdf.xacro
@@ -0,0 +1,36 @@
+<?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>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <geometry>
+ <box size="1 2 1" />
+ </geometry>
+ <material name="White">
+ <color rgba="1.0 1.0 1.0 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <geometry>
+ <box size="1 2 1" />
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- right arm with gripper -->
+ <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 5a14e23b6b4c0c962d970f0458e87f23bf6580c6..3f935f9538d4d248c68678f8f5e3273a7968aac7 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
@@ -20,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS
)
find_package(Eigen3 REQUIRED)
-find_package(Franka 0.5.0 REQUIRED)
+find_package(Franka 0.7.0 REQUIRED)
add_message_files(FILES
JointTorqueComparison.msg
@@ -31,16 +33,21 @@ generate_messages()
generate_dynamic_reconfigure_options(
cfg/compliance_param.cfg
cfg/desired_mass_param.cfg
+ cfg/dual_arm_compliance_param.cfg
)
catkin_package(
+ INCLUDE_DIRS include
LIBRARIES franka_example_controllers
CATKIN_DEPENDS
controller_interface
dynamic_reconfigure
+ eigen_conversions
franka_hw
geometry_msgs
hardware_interface
+ tf
+ tf_conversions
message_runtime
pluginlib
realtime_tools
@@ -58,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
@@ -98,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_example_controllers/cfg/dual_arm_compliance_param.cfg b/franka_example_controllers/cfg/dual_arm_compliance_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..ef80c4138f5093d77a42ab4b2a45070ff045545d
--- /dev/null
+++ b/franka_example_controllers/cfg/dual_arm_compliance_param.cfg
@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "franka_combined_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("left_translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("left_rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("left_nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100)
+
+gen.add("right_translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("right_rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("right_nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "dual_arm_compliance_param"))
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_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..3aab56f0206e39873acb7505c9670dafec81064b
--- /dev/null
+++ b/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
@@ -0,0 +1,182 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <Eigen/Dense>
+
+#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_example_controllers {
+
+/**
+ * This container holds all data and parameters used to control one panda arm with a Cartesian
+ * impedance control law tracking a desired target pose.
+ */
+struct FrankaDataContainer {
+ std::unique_ptr<franka_hw::FrankaStateHandle>
+ state_handle_; ///< To read to complete robot state.
+ std::unique_ptr<franka_hw::FrankaModelHandle>
+ model_handle_; ///< To have access to e.g. jacobians.
+ std::vector<hardware_interface::JointHandle> joint_handles_; ///< To command joint torques.
+ double filter_params_{0.005}; ///< [-] PT1-Filter constant to smooth target values set
+ ///< by dynamic reconfigure servers (stiffness/damping)
+ ///< or interactive markers for the target poses.
+ double nullspace_stiffness_{20.0}; ///< [Nm/rad] To track the initial joint configuration in
+ ///< the nullspace of the Cartesian motion.
+ double nullspace_stiffness_target_{20.0}; ///< [Nm/rad] Unfiltered raw value.
+ const double delta_tau_max_{1.0}; ///< [Nm/ms] Maximum difference in joint-torque per
+ ///< timestep. Used to saturated torque rates to ensure
+ ///< feasible commands.
+ Eigen::Matrix<double, 6, 6> cartesian_stiffness_; ///< To track the target pose.
+ Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_; ///< Unfiltered raw value.
+ Eigen::Matrix<double, 6, 6> cartesian_damping_; ///< To damp cartesian motions.
+ Eigen::Matrix<double, 6, 6> cartesian_damping_target_; ///< Unfiltered raw value.
+ Eigen::Matrix<double, 7, 1> q_d_nullspace_; ///< Target joint pose for nullspace
+ ///< motion. For now we track the
+ ///< initial joint pose.
+ Eigen::Vector3d position_d_; ///< Target position of the end effector.
+ Eigen::Quaterniond orientation_d_; ///< Target orientation of the end effector.
+ Eigen::Vector3d position_d_target_; ///< Unfiltered raw value.
+ Eigen::Quaterniond orientation_d_target_; ///< Unfiltered raw value.
+};
+
+/**
+ * Controller class for ros_control that renders two decoupled Cartesian impedances for the
+ * tracking of two target poses for the two endeffectors. The controller can be reparameterized at
+ * runtime via dynamic reconfigure servers.
+ */
+class DualArmCartesianImpedanceExampleController
+ : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaModelInterface,
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ /**
+ * Initializes the controller class to be ready to run.
+ *
+ * @param[in] robot_hw Pointer to a RobotHW class to get interfaces and resource handles.
+ * @param[in] node_handle Nodehandle that allows getting parameterizations from the server and
+ * starting subscribers.
+ * @return True if the controller was initialized successfully, false otherwise.
+ */
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+
+ /**
+ * Prepares the controller for the real-time execution. This method is executed once every time
+ * the controller is started and runs in real-time.
+ */
+ void starting(const ros::Time&) override;
+
+ /**
+ * Computes the control-law and commands the resulting joint torques to the robot.
+ *
+ * @param[in] period The control period (here 0.001s).
+ */
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ std::map<std::string, FrankaDataContainer>
+ arms_data_; ///< Holds all relevant data for both arms.
+ std::string left_arm_id_; ///< Name of the left arm, retrieved from the parameter server.
+ std::string right_arm_id_; ///< Name of the right arm, retrieved from the parameter server.
+
+ ///< Transformation between base frames of the robots.
+ Eigen::Affine3d Ol_T_Or_; // NOLINT (readability-identifier-naming)
+ ///< Target transformation between the two endeffectors.
+ Eigen::Affine3d EEr_T_EEl_; // NOLINT (readability-identifier-naming)
+ ///< Transformation from the centering frame to the left end effector.
+ Eigen::Affine3d EEl_T_C_;
+
+ ///< Publisher for the centering tracking frame of the coordinated motion.
+ realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> center_frame_pub_;
+ ///< Rate to trigger publishing the current pose of the centering frame.
+ franka_hw::TriggerRate publish_rate_;
+
+ /**
+ * Saturates torque commands to ensure feasibility.
+ *
+ * @param[in] arm_data The data container of the arm.
+ * @param[in] tau_d_calculated The raw command according to the control law.
+ * @param[in] tau_J_d The current desired torque, read from the robot state.
+ * @return The saturated torque command for the 7 joints of one arm.
+ */
+ Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+ const FrankaDataContainer& arm_data,
+ const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+ const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming)
+
+ /**
+ * Initializes a single Panda robot arm.
+ *
+ * @param[in] robot_hw A pointer the RobotHW class for getting interfaces and resource handles.
+ * @param[in] arm_id The name of the panda arm.
+ * @param[in] joint_names The names of all joints of the panda.
+ * @return True if successful, false otherwise.
+ */
+ bool initArm(hardware_interface::RobotHW* robot_hw,
+ const std::string& arm_id,
+ const std::vector<std::string>& joint_names);
+
+ /**
+ * Computes the decoupled controller update for a single arm.
+ *
+ * @param[in] arm_data The data container of the arm to control.
+ */
+ void updateArm(FrankaDataContainer& arm_data);
+
+ /**
+ * Prepares all internal states to be ready to run the real-time control for one arm.
+ *
+ * @param[in] arm_data The data container of the arm to prepare for the control loop.
+ */
+ void startingArm(FrankaDataContainer& arm_data);
+
+ ///< Dynamic reconfigure server
+ std::unique_ptr<dynamic_reconfigure::Server<
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig>>
+ dynamic_server_compliance_param_;
+
+ ///< Nodehandle for the dynamic reconfigure namespace
+ ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
+
+ /**
+ * Callback for updates on the parameterization of the controller in terms of stiffnesses.
+ *
+ * @param[in] config Data container for configuration updates.
+ */
+ void complianceParamCallback(
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig& config,
+ uint32_t /*level*/);
+
+ ///< Target pose subscriber
+ ros::Subscriber sub_target_pose_left_;
+
+ /**
+ * Callback method that handles updates of the target poses.
+ *
+ * @param[in] msg New target pose.
+ */
+ void targetPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+
+ /**
+ * Publishes a Pose Stamped for visualization of the current centering pose.
+ */
+ void publishCenteringPose();
+};
+
+} // namespace franka_example_controllers
diff --git a/franka_example_controllers/src/pseudo_inversion.h b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
similarity index 92%
rename from franka_example_controllers/src/pseudo_inversion.h
rename to franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
index c608b30f906c2b029b4c278f852dd60e124e9790..c820927483012b31290f30226806b70a3c917937 100644
--- a/franka_example_controllers/src/pseudo_inversion.h
+++ b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
@@ -11,6 +11,8 @@
#include <Eigen/LU>
#include <Eigen/SVD>
+namespace franka_example_controllers {
+
inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, bool damped = true) {
double lambda_ = damped ? 0.2 : 0.0;
@@ -24,3 +26,5 @@ inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, b
M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
}
+
+} // 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..9dafe440f537a5e02f5c423c5730464a9e4ed84f
--- /dev/null
+++ b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<launch>
+ <!--Be sure to pass the IPs of your pandas like robot_ips:={panda_1/robot_ip: <my_ip_1>, panda_2/robot_ip: <my_ip_2>} -->
+ <arg name="robot_ips" />
+
+ <arg name="robot_id" default="panda_dual" />
+ <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_ips" value="$(arg robot_ips)" />
+ </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/rviz/franka_dual_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..bdd4b593a530fb3c89e7423473e06f7e7635fa04
--- /dev/null
+++ b/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
@@ -0,0 +1,281 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /InteractiveMarkers1
+ Splitter Ratio: 0.655555546
+ Tree Height: 820
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Class: rviz/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ {}
+ Update Interval: 0
+ Value: false
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: panda_dual/robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/InteractiveMarkers
+ Enable Transparency: true
+ Enabled: true
+ Name: InteractiveMarkers
+ Show Axes: true
+ Show Descriptions: true
+ Show Visual Aids: true
+ Update Topic: /panda_dual/target_pose_marker/update
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 2.63399172
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0.42157951
+ Y: -0.0423400775
+ Z: 1.508255
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.429795891
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 6.16679144
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1020
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a00000375fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000375000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001bc000001e20000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ce000001d10000000000000000fb0000000a0049006d00610067006500000001a9000001f50000000000000000000000010000010f00000365fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f00000365000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003bfc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1855
+ X: 65
+ Y: 28
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_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
new file mode 100755
index 0000000000000000000000000000000000000000..d971629073c15809160441b37e53ddfc3a228286
--- /dev/null
+++ b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
@@ -0,0 +1,253 @@
+#!/usr/bin/env python
+""" This simple script creates an interactive marker for changing desired centering pose of
+ two the dual_panda_cartesian_impedance_example_controller. It features also resetting the
+ marker to current centering pose between the left and the right endeffector.
+"""
+
+import rospy
+import argparse
+
+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
+
+marker_pose = PoseStamped()
+
+centering_frame_ready = False
+
+has_error = False
+left_has_error = False
+right_has_error = False
+
+pose_pub = None
+
+
+def make_sphere(scale=0.3):
+ """
+ This function returns sphere marker for 3D translational movements.
+ :param scale: scales the size of the sphere
+ :return: sphere marker
+ """
+ marker = Marker()
+ marker.type = Marker.SPHERE
+ marker.scale.x = scale * 0.45
+ marker.scale.y = scale * 0.45
+ marker.scale.z = scale * 0.45
+ marker.color.r = 0.5
+ marker.color.g = 0.5
+ marker.color.b = 0.5
+ marker.color.a = 1.0
+ return marker
+
+
+def publish_target_pose():
+ """
+ This function publishes desired centering pose which the controller will subscribe to.
+ :return: None
+ """
+ marker_pose.header.stamp = rospy.Time(0)
+ pose_pub.publish(marker_pose)
+
+
+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: FrankaState msg data
+ :return: None
+ """
+ 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_franka_state_callback(msg):
+ """
+ This callback function set `has_error` variable to True if the right arm is having an error.
+ :param msg: FrankaState msg data
+ :return: None
+ """
+ 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
+
+
+def centering_pose_callback(msg):
+ """
+ This callback function sets the marker pose to the current centering pose from a subscribed topic.
+ :return: None
+ """
+ global centering_frame_ready
+ global marker_pose
+
+ marker_pose = msg
+ centering_frame_ready = True
+
+
+def reset_marker_pose_blocking():
+ """
+ This function resets the marker pose to current "middle pose" of left and right arm EEs.
+ :return: None
+ """
+
+ global centering_frame_ready
+ global marker_pose
+
+ centering_frame_ready = False
+
+ centering_frame_pose_sub = rospy.Subscriber(
+ "dual_arm_cartesian_impedance_example_controller/centering_frame",
+ PoseStamped, centering_pose_callback)
+
+ # Get initial pose for the interactive marker
+ while not centering_frame_ready:
+ rospy.sleep(0.1)
+
+ centering_frame_pose_sub.unregister()
+
+
+def process_feedback(feedback):
+ """
+ This callback function clips the marker_pose inside a predefined box to prevent misuse of the marker.
+ :param feedback: feedback data of interactive marker
+ :return: None
+ """
+ global marker_pose
+ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
+ marker_pose.pose.position.x = feedback.pose.position.x
+ marker_pose.pose.position.y = feedback.pose.position.y
+ marker_pose.pose.position.z = feedback.pose.position.z
+ marker_pose.pose.orientation = feedback.pose.orientation
+ server.applyChanges()
+
+
+if __name__ == "__main__":
+ rospy.init_node("target_pose_node")
+
+ parser = argparse.ArgumentParser("dual_panda_interactive_marker.py")
+ parser.add_argument("--left_arm_id",
+ help="The id of the left arm.",
+ required=True)
+ parser.add_argument("--right_arm_id",
+ help="The id of the right arm.",
+ required=True)
+ parser.add_argument('args', nargs=argparse.REMAINDER)
+ args = parser.parse_args()
+
+ # Arm IDs of left and right arms
+ left_arm_id = args.left_arm_id
+ right_arm_id = args.right_arm_id
+
+ # Initialize subscribers for error states of the arms
+ 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()
+
+ # Initialize publisher for publishing desired centering pose
+ pose_pub = rospy.Publisher(
+ "dual_arm_cartesian_impedance_example_controller/centering_frame_target_pose",
+ PoseStamped,
+ queue_size=1)
+
+ # Interactive marker settings
+ server = InteractiveMarkerServer("target_pose_marker")
+ int_marker = InteractiveMarker()
+ int_marker.header.frame_id = marker_pose.header.frame_id
+ int_marker.scale = 0.3
+ int_marker.name = "centering_frame_pose"
+ int_marker.description = ("Target centering pose\n"
+ "BE CAREFUL! \n"
+ "If you move the target marker\n"
+ "both robots will follow it \n"
+ "as the center between the two\n"
+ "endeffectors. Be aware of\n"
+ "potential collisions!")
+ int_marker.pose = marker_pose.pose
+
+ # insert a box
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "rotate_x"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "move_x"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "rotate_y"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "move_y"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "rotate_z"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "move_z"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 1
+ control.orientation.z = 1
+ control.name = "move_3D"
+ control.always_visible = True
+ control.markers.append(make_sphere())
+ control.interaction_mode = InteractiveMarkerControl.MOVE_3D
+ int_marker.controls.append(control)
+
+ server.insert(int_marker, process_feedback)
+ server.applyChanges()
+
+ # main loop
+ while not rospy.is_shutdown():
+ publish_target_pose()
+ if has_error:
+ reset_marker_pose_blocking()
+ publish_target_pose()
+ server.setPose("centering_frame_pose", marker_pose.pose)
+ server.applyChanges()
+ rospy.sleep(0.5)
+ rospy.sleep(0.1)
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index e93daf9d7937c615da602f0b1a03078641225560..525b7851de43e0ba70e75e84e9d721d9c10b950e 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -10,7 +10,7 @@
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
-#include "pseudo_inversion.h"
+#include <franka_example_controllers/pseudo_inversion.h>
namespace franka_example_controllers {
@@ -113,8 +113,6 @@ void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
std::array<double, 42> jacobian_array =
model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
// convert to eigen
- Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1>> dq_initial(initial_state.dq.data());
Eigen::Map<Eigen::Matrix<double, 7, 1>> q_initial(initial_state.q.data());
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
diff --git a/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aee01b63bd1f33392754375ca340e20e1d2ed265
--- /dev/null
+++ b/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
@@ -0,0 +1,412 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h>
+
+#include <cmath>
+#include <functional>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <franka/robot_state.h>
+#include <franka_example_controllers/pseudo_inversion.h>
+#include <franka_hw/trigger_rate.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <ros/transport_hints.h>
+#include <tf/transform_listener.h>
+#include <tf_conversions/tf_eigen.h>
+
+namespace franka_example_controllers {
+
+bool DualArmCartesianImpedanceExampleController::initArm(
+ hardware_interface::RobotHW* robot_hw,
+ const std::string& arm_id,
+ const std::vector<std::string>& joint_names) {
+ FrankaDataContainer arm_data;
+ auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Error getting model interface from hardware");
+ return false;
+ }
+ try {
+ arm_data.model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+ model_interface->getHandle(arm_id + "_model"));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Exception getting model handle from "
+ "interface: "
+ << ex.what());
+ return false;
+ }
+
+ auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Error getting state interface from hardware");
+ return false;
+ }
+ try {
+ arm_data.state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ state_interface->getHandle(arm_id + "_robot"));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Exception getting state handle from "
+ "interface: "
+ << ex.what());
+ return false;
+ }
+
+ auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
+ if (effort_joint_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Error getting effort joint interface from "
+ "hardware");
+ return false;
+ }
+ for (size_t i = 0; i < 7; ++i) {
+ try {
+ arm_data.joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Exception getting joint handles: "
+ << ex.what());
+ return false;
+ }
+ }
+
+ arm_data.position_d_.setZero();
+ arm_data.orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+ arm_data.position_d_target_.setZero();
+ arm_data.orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+
+ arm_data.cartesian_stiffness_.setZero();
+ arm_data.cartesian_damping_.setZero();
+
+ arms_data_.emplace(std::make_pair(arm_id, std::move(arm_data)));
+
+ return true;
+}
+
+bool DualArmCartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::vector<double> cartesian_stiffness_vector;
+ std::vector<double> cartesian_damping_vector;
+
+ if (!node_handle.getParam("left/arm_id", left_arm_id_)) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Could not read parameter left_arm_id_");
+ return false;
+ }
+ std::vector<std::string> left_joint_names;
+ if (!node_handle.getParam("left/joint_names", left_joint_names) || left_joint_names.size() != 7) {
+ ROS_ERROR(
+ "DualArmCartesianImpedanceExampleController: Invalid or no left_joint_names parameters "
+ "provided, "
+ "aborting controller init!");
+ return false;
+ }
+
+ if (!node_handle.getParam("right/arm_id", right_arm_id_)) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Could not read parameter right_arm_id_");
+ return false;
+ }
+
+ boost::function<void(const geometry_msgs::PoseStamped::ConstPtr&)> callback =
+ boost::bind(&DualArmCartesianImpedanceExampleController::targetPoseCallback, this, _1);
+
+ ros::SubscribeOptions subscribe_options;
+ subscribe_options.init("centering_frame_target_pose", 1, callback);
+ subscribe_options.transport_hints = ros::TransportHints().reliable().tcpNoDelay();
+ sub_target_pose_left_ = node_handle.subscribe(subscribe_options);
+
+ std::vector<std::string> right_joint_names;
+ if (!node_handle.getParam("right/joint_names", right_joint_names) ||
+ right_joint_names.size() != 7) {
+ ROS_ERROR(
+ "DualArmCartesianImpedanceExampleController: Invalid or no right_joint_names parameters "
+ "provided, "
+ "aborting controller init!");
+ return false;
+ }
+
+ bool left_success = initArm(robot_hw, left_arm_id_, left_joint_names);
+ bool right_success = initArm(robot_hw, right_arm_id_, right_joint_names);
+
+ dynamic_reconfigure_compliance_param_node_ =
+ ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
+
+ dynamic_server_compliance_param_ = std::make_unique<dynamic_reconfigure::Server<
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig>>(
+ dynamic_reconfigure_compliance_param_node_);
+
+ dynamic_server_compliance_param_->setCallback(boost::bind(
+ &DualArmCartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
+
+ // Get the transformation from right_O_frame to left_O_frame
+ tf::StampedTransform transform;
+ tf::TransformListener listener;
+ try {
+ if (listener.waitForTransform(left_arm_id_ + "_link0", right_arm_id_ + "_link0", ros::Time(0),
+ ros::Duration(4.0))) {
+ listener.lookupTransform(left_arm_id_ + "_link0", right_arm_id_ + "_link0", ros::Time(0),
+ transform);
+ } else {
+ ROS_ERROR(
+ "DualArmCartesianImpedanceExampleController: Failed to read transform from %s to %s. "
+ "Aborting init!",
+ (right_arm_id_ + "_link0").c_str(), (left_arm_id_ + "_link0").c_str());
+ return false;
+ }
+ } catch (tf::TransformException& ex) {
+ ROS_ERROR("DualArmCartesianImpedanceExampleController: %s", ex.what());
+ return false;
+ }
+ tf::transformTFToEigen(transform, Ol_T_Or_); // NOLINT (readability-identifier-naming)
+
+ // Setup publisher for the centering frame.
+ publish_rate_ = franka_hw::TriggerRate(30.0);
+ center_frame_pub_.init(node_handle, "centering_frame", 1, true);
+
+ return left_success && right_success;
+}
+
+void DualArmCartesianImpedanceExampleController::startingArm(FrankaDataContainer& arm_data) {
+ // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
+ // to initial configuration
+ franka::RobotState initial_state = arm_data.state_handle_->getRobotState();
+ // get jacobian
+ std::array<double, 42> jacobian_array =
+ arm_data.model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+ // convert to eigen
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> dq_initial(initial_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> q_initial(initial_state.q.data());
+ Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+ // set target point to current state
+ arm_data.position_d_ = initial_transform.translation();
+ arm_data.orientation_d_ = Eigen::Quaterniond(initial_transform.linear());
+ arm_data.position_d_target_ = initial_transform.translation();
+ arm_data.orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear());
+
+ // set nullspace target configuration to initial q
+ arm_data.q_d_nullspace_ = q_initial;
+}
+
+void DualArmCartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
+ for (auto& arm_data : arms_data_) {
+ startingArm(arm_data.second);
+ }
+ franka::RobotState robot_state_right =
+ arms_data_.at(right_arm_id_).state_handle_->getRobotState();
+ franka::RobotState robot_state_left = arms_data_.at(left_arm_id_).state_handle_->getRobotState();
+ Eigen::Affine3d Ol_T_EEl(Eigen::Matrix4d::Map( // NOLINT (readability-identifier-naming)
+ robot_state_left.O_T_EE.data())); // NOLINT (readability-identifier-naming)
+ Eigen::Affine3d Or_T_EEr(Eigen::Matrix4d::Map( // NOLINT (readability-identifier-naming)
+ robot_state_right.O_T_EE.data())); // NOLINT (readability-identifier-naming)
+ EEr_T_EEl_ =
+ Or_T_EEr.inverse() * Ol_T_Or_.inverse() * Ol_T_EEl; // NOLINT (readability-identifier-naming)
+ EEl_T_C_.setIdentity();
+ Eigen::Vector3d EEr_r_EEr_EEl = // NOLINT (readability-identifier-naming)
+ EEr_T_EEl_.translation(); // NOLINT (readability-identifier-naming)
+ EEl_T_C_.translation() = -0.5 * EEr_T_EEl_.inverse().rotation() * EEr_r_EEr_EEl;
+}
+
+void DualArmCartesianImpedanceExampleController::update(const ros::Time& /*time*/,
+ const ros::Duration& /*period*/) {
+ for (auto& arm_data : arms_data_) {
+ updateArm(arm_data.second);
+ }
+ if (publish_rate_()) {
+ publishCenteringPose();
+ }
+}
+
+void DualArmCartesianImpedanceExampleController::updateArm(FrankaDataContainer& arm_data) {
+ // get state variables
+ franka::RobotState robot_state = arm_data.state_handle_->getRobotState();
+ std::array<double, 49> inertia_array = arm_data.model_handle_->getMass();
+ std::array<double, 7> coriolis_array = arm_data.model_handle_->getCoriolis();
+ std::array<double, 42> jacobian_array =
+ arm_data.model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+
+ // convert to Eigen
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
+ robot_state.tau_J_d.data());
+ Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
+ Eigen::Vector3d position(transform.translation());
+ Eigen::Quaterniond orientation(transform.linear());
+
+ // compute error to desired pose
+ // position error
+ Eigen::Matrix<double, 6, 1> error;
+ error.head(3) << position - arm_data.position_d_;
+
+ // orientation error
+ if (arm_data.orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) {
+ orientation.coeffs() << -orientation.coeffs();
+ }
+ // "difference" quaternion
+ Eigen::Quaterniond error_quaternion(orientation * arm_data.orientation_d_.inverse());
+ // convert to axis angle
+ Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
+ // compute "orientation error"
+ error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();
+
+ // compute control
+ // allocate variables
+ Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);
+
+ // pseudoinverse for nullspace handling
+ // kinematic pseuoinverse
+ Eigen::MatrixXd jacobian_transpose_pinv;
+ franka_example_controllers::pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);
+
+ // Cartesian PD control with damping ratio = 1
+ tau_task << jacobian.transpose() * (-arm_data.cartesian_stiffness_ * error -
+ arm_data.cartesian_damping_ * (jacobian * dq));
+ // nullspace PD control with damping ratio = 1
+ tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
+ jacobian.transpose() * jacobian_transpose_pinv) *
+ (arm_data.nullspace_stiffness_ * (arm_data.q_d_nullspace_ - q) -
+ (2.0 * sqrt(arm_data.nullspace_stiffness_)) * dq);
+ // Desired torque
+ tau_d << tau_task + tau_nullspace + coriolis;
+ // Saturate torque rate to avoid discontinuities
+ tau_d << saturateTorqueRate(arm_data, tau_d, tau_J_d);
+ for (size_t i = 0; i < 7; ++i) {
+ arm_data.joint_handles_[i].setCommand(tau_d(i));
+ }
+
+ // update parameters changed online either through dynamic reconfigure or through the interactive
+ // target by filtering
+ arm_data.cartesian_stiffness_ = arm_data.filter_params_ * arm_data.cartesian_stiffness_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.cartesian_stiffness_;
+ arm_data.cartesian_damping_ = arm_data.filter_params_ * arm_data.cartesian_damping_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.cartesian_damping_;
+ arm_data.nullspace_stiffness_ = arm_data.filter_params_ * arm_data.nullspace_stiffness_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.nullspace_stiffness_;
+ arm_data.position_d_ = arm_data.filter_params_ * arm_data.position_d_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.position_d_;
+ arm_data.orientation_d_ =
+ arm_data.orientation_d_.slerp(arm_data.filter_params_, arm_data.orientation_d_target_);
+}
+
+Eigen::Matrix<double, 7, 1> DualArmCartesianImpedanceExampleController::saturateTorqueRate(
+ const FrankaDataContainer& arm_data,
+ const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+ const Eigen::Matrix<double, 7, 1>& tau_J_d) { // NOLINT (readability-identifier-naming)
+ Eigen::Matrix<double, 7, 1> 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, arm_data.delta_tau_max_),
+ -arm_data.delta_tau_max_);
+ }
+ return tau_d_saturated;
+}
+
+void DualArmCartesianImpedanceExampleController::complianceParamCallback(
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig& config,
+ uint32_t /*level*/) {
+ auto& left_arm_data = arms_data_.at(left_arm_id_);
+ left_arm_data.cartesian_stiffness_target_.setIdentity();
+ left_arm_data.cartesian_stiffness_target_.topLeftCorner(3, 3)
+ << config.left_translational_stiffness * Eigen::Matrix3d::Identity();
+ left_arm_data.cartesian_stiffness_target_.bottomRightCorner(3, 3)
+ << config.left_rotational_stiffness * Eigen::Matrix3d::Identity();
+ left_arm_data.cartesian_damping_target_.setIdentity();
+
+ left_arm_data.cartesian_damping_target_.topLeftCorner(3, 3)
+ << 2 * sqrt(config.left_translational_stiffness) * Eigen::Matrix3d::Identity();
+ left_arm_data.cartesian_damping_target_.bottomRightCorner(3, 3)
+ << 2 * sqrt(config.left_rotational_stiffness) * Eigen::Matrix3d::Identity();
+ left_arm_data.nullspace_stiffness_target_ = config.left_nullspace_stiffness;
+
+ auto& right_arm_data = arms_data_.at(right_arm_id_);
+ right_arm_data.cartesian_stiffness_target_.setIdentity();
+ right_arm_data.cartesian_stiffness_target_.topLeftCorner(3, 3)
+ << config.right_translational_stiffness * Eigen::Matrix3d::Identity();
+ right_arm_data.cartesian_stiffness_target_.bottomRightCorner(3, 3)
+ << config.right_rotational_stiffness * Eigen::Matrix3d::Identity();
+ right_arm_data.cartesian_damping_target_.setIdentity();
+
+ right_arm_data.cartesian_damping_target_.topLeftCorner(3, 3)
+ << 2 * sqrt(config.right_translational_stiffness) * Eigen::Matrix3d::Identity();
+ right_arm_data.cartesian_damping_target_.bottomRightCorner(3, 3)
+ << 2 * sqrt(config.right_rotational_stiffness) * Eigen::Matrix3d::Identity();
+ right_arm_data.nullspace_stiffness_target_ = config.right_nullspace_stiffness;
+}
+
+void DualArmCartesianImpedanceExampleController::targetPoseCallback(
+ const geometry_msgs::PoseStamped::ConstPtr& msg) {
+ try {
+ if (msg->header.frame_id != left_arm_id_ + "_link0") {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Got pose target with invalid"
+ " frame_id "
+ << msg->header.frame_id << ". Expected " << left_arm_id_ + "_link0");
+ return;
+ }
+
+ // Set target for the left robot.
+ auto& left_arm_data = arms_data_.at(left_arm_id_);
+ Eigen::Affine3d Ol_T_C; // NOLINT (readability-identifier-naming)
+ tf::poseMsgToEigen(msg->pose, Ol_T_C);
+ Eigen::Affine3d Ol_T_EEl_d = // NOLINT (readability-identifier-naming)
+ Ol_T_C * EEl_T_C_.inverse(); // NOLINT (readability-identifier-naming)
+ left_arm_data.position_d_target_ = Ol_T_EEl_d.translation();
+ Eigen::Quaterniond last_orientation_d_target(left_arm_data.orientation_d_target_);
+ Eigen::Quaterniond new_orientation_target(Ol_T_EEl_d.linear());
+ if (last_orientation_d_target.coeffs().dot(new_orientation_target.coeffs()) < 0.0) {
+ new_orientation_target.coeffs() << -new_orientation_target.coeffs();
+ }
+ Ol_T_EEl_d.linear() = new_orientation_target.matrix();
+ left_arm_data.orientation_d_target_ = Ol_T_EEl_d.linear();
+
+ // Compute target for the right endeffector given the static desired transform from left to
+ // right endeffector.
+ Eigen::Affine3d Or_T_EEr_d = Ol_T_Or_.inverse() // NOLINT (readability-identifier-naming)
+ * Ol_T_EEl_d * // NOLINT (readability-identifier-naming)
+ EEr_T_EEl_.inverse(); // NOLINT (readability-identifier-naming)
+ auto& right_arm_data = arms_data_.at(right_arm_id_);
+ right_arm_data.position_d_target_ =
+ Or_T_EEr_d.translation(); // NOLINT (readability-identifier-naming)
+ last_orientation_d_target = Eigen::Quaterniond(right_arm_data.orientation_d_target_);
+ right_arm_data.orientation_d_target_ =
+ Or_T_EEr_d.linear(); // NOLINT (readability-identifier-naming)
+ if (last_orientation_d_target.coeffs().dot(right_arm_data.orientation_d_target_.coeffs()) <
+ 0.0) {
+ right_arm_data.orientation_d_target_.coeffs()
+ << -right_arm_data.orientation_d_target_.coeffs();
+ }
+
+ } catch (std::out_of_range& ex) {
+ ROS_ERROR_STREAM("DualArmCartesianImpedanceExampleController: Exception setting target poses.");
+ }
+}
+
+void DualArmCartesianImpedanceExampleController::publishCenteringPose() {
+ if (center_frame_pub_.trylock()) {
+ franka::RobotState robot_state_left =
+ arms_data_.at(left_arm_id_).state_handle_->getRobotState();
+ Eigen::Affine3d Ol_T_EEl(Eigen::Matrix4d::Map( // NOLINT (readability-identifier-naming)
+ robot_state_left.O_T_EE.data())); // NOLINT (readability-identifier-naming)
+ Eigen::Affine3d Ol_T_C = Ol_T_EEl * EEl_T_C_; // NOLINT (readability-identifier-naming)
+ tf::poseEigenToMsg(Ol_T_C, center_frame_pub_.msg_.pose);
+ center_frame_pub_.msg_.header.frame_id = left_arm_id_ + "_link0";
+ center_frame_pub_.unlockAndPublish();
+ }
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::DualArmCartesianImpedanceExampleController,
+ controller_interface::ControllerBase)
diff --git a/franka_gripper/CMakeLists.txt b/franka_gripper/CMakeLists.txt
index 38070193399771b33f4ab6ac43cecb56e05dbdf8..14dd8524315cf9d7aa8d2b446d93010545fa1096 100644
--- a/franka_gripper/CMakeLists.txt
+++ b/franka_gripper/CMakeLists.txt
@@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
actionlib_msgs
)
-find_package(Franka 0.5.0 REQUIRED)
+find_package(Franka 0.7.0 REQUIRED)
add_action_files(
DIRECTORY action
diff --git a/franka_gripper/config/franka_gripper_node.yaml b/franka_gripper/config/franka_gripper_node.yaml
index 195ec8f399bacbcdf43994ab03b5d9bc8694a023..687e5d3db2cbfb5afd3c3525e3b6f3efb5ca7180 100644
--- a/franka_gripper/config/franka_gripper_node.yaml
+++ b/franka_gripper/config/franka_gripper_node.yaml
@@ -1,7 +1,4 @@
publish_rate: 30 # [Hz]
-joint_names:
- - panda_finger_joint1
- - panda_finger_joint2
default_speed: 0.1 # [m/s]
default_grasp_epsilon:
inner: 0.005 # [m]
diff --git a/franka_gripper/launch/franka_gripper.launch b/franka_gripper/launch/franka_gripper.launch
index 049bc3d54d434806f6c8abdc04cb7dbaff8f5fb2..d56656352efaf03356d7d3a82ccfe528f2003013 100644
--- a/franka_gripper/launch/franka_gripper.launch
+++ b/franka_gripper/launch/franka_gripper.launch
@@ -1,9 +1,13 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" />
+ <arg name="arm_id" default="panda" />
+ <arg name="joint_names" default="[$(arg arm_id)_finger_joint1, $(arg arm_id)_finger_joint2]" />
<node name="franka_gripper" pkg="franka_gripper" type="franka_gripper_node" output="screen">
<param name="robot_ip" value="$(arg robot_ip)"/>
<rosparam command="load" file="$(find franka_gripper)/config/franka_gripper_node.yaml" />
+ <rosparam param="joint_names" subst_value="true">$(arg joint_names)</rosparam>
</node>
+
</launch>
diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
index db3f0916e77478974fbd3e4cbd2f615471d83665..417b816f8f529274f51081db88926bfc0b32bba9 100644
--- a/franka_hw/CMakeLists.txt
+++ b/franka_hw/CMakeLists.txt
@@ -5,25 +5,42 @@ 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)
+find_package(Franka 0.7.0 REQUIRED)
catkin_package(
INCLUDE_DIRS include
- LIBRARIES franka_hw
- CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp urdf
+ LIBRARIES franka_hw franka_control_services
+ 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..bc2dffd8da5f550d041bf5204b9d318fe3f20f3b
--- /dev/null
+++ b/franka_hw/include/franka_hw/franka_combinable_hw.h
@@ -0,0 +1,170 @@
+// 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 multiple 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 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(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 multiple
+ * instances of FrankaCombinableHW.
+ *
+ * @return A copy of the arm_id string identifying 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();
+ {
+ std::lock_guard<std::mutex> state_lock(libfranka_state_mutex_);
+ robot_state_libfranka_ = robot_state;
+ }
+
+ std::lock_guard<std::mutex> command_lock(libfranka_cmd_mutex_);
+ T current_cmd = command;
+ if (has_error_ || !controller_active_) {
+ return franka::MotionFinished(current_cmd);
+ }
+ return current_cmd;
+ }
+
+ void publishErrorState(bool error);
+
+ void setupServicesAndActionServers(ros::NodeHandle& node_handle);
+
+ void initRobot() override;
+
+ bool setRunFunction(const ControlMode& requested_control_mode,
+ bool limit_rate,
+ double cutoff_frequency,
+ franka::ControllerMode internal_controller) override;
+
+ void controlLoop();
+
+ 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_hw/include/franka_hw/franka_combined_hw.h b/franka_hw/include/franka_hw/franka_combined_hw.h
new file mode 100644
index 0000000000000000000000000000000000000000..3072f31429090fad85c805134dddd4b5845edd7e
--- /dev/null
+++ b/franka_hw/include/franka_hw/franka_combined_hw.h
@@ -0,0 +1,64 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <combined_robot_hw/combined_robot_hw.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>
+#include <ros/time.h>
+
+#include <memory>
+
+namespace franka_hw {
+
+class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW {
+ public:
+ /**
+ * Creates an instance of CombinedFrankaHW.
+ *
+ */
+ FrankaCombinedHW();
+
+ ~FrankaCombinedHW() override = default; // NOLINT (clang-analyzer-optin.cplusplus.VirtualCall)
+
+ /**
+ * The init function is called to initialize the CombinedFrankaHW from a
+ * non-realtime thread.
+ *
+ * @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.
+ */
+ bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
+
+ /**
+ * Reads data from the robot HW
+ *
+ * @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;
+
+ /**
+ * Checks whether the controller needs to be reset.
+ *
+ * @return True if the controllers needs to be reset, false otherwise.
+ */
+ bool controllerNeedsReset();
+
+ protected:
+ std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
+ combined_recovery_action_server_;
+
+ private:
+ void handleError();
+ bool hasError();
+ void triggerError();
+ bool is_recovering_{false};
+};
+
+} // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h
index 3950d81fbca89d91b2ef96dff788c26021b5acef..2b6ee5eb2b17be4d2dd44bf9493e29647c00daf0 100644
--- a/franka_hw/include/franka_hw/franka_hw.h
+++ b/franka_hw/include/franka_hw/franka_hw.h
@@ -4,77 +4,87 @@
#include <array>
#include <atomic>
+#include <exception>
#include <functional>
+#include <list>
#include <string>
+#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/model.h>
+#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 <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 <joint_limits_interface/joint_limits_urdf.h>
#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; });
-
- ~FrankaHW() override = default;
+ /**
+ * 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 parameterization 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.
@@ -82,7 +92,6 @@ class FrankaHW : public hardware_interface::RobotHW {
* 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.
*
@@ -92,25 +101,22 @@ class FrankaHW : public hardware_interface::RobotHW {
* @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,
- const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback);
+ virtual void control(
+ 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.
*/
- void update(const franka::RobotState& robot_state);
+ virtual 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;
+ virtual bool controllerActive() const noexcept;
/**
* Checks whether a requested controller can be run, based on the resources
@@ -120,13 +126,15 @@ class FrankaHW : public hardware_interface::RobotHW {
*
* @return True in case of a conflict, false in case of valid controllers.
*/
- bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override;
+ virtual 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;
+ 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).
@@ -136,88 +144,373 @@ class FrankaHW : public hardware_interface::RobotHW {
*
* @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;
+ virtual bool prepareSwitch(
+ const std::list<hardware_interface::ControllerInfo>& start_list,
+ const std::list<hardware_interface::ControllerInfo>& stop_list) override;
/**
* Gets the current joint position command.
*
- * @return Current joint position command.
+ * @return The current joint position command.
*/
- std::array<double, 7> getJointPositionCommand() const noexcept;
+ 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.
*/
- std::array<double, 7> getJointVelocityCommand() const noexcept;
+ 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.
*/
- std::array<double, 7> getJointEffortCommand() const noexcept;
+ 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.
*/
- void enforceLimits(const ros::Duration& period);
+ virtual void enforceLimits(const ros::Duration& period);
/**
* Resets the limit interfaces.
*/
- void reset();
+ virtual void reset();
+
+ /**
+ * Checks the proximity of each joint to its joint position limits and prints
+ * a warning whenever a joint is close to a limit.
+ */
+ 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); });
+ }
- private:
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(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);
+ 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 "
+ "limit interface!",
+ joint_name.c_str());
+ continue;
+ }
+ if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
+ if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
+ joint_limits.max_acceleration = franka::kMaxJointAcceleration[i];
+ joint_limits.has_acceleration_limits = true;
+ joint_limits.max_jerk = franka::kMaxJointJerk[i];
+ joint_limits.has_jerk_limits = true;
+ T limit_handle(command_interface.getHandle(joint_name), joint_limits, soft_limits);
+ limit_interface.registerHandle(limit_handle);
+ } else {
+ ROS_ERROR(
+ "FrankaHW: Could not parse joint limit for joint: %s for joint limit interfaces",
+ joint_name.c_str());
+ }
+ } else {
+ ROS_ERROR(
+ "FrankaHW: Could not parse soft joint limit for joint %s for joint limit interfaces",
+ joint_name.c_str());
+ }
+ }
+ }
+
+ /**
+ * 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,
+ bool use_q_d,
+ T& interface) {
+ for (size_t i = 0; i < joint_names_.size(); i++) {
+ hardware_interface::JointStateHandle state_handle;
+ if (use_q_d) {
+ state_handle = hardware_interface::JointStateHandle(joint_names_[i], &state.q_d[i],
+ &state.dq_d[i], &state.tau_J[i]);
+ } else {
+ state_handle = hardware_interface::JointStateHandle(joint_names_[i], &state.q[i],
+ &state.dq[i], &state.tau_J[i]);
+ }
+ hardware_interface::JointHandle joint_handle(state_handle, &command[i]);
+ interface.registerHandle(joint_handle);
+ }
+ 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);
+
+ /**
+ * 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,
+ bool limit_rate,
+ double cutoff_frequency,
+ franka::ControllerMode internal_controller);
+ /**
+ * Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model.
+ */
+ virtual void initRobot();
+
+ /**
+ * 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_{};
- franka_hw::FrankaStateInterface franka_state_interface_{};
+ FrankaStateInterface franka_state_interface_{};
hardware_interface::PositionJointInterface position_joint_interface_{};
hardware_interface::VelocityJointInterface velocity_joint_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_{};
-
+ FrankaPoseCartesianInterface franka_pose_cartesian_interface_{};
+ FrankaVelocityCartesianInterface franka_velocity_cartesian_interface_{};
+ FrankaModelInterface franka_model_interface_{};
joint_limits_interface::PositionJointSoftLimitsInterface position_joint_limit_interface_{};
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};
+ franka::RealtimeConfig realtime_config_;
+
+ 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/src/resource_helpers.h b/franka_hw/include/franka_hw/resource_helpers.h
similarity index 72%
rename from franka_hw/src/resource_helpers.h
rename to franka_hw/include/franka_hw/resource_helpers.h
index 15f00b9df93252519e7e953a76c9e2d56b059030..f2231ba58d3d3867f2942b6e22224d19087e6e46 100644
--- a/franka_hw/src/resource_helpers.h
+++ b/franka_hw/include/franka_hw/resource_helpers.h
@@ -33,4 +33,13 @@ bool getArmClaimedMap(ResourceWithClaimsMap& resource_map, ArmClaimedMap& arm_cl
ControlMode getControlMode(const std::string& arm_id, ArmClaimedMap& arm_claim_map);
+bool hasConflictingMultiClaim(const ResourceWithClaimsMap& resource_map);
+
+bool hasConflictingJointAndCartesianClaim(const ArmClaimedMap& arm_claim_map,
+ const std::string& arm_id);
+
+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..5d010b31ba67a3e292ff9777af61f22e0fad40e2
--- /dev/null
+++ b/franka_hw/include/franka_hw/services.h
@@ -0,0 +1,160 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+#include <vector>
+
+#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>
+
+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..3b2b1650a195d730baa9766cf071151153e1af8e
--- /dev/null
+++ b/franka_hw/src/franka_combinable_hw.cpp
@@ -0,0 +1,218 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_hw/franka_combinable_hw.h>
+
+#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/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();
+
+ {
+ std::lock_guard<std::mutex> ros_state_lock(ros_state_mutex_);
+ std::lock_guard<std::mutex> libfranka_state_lock(libfranka_state_mutex_);
+ robot_state_libfranka_ = robot_->readOnce();
+ robot_state_ros_ = robot_->readOnce();
+ }
+
+ if (!ros::ok()) {
+ return;
+ }
+ }
+ ROS_INFO("FrankaCombinableHW::%s::control_loop(): controller is active.", arm_id_.c_str());
+
+ // Reset commands
+ {
+ std::lock_guard<std::mutex> command_lock(libfranka_cmd_mutex_);
+ effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.});
+ }
+
+ try {
+ control();
+ } catch (const franka::ControlException& e) {
+ // Reflex could be caught and it needs to wait for automatic error recovery
+ ROS_ERROR("%s: %s", arm_id_.c_str(), 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::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_hw/src/franka_combined_hw.cpp b/franka_hw/src/franka_combined_hw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..357b5bb224e4636b6b3b3f701c7238b215e8d9d0
--- /dev/null
+++ b/franka_hw/src/franka_combined_hw.cpp
@@ -0,0 +1,111 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_hw/franka_combined_hw.h>
+
+#include <algorithm>
+#include <memory>
+
+#include <actionlib/server/simple_action_server.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_combinable_hw.h>
+#include <franka_hw/franka_hw.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
+
+namespace franka_hw {
+
+FrankaCombinedHW::FrankaCombinedHW() = default;
+
+bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
+ 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_msgs::ErrorRecoveryAction>>(
+ robot_hw_nh, "error_recovery",
+ [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
+ try {
+ is_recovering_ = true;
+ for (const auto& robot_hw : robot_hw_list_) {
+ auto* franka_combinable_hw_ptr =
+ dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get());
+ if (franka_combinable_hw_ptr != nullptr) {
+ franka_combinable_hw_ptr->resetError();
+ } else {
+ ROS_ERROR(
+ "FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed.");
+ is_recovering_ = false;
+ combined_recovery_action_server_->setAborted(
+ franka_msgs::ErrorRecoveryResult(),
+ "dynamic_cast from RobotHW to FrankaCombinableHW failed");
+ return;
+ }
+ }
+ is_recovering_ = false;
+ combined_recovery_action_server_->setSucceeded();
+ } catch (const franka::Exception& ex) {
+ is_recovering_ = false;
+ combined_recovery_action_server_->setAborted(franka_msgs::ErrorRecoveryResult(),
+ ex.what());
+ }
+ },
+ false);
+ combined_recovery_action_server_->start();
+ return success;
+}
+
+void FrankaCombinedHW::read(const ros::Time& time, const ros::Duration& period) {
+ // Call the read method of the single RobotHW objects.
+ CombinedRobotHW::read(time, period);
+ handleError();
+}
+
+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_hw::FrankaCombinableHW*>(robot_hw.get());
+ if (franka_combinable_hw_ptr != nullptr) {
+ controller_reset = controller_reset || franka_combinable_hw_ptr->controllerNeedsReset();
+ } else {
+ ROS_ERROR("FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed.");
+ return false;
+ }
+ }
+ return controller_reset;
+}
+
+void FrankaCombinedHW::handleError() {
+ // Trigger error state of all other RobotHW objects when one of them has a error.
+ if (hasError() && !is_recovering_) {
+ triggerError();
+ }
+}
+
+bool FrankaCombinedHW::hasError() {
+ bool has_error = false;
+ for (const auto& robot_hw : robot_hw_list_) {
+ 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 {
+ ROS_ERROR("FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed.");
+ return false;
+ }
+ }
+ return has_error;
+}
+
+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_hw::FrankaCombinableHW*>(robot_hw.get());
+ if (franka_combinable_hw_ptr != nullptr) {
+ franka_combinable_hw_ptr->triggerError();
+ } else {
+ ROS_ERROR("FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed.");
+ }
+ }
+}
+
+} // namespace franka_hw
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index 75ca901da3be4255154d9c4090a7719349702a21..2fce7191d981bcde5dda8e6fb86006a7b6217eaf 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -1,16 +1,24 @@
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_hw/franka_hw.h>
+#include <franka_hw/resource_helpers.h>
+#include <array>
#include <cstdint>
+#include <exception>
+#include <functional>
+#include <list>
+#include <mutex>
#include <ostream>
+#include <sstream>
+#include <string>
#include <utility>
+#include <franka/control_types.h>
#include <franka/rate_limiting.h>
+#include <franka/robot.h>
#include <joint_limits_interface/joint_limits_urdf.h>
-#include "resource_helpers.h"
-
namespace franka_hw {
namespace {
@@ -24,133 +32,153 @@ std::ostream& operator<<(std::ostream& ostream, franka::ControllerMode mode) {
}
return ostream;
}
+
+std::string toStringWithPrecision(const double value, const size_t precision = 6) {
+ std::ostringstream out;
+ out.precision(precision);
+ out << std::fixed << value;
+ return out.str();
+}
+
} // 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}) {
- for (size_t i = 0; i < joint_names_.size(); i++) {
- hardware_interface::JointStateHandle joint_handle_q_d(
- joint_names_[i], &robot_state_.q_d[i], &robot_state_.dq_d[i], &robot_state_.tau_J[i]);
+ 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}) {}
- hardware_interface::JointStateHandle joint_handle_q(
- joint_names_[i], &robot_state_.q[i], &robot_state_.dq[i], &robot_state_.tau_J[i]);
+bool FrankaHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
+ if (initialized_) {
+ ROS_ERROR("FrankaHW: Cannot be initialized twice.");
+ return false;
+ }
- joint_state_interface_.registerHandle(joint_handle_q);
+ 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);
- hardware_interface::JointHandle position_joint_handle(joint_handle_q_d,
- &position_joint_command_.q[i]);
- position_joint_interface_.registerHandle(position_joint_handle);
+ initialized_ = true;
+ return true;
+}
- hardware_interface::JointHandle velocity_joint_handle(joint_handle_q_d,
- &velocity_joint_command_.dq[i]);
- velocity_joint_interface_.registerHandle(velocity_joint_handle);
+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());
- hardware_interface::JointHandle effort_joint_handle(joint_handle_q,
- &effort_joint_command_.tau_J[i]);
- effort_joint_interface_.registerHandle(effort_joint_handle);
+ bool rate_limiting;
+ if (!robot_hw_nh.getParamCached("rate_limiting", rate_limiting)) {
+ ROS_ERROR("Invalid or no rate_limiting parameter provided");
+ return false;
}
- joint_limits_interface::SoftJointLimits soft_limits;
- joint_limits_interface::JointLimits joint_limits;
+ double cutoff_frequency;
+ if (!robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency)) {
+ ROS_ERROR("Invalid or no cutoff_frequency parameter provided");
+ return false;
+ }
- 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);
- if (!urdf_joint) {
- ROS_ERROR_STREAM("FrankaHW: Could not get joint " << joint_name << " from urdf");
- continue;
- }
- if (!urdf_joint->safety) {
- ROS_ERROR_STREAM("FrankaHW: Joint " << joint_name << " has no safety");
- continue;
- }
- if (!urdf_joint->limits) {
- ROS_ERROR_STREAM("FrankaHW: Joint " << joint_name << " has no limits");
- continue;
- }
+ std::string internal_controller;
+ if (!robot_hw_nh.getParam("internal_controller", internal_controller)) {
+ ROS_ERROR("No internal_controller parameter provided");
+ return false;
+ }
- if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
- if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
- joint_limits.max_acceleration = franka::kMaxJointAcceleration[i];
- joint_limits.has_acceleration_limits = true;
- joint_limits.max_jerk = franka::kMaxJointJerk[i];
- joint_limits.has_jerk_limits = true;
- joint_limits_interface::PositionJointSoftLimitsHandle position_limit_handle(
- position_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
- position_joint_limit_interface_.registerHandle(position_limit_handle);
-
- joint_limits_interface::VelocityJointSoftLimitsHandle velocity_limit_handle(
- velocity_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
- velocity_joint_limit_interface_.registerHandle(velocity_limit_handle);
-
- 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("FrankaHW: Could not parse joint limit for joint "
- << joint_name << " for joint limit interfaces");
- }
- } else {
- ROS_ERROR_STREAM("FrankaHW: Could not parse soft joint limit for joint "
- << joint_name << " for joint limit interfaces");
- }
+ if (!robot_hw_nh.getParam("arm_id", arm_id_)) {
+ ROS_ERROR("Invalid or no arm_id parameter provided");
+ return false;
}
- FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state_);
- franka_state_interface_.registerHandle(franka_state_handle);
- FrankaCartesianPoseHandle franka_cartesian_pose_handle(
- franka_state_handle, pose_cartesian_command_.O_T_EE, pose_cartesian_command_.elbow);
- franka_pose_cartesian_interface_.registerHandle(franka_cartesian_pose_handle);
- FrankaCartesianVelocityHandle franka_cartesian_velocity_handle(
- franka_state_handle, velocity_cartesian_command_.O_dP_EE, velocity_cartesian_command_.elbow);
- franka_velocity_cartesian_interface_.registerHandle(franka_cartesian_velocity_handle);
+ if (!urdf_model_.initParamWithNodeHandle("robot_description", root_nh)) {
+ ROS_ERROR("Could not initialize URDF model from robot_description");
+ return false;
+ }
- registerInterface(&franka_state_interface_);
- registerInterface(&joint_state_interface_);
- registerInterface(&position_joint_interface_);
- registerInterface(&velocity_joint_interface_);
- registerInterface(&effort_joint_interface_);
- registerInterface(&franka_pose_cartesian_interface_);
- registerInterface(&franka_velocity_cartesian_interface_);
-}
+ 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_);
+ }
-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)) {
- franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", model, robot_state_);
+ std::string realtime_config_param = robot_hw_nh.param("realtime_config", std::string("enforce"));
+ if (realtime_config_param == "enforce") {
+ realtime_config_ = franka::RealtimeConfig::kEnforce;
+ } else if (realtime_config_param == "ignore") {
+ realtime_config_ = franka::RealtimeConfig::kIgnore;
+ } else {
+ ROS_ERROR("Invalid realtime_config parameter provided. Valid values are 'enforce', 'ignore'.");
+ return false;
+ }
- franka_model_interface_.registerHandle(model_handle);
+ // 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());
- registerInterface(&franka_model_interface_);
+ 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 {
@@ -158,18 +186,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;
@@ -186,68 +218,19 @@ void FrankaHW::enforceLimits(const ros::Duration& period) {
bool FrankaHW::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("FrankaHW: 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 != 1) {
- ROS_ERROR_STREAM("FrankaHW: Resource "
- << map_it->first
- << " is claimed with two non-compatible interfaces. Conflict!");
- return true;
- }
- }
+ if (hasConflictingMultiClaim(resource_map)) {
+ return true;
}
-
ArmClaimedMap arm_claim_map;
if (!getArmClaimedMap(resource_map, arm_claim_map)) {
ROS_ERROR_STREAM("FrankaHW: Unknown interface claimed. 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("FrankaHW: 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("FrankaHW: Non-consistent claims on the joints of "
- << arm_id_ << ". Not supported. Conflict!");
- return true;
- }
- }
- return false;
+ return hasConflictingJointAndCartesianClaim(arm_claim_map, arm_id_) ||
+ partiallyClaimsArmJoints(arm_claim_map, arm_id_);
}
-// doSwitch runs on the main realtime thread
+// doSwitch runs on the main realtime thread.
void FrankaHW::doSwitch(const std::list<hardware_interface::ControllerInfo>& /* start_list */,
const std::list<hardware_interface::ControllerInfo>& /* stop_list */) {
if (current_control_mode_ != ControlMode::None) {
@@ -256,7 +239,7 @@ void FrankaHW::doSwitch(const std::list<hardware_interface::ControllerInfo>& /*
}
}
-// prepareSwitch runs on the background message handling thread
+// prepareSwitch runs on the background message handling thread.
bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) {
ResourceWithClaimsMap start_resource_map = getResourceMap(start_list);
@@ -265,6 +248,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);
@@ -279,12 +263,146 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
requested_control_mode &= ~stop_control_mode;
requested_control_mode |= start_control_mode;
- auto limit_rate = get_limit_rate_();
- auto cutoff_frequency = get_cutoff_frequency_();
- auto internal_controller = get_internal_controller_();
+ if (!setRunFunction(requested_control_mode, get_limit_rate_(), get_cutoff_frequency_(),
+ 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 "
+ << "limit_rate=" << get_limit_rate_()
+ << ", cutoff_frequency=" << get_cutoff_frequency_()
+ << ", internal_controller=" << get_internal_controller_());
+ current_control_mode_ = requested_control_mode;
+ controller_active_ = false;
+ }
+
+ return true;
+}
+
+std::array<double, 7> FrankaHW::getJointPositionCommand() const noexcept {
+ return position_joint_command_ros_.q;
+}
+
+std::array<double, 7> FrankaHW::getJointVelocityCommand() const noexcept {
+ return velocity_joint_command_ros_.dq;
+}
+
+std::array<double, 7> FrankaHW::getJointEffortCommand() const noexcept {
+ return effort_joint_command_ros_.tau_J;
+}
+
+void FrankaHW::reset() {
+ position_joint_limit_interface_.reset();
+}
+
+void FrankaHW::checkJointLimits() {
+ 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 (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 + ": " + toStringWithPrecision(dist * 180 / M_PI) +
+ " degrees to joint limits (limits: [" + toStringWithPrecision(joint_lower) + ", " +
+ toStringWithPrecision(joint_upper) + "]" +
+ " q: " + toStringWithPrecision(joint_position) + ") ";
+ }
+ } else {
+ ROS_ERROR_STREAM_ONCE("FrankaHW: Could not parse joint limit for joint "
+ << k_joint_name << " for joint limit interfaces");
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM_ONCE("FrankaHW: Could not get joint handle " << k_joint_name << " .\n"
+ << ex.what());
+ return;
+ }
+ }
+ if (!joint_limits_warning.empty()) {
+ ROS_WARN_THROTTLE(5, "FrankaHW: %s", joint_limits_warning.c_str());
+ }
+}
+
+franka::Robot& FrankaHW::robot() const {
+ if (!initialized_ || !robot_) {
+ std::string error_message = "FrankaHW: Attempt 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],
+ &robot_state.dq[i], &robot_state.tau_J[i]);
+ joint_state_interface_.registerHandle(joint_handle_q);
+ }
+ registerInterface(&joint_state_interface_);
+}
+
+void FrankaHW::setupFrankaStateInterface(franka::RobotState& robot_state) {
+ FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state);
+ franka_state_interface_.registerHandle(franka_state_handle);
+ registerInterface(&franka_state_interface_);
+}
+
+void FrankaHW::setupFrankaCartesianPoseInterface(franka::CartesianPose& pose_cartesian_command) {
+ FrankaCartesianPoseHandle franka_cartesian_pose_handle(
+ franka_state_interface_.getHandle(arm_id_ + "_robot"), pose_cartesian_command.O_T_EE,
+ pose_cartesian_command.elbow);
+ franka_pose_cartesian_interface_.registerHandle(franka_cartesian_pose_handle);
+ registerInterface(&franka_pose_cartesian_interface_);
+}
+
+void FrankaHW::setupFrankaCartesianVelocityInterface(
+ franka::CartesianVelocities& velocity_cartesian_command) {
+ FrankaCartesianVelocityHandle franka_cartesian_velocity_handle(
+ franka_state_interface_.getHandle(arm_id_ + "_robot"), velocity_cartesian_command.O_dP_EE,
+ velocity_cartesian_command.elbow);
+ franka_velocity_cartesian_interface_.registerHandle(franka_cartesian_velocity_handle);
+ registerInterface(&franka_velocity_cartesian_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,
+ const bool limit_rate,
+ const double cutoff_frequency,
+ const franka::ControllerMode internal_controller) {
using std::placeholders::_1;
using std::placeholders::_2;
+ using Callback = std::function<bool(const franka::RobotState&, franka::Duration)>;
switch (requested_control_mode) {
case ControlMode::None:
@@ -292,106 +410,181 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
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:
ROS_WARN("FrankaHW: No valid control mode selected; cannot switch controllers.");
return false;
}
+ return true;
+}
- if (current_control_mode_ != requested_control_mode) {
- ROS_INFO_STREAM("FrankaHW: Prepared switching controllers to "
- << requested_control_mode << " with parameters "
- << "limit_rate=" << limit_rate << ", cutoff_frequency=" << cutoff_frequency
- << ", internal_controller=" << internal_controller);
- current_control_mode_ = requested_control_mode;
+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_ros_.dq, robot_state_ros_, true,
+ velocity_joint_interface_);
+ setupJointCommandInterface(effort_joint_command_ros_.tau_J, robot_state_ros_, false,
+ effort_joint_interface_);
+ setupLimitInterface<joint_limits_interface::PositionJointSoftLimitsHandle>(
+ position_joint_limit_interface_, position_joint_interface_);
+ setupLimitInterface<joint_limits_interface::VelocityJointSoftLimitsHandle>(
+ velocity_joint_limit_interface_, velocity_joint_interface_);
+ setupLimitInterface<joint_limits_interface::EffortJointSoftLimitsHandle>(
+ 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_);
+}
- controller_active_ = false;
- }
+void FrankaHW::initRobot() {
+ robot_ = std::make_unique<franka::Robot>(robot_ip_, realtime_config_);
+ 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());
+}
- return true;
+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;
+ };
}
-std::array<double, 7> FrankaHW::getJointPositionCommand() const noexcept {
- return position_joint_command_.q;
+bool FrankaHW::commandHasNaN(const franka::Torques& command) {
+ return arrayHasNaN(command.tau_J);
}
-std::array<double, 7> FrankaHW::getJointVelocityCommand() const noexcept {
- return velocity_joint_command_.dq;
+bool FrankaHW::commandHasNaN(const franka::JointPositions& command) {
+ return arrayHasNaN(command.q);
}
-std::array<double, 7> FrankaHW::getJointEffortCommand() const noexcept {
- return effort_joint_command_.tau_J;
+bool FrankaHW::commandHasNaN(const franka::JointVelocities& command) {
+ return arrayHasNaN(command.dq);
}
-void FrankaHW::reset() {
- position_joint_limit_interface_.reset();
+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 d8a2166bcfe9d88c7b5e6ddca243987f38060b0c..f9219967f7560a1b83b53ae7de396c91ecfbb317 100644
--- a/franka_hw/src/resource_helpers.cpp
+++ b/franka_hw/src/resource_helpers.cpp
@@ -1,6 +1,6 @@
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include "resource_helpers.h"
+#include <franka_hw/resource_helpers.h>
#include <ros/console.h>
@@ -46,9 +46,8 @@ bool getArmClaimedMap(ResourceWithClaimsMap& resource_map, ArmClaimedMap& arm_cl
// 7 non-torque claims on joint_level or one claim on cartesian level.
for (auto map_it = resource_map.begin(); map_it != resource_map.end(); map_it++) {
if (!findArmIdInResourceId(map_it->first, ¤t_arm_id)) {
- ROS_ERROR_STREAM("Could not find arm_id in resource "
- << map_it->first
- << ". Conflict! Name joints as '<robot_arm_id>_joint<jointnumber>'");
+ ROS_ERROR_STREAM("Resource conflict: Could not find arm_id in resource "
+ << map_it->first << ". Name joints as '<robot_arm_id>_joint<jointnumber>'");
return false;
}
ResourceClaims new_claim;
@@ -136,4 +135,85 @@ ControlMode getControlMode(const std::string& arm_id, ArmClaimedMap& arm_claim_m
return control_mode;
}
+bool hasConflictingMultiClaim(const ResourceWithClaimsMap& resource_map) {
+ for (const auto& resource : resource_map) {
+ if (resource.second.size() > 2) {
+ ROS_ERROR_STREAM("Resource conflict: " << resource.first
+ << " is claimed with more than two command interfaces "
+ "which is not supported.");
+ return true;
+ }
+ uint8_t torque_claims = 0;
+ uint8_t other_claims = 0;
+ if (resource.second.size() == 2) {
+ for (const auto& claimed_by : resource.second) {
+ if (claimed_by.at(2) == "hardware_interface::EffortJointInterface") {
+ torque_claims++;
+ } else {
+ other_claims++;
+ }
+ }
+ if (torque_claims != 1) {
+ ROS_ERROR_STREAM("Resource conflict: "
+ << resource.first
+ << " is claimed with a combination of two interfaces that is not "
+ "supported.");
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+bool hasConflictingJointAndCartesianClaim(const ArmClaimedMap& arm_claim_map,
+ const std::string& arm_id) {
+ // check for conflicts between joint and cartesian level for each arm.
+ if (arm_claim_map.find(arm_id) != arm_claim_map.end()) {
+ if ((arm_claim_map.at(arm_id).cartesian_velocity_claims +
+ arm_claim_map.at(arm_id).cartesian_pose_claims >
+ 0 &&
+ arm_claim_map.at(arm_id).joint_position_claims +
+ arm_claim_map.at(arm_id).joint_velocity_claims >
+ 0)) {
+ ROS_ERROR_STREAM(
+ "Resource conflict: Invalid combination of claims on joint AND cartesian level on arm "
+ << arm_id << " which is not supported.");
+ return true;
+ }
+ }
+ return false;
+}
+
+bool partiallyClaimsArmJoints(const ArmClaimedMap& arm_claim_map, const std::string& arm_id) {
+ // 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.at(arm_id).joint_position_claims > 0 &&
+ arm_claim_map.at(arm_id).joint_position_claims != 7) ||
+ (arm_claim_map.at(arm_id).joint_velocity_claims > 0 &&
+ arm_claim_map.at(arm_id).joint_velocity_claims != 7) ||
+ (arm_claim_map.at(arm_id).joint_torque_claims > 0 &&
+ arm_claim_map.at(arm_id).joint_torque_claims != 7)) {
+ ROS_ERROR_STREAM("Resource conflict: Partially claiming joints of arm "
+ << arm_id
+ << " is not supported. Make sure to claim all 7 joints of the robot.");
+ return true;
+ }
+ }
+ 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
diff --git a/franka_visualization/CMakeLists.txt b/franka_visualization/CMakeLists.txt
index 65322ddd8362228a9b76cdb73ac829d521bcccaa..3354d32e3b1730f67463d546cb93410a53fe3954 100644
--- a/franka_visualization/CMakeLists.txt
+++ b/franka_visualization/CMakeLists.txt
@@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
)
-find_package(Franka 0.5.0 REQUIRED)
+find_package(Franka 0.7.0 REQUIRED)
catkin_package(CATKIN_DEPENDS sensor_msgs roscpp)
diff --git a/scripts/fail-on-output.sh b/scripts/fail-on-output.sh
deleted file mode 100755
index ea21216d60af5af4527f6b1d1c0d155bf8faaa40..0000000000000000000000000000000000000000
--- a/scripts/fail-on-output.sh
+++ /dev/null
@@ -1,6 +0,0 @@
-#!/usr/bin/env bash
-output="$($@)"
-if [[ "$output" ]]; then
- echo "$output"
- exit 1
-fi
diff --git a/scripts/format-check.sh b/scripts/format-check.sh
deleted file mode 100755
index 916855f9578dbe9e46a4f8ea264df36a0f5d0bcf..0000000000000000000000000000000000000000
--- a/scripts/format-check.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/usr/bin/env bash
-# grep the result of clang-format with -output-replacements-xml flag
-# if any "replacement" tags are found, then code is incorrectly formatted
-echo $($@) | grep '<replacement ' > /dev/null
-if [ $? -ne 1 ]; then
- echo "Files have to be formatted with clang-format"
- exit 1;
-fi