From ab47d8065a77370186ff41130b5d1c45254b047e Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Tue, 24 Mar 2020 10:00:59 +0100
Subject: [PATCH] reduce repo to just franka_description
---
CHANGELOG.md | 136 ----
.../CMakeLists.txt => CMakeLists.txt | 0
Jenkinsfile | 73 ---
README.md | 2 -
cmake/ClangTools.cmake | 65 --
franka_control/CMakeLists.txt | 136 ----
.../config/default_combined_controllers.yaml | 88 ---
.../config/default_controllers.yaml | 39 --
.../config/franka_combined_control_node.yaml | 67 --
.../config/franka_control_node.yaml | 29 -
franka_control/franka_controller_plugins.xml | 5 -
.../franka_control/franka_state_controller.h | 72 ---
.../launch/franka_combined_control.launch | 53 --
franka_control/launch/franka_control.launch | 32 -
franka_control/package.xml | 37 --
.../src/franka_combined_control_node.cpp | 46 --
franka_control/src/franka_control_node.cpp | 100 ---
.../src/franka_state_controller.cpp | 461 --------------
franka_description/mainpage.dox | 6 -
franka_description/rosdoc.yaml | 2 -
franka_example_controllers/CMakeLists.txt | 128 ----
.../cfg/compliance_param.cfg | 12 -
.../cfg/desired_mass_param.cfg | 12 -
.../cfg/dual_arm_compliance_param.cfg | 16 -
.../config/franka_example_controllers.yaml | 117 ----
.../franka_example_controllers_plugin.xml | 52 --
.../cartesian_impedance_example_controller.h | 69 --
.../cartesian_pose_example_controller.h | 34 -
.../cartesian_velocity_example_controller.h | 32 -
...m_cartesian_impedance_example_controller.h | 182 ------
.../elbow_example_controller.h | 35 --
.../force_example_controller.h | 61 --
.../joint_impedance_example_controller.h | 60 --
.../joint_position_example_controller.h | 31 -
.../joint_velocity_example_controller.h | 32 -
.../model_example_controller.h | 34 -
.../pseudo_inversion.h | 30 -
...tesian_impedance_example_controller.launch | 17 -
.../cartesian_pose_example_controller.launch | 13 -
...rtesian_velocity_example_controller.launch | 13 -
...tesian_impedance_example_controller.launch | 23 -
.../launch/elbow_example_controller.launch | 13 -
.../launch/force_example_controller.launch | 14 -
.../joint_impedance_example_controller.launch | 13 -
.../joint_position_example_controller.launch | 13 -
.../joint_velocity_example_controller.launch | 13 -
.../launch/model_example_controller.launch | 13 -
.../launch/move_to_start.launch | 13 -
franka_example_controllers/launch/robot.rviz | 187 ------
.../rviz/franka_description_with_marker.rviz | 181 ------
.../franka_dual_description_with_marker.rviz | 281 ---------
franka_example_controllers/mainpage.dox | 6 -
.../msg/JointTorqueComparison.msg | 4 -
franka_example_controllers/package.xml | 43 --
franka_example_controllers/rosdoc.yaml | 2 -
.../scripts/dual_arm_interactive_marker.py | 253 --------
.../scripts/interactive_marker.py | 141 -----
.../scripts/move_to_start.py | 12 -
...cartesian_impedance_example_controller.cpp | 243 --------
.../src/cartesian_pose_example_controller.cpp | 93 ---
.../cartesian_velocity_example_controller.cpp | 101 ---
...cartesian_impedance_example_controller.cpp | 412 ------------
.../src/elbow_example_controller.cpp | 86 ---
.../src/force_example_controller.cpp | 151 -----
.../joint_impedance_example_controller.cpp | 214 -------
.../src/joint_position_example_controller.cpp | 81 ---
.../src/joint_velocity_example_controller.cpp | 101 ---
.../src/model_example_controller.cpp | 95 ---
franka_gripper/CMakeLists.txt | 121 ----
franka_gripper/action/Grasp.action | 8 -
franka_gripper/action/Homing.action | 4 -
franka_gripper/action/Move.action | 6 -
franka_gripper/action/Stop.action | 4 -
.../config/franka_gripper_node.yaml | 5 -
.../include/franka_gripper/franka_gripper.h | 93 ---
franka_gripper/launch/franka_gripper.launch | 13 -
franka_gripper/mainpage.dox | 6 -
franka_gripper/msg/GraspEpsilon.msg | 2 -
franka_gripper/package.xml | 28 -
franka_gripper/rosdoc.yaml | 2 -
franka_gripper/src/franka_gripper.cpp | 94 ---
franka_gripper/src/franka_gripper_node.cpp | 192 ------
franka_hw/CMakeLists.txt | 119 ----
franka_hw/franka_combinable_hw_plugin.xml | 7 -
franka_hw/include/franka_hw/control_mode.h | 49 --
.../franka_cartesian_command_interface.h | 130 ----
.../include/franka_hw/franka_combinable_hw.h | 170 -----
.../include/franka_hw/franka_combined_hw.h | 64 --
franka_hw/include/franka_hw/franka_hw.h | 516 ---------------
.../franka_hw/franka_model_interface.h | 279 ---------
.../franka_hw/franka_state_interface.h | 55 --
.../include/franka_hw/resource_helpers.h | 45 --
franka_hw/include/franka_hw/services.h | 160 -----
franka_hw/include/franka_hw/trigger_rate.h | 19 -
franka_hw/mainpage.dox | 6 -
franka_hw/package.xml | 37 --
franka_hw/rosdoc.yaml | 2 -
franka_hw/src/control_mode.cpp | 38 --
franka_hw/src/franka_combinable_hw.cpp | 218 -------
franka_hw/src/franka_combined_hw.cpp | 111 ----
franka_hw/src/franka_hw.cpp | 590 ------------------
franka_hw/src/resource_helpers.cpp | 219 -------
franka_hw/src/services.cpp | 145 -----
franka_hw/src/trigger_rate.cpp | 17 -
franka_hw/test/CMakeLists.txt | 24 -
.../ros_console_settings_for_tests.conf | 4 -
...ombinable_hw_controller_switching_test.cpp | 198 ------
.../franka_hw_controller_switching_test.cpp | 196 ------
franka_hw/test/franka_hw_interfaces_test.cpp | 145 -----
franka_hw/test/launch/franka_hw_test.test | 8 -
franka_hw/test/main.cpp | 10 -
franka_msgs/CMakeLists.txt | 22 -
franka_msgs/action/ErrorRecovery.action | 2 -
franka_msgs/mainpage.dox | 6 -
franka_msgs/msg/Errors.msg | 36 --
franka_msgs/msg/FrankaState.msg | 52 --
franka_msgs/package.xml | 28 -
franka_msgs/rosdoc.yaml | 2 -
franka_msgs/srv/SetCartesianImpedance.srv | 5 -
franka_msgs/srv/SetEEFrame.srv | 5 -
.../srv/SetForceTorqueCollisionBehavior.srv | 8 -
franka_msgs/srv/SetFullCollisionBehavior.srv | 12 -
franka_msgs/srv/SetJointImpedance.srv | 5 -
franka_msgs/srv/SetKFrame.srv | 5 -
franka_msgs/srv/SetLoad.srv | 7 -
franka_ros/CMakeLists.txt | 4 -
franka_ros/mainpage.dox | 6 -
franka_ros/package.xml | 28 -
franka_ros/rosdoc.yaml | 2 -
franka_visualization/CMakeLists.txt | 64 --
.../config/gripper_settings.yaml | 3 -
.../config/robot_settings.yaml | 8 -
.../launch/franka_visualization.launch | 30 -
.../launch/franka_visualization.rviz | 187 ------
franka_visualization/mainpage.dox | 6 -
franka_visualization/package.xml | 22 -
franka_visualization/rosdoc.yaml | 2 -
.../src/gripper_joint_state_publisher.cpp | 57 --
.../src/robot_joint_state_publisher.cpp | 57 --
franka_control/mainpage.dox => mainpage.dox | 0
.../meshes => meshes}/collision/finger.stl | Bin
.../meshes => meshes}/collision/hand.stl | Bin
.../meshes => meshes}/collision/link0.stl | Bin
.../meshes => meshes}/collision/link1.stl | Bin
.../meshes => meshes}/collision/link2.stl | Bin
.../meshes => meshes}/collision/link3.stl | Bin
.../meshes => meshes}/collision/link4.stl | Bin
.../meshes => meshes}/collision/link5.stl | Bin
.../meshes => meshes}/collision/link6.stl | Bin
.../meshes => meshes}/collision/link7.stl | Bin
.../meshes => meshes}/visual/finger.dae | 0
.../meshes => meshes}/visual/hand.dae | 0
.../meshes => meshes}/visual/link0.dae | 0
.../meshes => meshes}/visual/link1.dae | 0
.../meshes => meshes}/visual/link2.dae | 0
.../meshes => meshes}/visual/link3.dae | 0
.../meshes => meshes}/visual/link4.dae | 0
.../meshes => meshes}/visual/link5.dae | 0
.../meshes => meshes}/visual/link6.dae | 0
.../meshes => meshes}/visual/link7.dae | 0
franka_description/package.xml => package.xml | 0
.../dual_panda_example.urdf.xacro | 0
.../robots => robots}/hand.urdf.xacro | 0
.../robots => robots}/hand.xacro | 0
.../robots => robots}/panda.gazebo.xacro | 0
.../panda.transmission.xacro | 0
.../robots => robots}/panda_arm.urdf.xacro | 0
.../robots => robots}/panda_arm.xacro | 0
.../panda_arm_hand.urdf.xacro | 0
franka_control/rosdoc.yaml => rosdoc.yaml | 0
170 files changed, 10061 deletions(-)
delete mode 100644 CHANGELOG.md
rename franka_description/CMakeLists.txt => CMakeLists.txt (100%)
delete mode 100644 Jenkinsfile
delete mode 100644 cmake/ClangTools.cmake
delete mode 100644 franka_control/CMakeLists.txt
delete mode 100644 franka_control/config/default_combined_controllers.yaml
delete mode 100644 franka_control/config/default_controllers.yaml
delete mode 100644 franka_control/config/franka_combined_control_node.yaml
delete mode 100644 franka_control/config/franka_control_node.yaml
delete mode 100644 franka_control/franka_controller_plugins.xml
delete mode 100644 franka_control/include/franka_control/franka_state_controller.h
delete mode 100644 franka_control/launch/franka_combined_control.launch
delete mode 100644 franka_control/launch/franka_control.launch
delete mode 100644 franka_control/package.xml
delete mode 100644 franka_control/src/franka_combined_control_node.cpp
delete mode 100644 franka_control/src/franka_control_node.cpp
delete mode 100644 franka_control/src/franka_state_controller.cpp
delete mode 100644 franka_description/mainpage.dox
delete mode 100644 franka_description/rosdoc.yaml
delete mode 100644 franka_example_controllers/CMakeLists.txt
delete mode 100755 franka_example_controllers/cfg/compliance_param.cfg
delete mode 100755 franka_example_controllers/cfg/desired_mass_param.cfg
delete mode 100755 franka_example_controllers/cfg/dual_arm_compliance_param.cfg
delete mode 100644 franka_example_controllers/config/franka_example_controllers.yaml
delete mode 100644 franka_example_controllers/franka_example_controllers_plugin.xml
delete mode 100644 franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/force_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/model_example_controller.h
delete mode 100644 franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
delete mode 100644 franka_example_controllers/launch/cartesian_impedance_example_controller.launch
delete mode 100644 franka_example_controllers/launch/cartesian_pose_example_controller.launch
delete mode 100644 franka_example_controllers/launch/cartesian_velocity_example_controller.launch
delete mode 100644 franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
delete mode 100644 franka_example_controllers/launch/elbow_example_controller.launch
delete mode 100644 franka_example_controllers/launch/force_example_controller.launch
delete mode 100644 franka_example_controllers/launch/joint_impedance_example_controller.launch
delete mode 100644 franka_example_controllers/launch/joint_position_example_controller.launch
delete mode 100644 franka_example_controllers/launch/joint_velocity_example_controller.launch
delete mode 100644 franka_example_controllers/launch/model_example_controller.launch
delete mode 100644 franka_example_controllers/launch/move_to_start.launch
delete mode 100644 franka_example_controllers/launch/robot.rviz
delete mode 100644 franka_example_controllers/launch/rviz/franka_description_with_marker.rviz
delete mode 100644 franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
delete mode 100644 franka_example_controllers/mainpage.dox
delete mode 100644 franka_example_controllers/msg/JointTorqueComparison.msg
delete mode 100644 franka_example_controllers/package.xml
delete mode 100644 franka_example_controllers/rosdoc.yaml
delete mode 100755 franka_example_controllers/scripts/dual_arm_interactive_marker.py
delete mode 100755 franka_example_controllers/scripts/interactive_marker.py
delete mode 100755 franka_example_controllers/scripts/move_to_start.py
delete mode 100644 franka_example_controllers/src/cartesian_impedance_example_controller.cpp
delete mode 100644 franka_example_controllers/src/cartesian_pose_example_controller.cpp
delete mode 100644 franka_example_controllers/src/cartesian_velocity_example_controller.cpp
delete mode 100644 franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
delete mode 100644 franka_example_controllers/src/elbow_example_controller.cpp
delete mode 100644 franka_example_controllers/src/force_example_controller.cpp
delete mode 100644 franka_example_controllers/src/joint_impedance_example_controller.cpp
delete mode 100644 franka_example_controllers/src/joint_position_example_controller.cpp
delete mode 100644 franka_example_controllers/src/joint_velocity_example_controller.cpp
delete mode 100644 franka_example_controllers/src/model_example_controller.cpp
delete mode 100644 franka_gripper/CMakeLists.txt
delete mode 100644 franka_gripper/action/Grasp.action
delete mode 100644 franka_gripper/action/Homing.action
delete mode 100644 franka_gripper/action/Move.action
delete mode 100644 franka_gripper/action/Stop.action
delete mode 100644 franka_gripper/config/franka_gripper_node.yaml
delete mode 100644 franka_gripper/include/franka_gripper/franka_gripper.h
delete mode 100644 franka_gripper/launch/franka_gripper.launch
delete mode 100644 franka_gripper/mainpage.dox
delete mode 100644 franka_gripper/msg/GraspEpsilon.msg
delete mode 100644 franka_gripper/package.xml
delete mode 100644 franka_gripper/rosdoc.yaml
delete mode 100644 franka_gripper/src/franka_gripper.cpp
delete mode 100644 franka_gripper/src/franka_gripper_node.cpp
delete mode 100644 franka_hw/CMakeLists.txt
delete mode 100644 franka_hw/franka_combinable_hw_plugin.xml
delete mode 100644 franka_hw/include/franka_hw/control_mode.h
delete mode 100644 franka_hw/include/franka_hw/franka_cartesian_command_interface.h
delete mode 100644 franka_hw/include/franka_hw/franka_combinable_hw.h
delete mode 100644 franka_hw/include/franka_hw/franka_combined_hw.h
delete mode 100644 franka_hw/include/franka_hw/franka_hw.h
delete mode 100644 franka_hw/include/franka_hw/franka_model_interface.h
delete mode 100644 franka_hw/include/franka_hw/franka_state_interface.h
delete mode 100644 franka_hw/include/franka_hw/resource_helpers.h
delete mode 100644 franka_hw/include/franka_hw/services.h
delete mode 100644 franka_hw/include/franka_hw/trigger_rate.h
delete mode 100644 franka_hw/mainpage.dox
delete mode 100644 franka_hw/package.xml
delete mode 100644 franka_hw/rosdoc.yaml
delete mode 100644 franka_hw/src/control_mode.cpp
delete mode 100644 franka_hw/src/franka_combinable_hw.cpp
delete mode 100644 franka_hw/src/franka_combined_hw.cpp
delete mode 100644 franka_hw/src/franka_hw.cpp
delete mode 100644 franka_hw/src/resource_helpers.cpp
delete mode 100644 franka_hw/src/services.cpp
delete mode 100644 franka_hw/src/trigger_rate.cpp
delete mode 100644 franka_hw/test/CMakeLists.txt
delete mode 100644 franka_hw/test/config/ros_console_settings_for_tests.conf
delete mode 100644 franka_hw/test/franka_combinable_hw_controller_switching_test.cpp
delete mode 100644 franka_hw/test/franka_hw_controller_switching_test.cpp
delete mode 100644 franka_hw/test/franka_hw_interfaces_test.cpp
delete mode 100644 franka_hw/test/launch/franka_hw_test.test
delete mode 100644 franka_hw/test/main.cpp
delete mode 100644 franka_msgs/CMakeLists.txt
delete mode 100644 franka_msgs/action/ErrorRecovery.action
delete mode 100644 franka_msgs/mainpage.dox
delete mode 100644 franka_msgs/msg/Errors.msg
delete mode 100644 franka_msgs/msg/FrankaState.msg
delete mode 100644 franka_msgs/package.xml
delete mode 100644 franka_msgs/rosdoc.yaml
delete mode 100644 franka_msgs/srv/SetCartesianImpedance.srv
delete mode 100644 franka_msgs/srv/SetEEFrame.srv
delete mode 100644 franka_msgs/srv/SetForceTorqueCollisionBehavior.srv
delete mode 100644 franka_msgs/srv/SetFullCollisionBehavior.srv
delete mode 100644 franka_msgs/srv/SetJointImpedance.srv
delete mode 100644 franka_msgs/srv/SetKFrame.srv
delete mode 100644 franka_msgs/srv/SetLoad.srv
delete mode 100644 franka_ros/CMakeLists.txt
delete mode 100644 franka_ros/mainpage.dox
delete mode 100644 franka_ros/package.xml
delete mode 100644 franka_ros/rosdoc.yaml
delete mode 100644 franka_visualization/CMakeLists.txt
delete mode 100644 franka_visualization/config/gripper_settings.yaml
delete mode 100644 franka_visualization/config/robot_settings.yaml
delete mode 100644 franka_visualization/launch/franka_visualization.launch
delete mode 100644 franka_visualization/launch/franka_visualization.rviz
delete mode 100644 franka_visualization/mainpage.dox
delete mode 100644 franka_visualization/package.xml
delete mode 100644 franka_visualization/rosdoc.yaml
delete mode 100644 franka_visualization/src/gripper_joint_state_publisher.cpp
delete mode 100644 franka_visualization/src/robot_joint_state_publisher.cpp
rename franka_control/mainpage.dox => mainpage.dox (100%)
rename {franka_description/meshes => meshes}/collision/finger.stl (100%)
rename {franka_description/meshes => meshes}/collision/hand.stl (100%)
rename {franka_description/meshes => meshes}/collision/link0.stl (100%)
rename {franka_description/meshes => meshes}/collision/link1.stl (100%)
rename {franka_description/meshes => meshes}/collision/link2.stl (100%)
rename {franka_description/meshes => meshes}/collision/link3.stl (100%)
rename {franka_description/meshes => meshes}/collision/link4.stl (100%)
rename {franka_description/meshes => meshes}/collision/link5.stl (100%)
rename {franka_description/meshes => meshes}/collision/link6.stl (100%)
rename {franka_description/meshes => meshes}/collision/link7.stl (100%)
rename {franka_description/meshes => meshes}/visual/finger.dae (100%)
rename {franka_description/meshes => meshes}/visual/hand.dae (100%)
rename {franka_description/meshes => meshes}/visual/link0.dae (100%)
rename {franka_description/meshes => meshes}/visual/link1.dae (100%)
rename {franka_description/meshes => meshes}/visual/link2.dae (100%)
rename {franka_description/meshes => meshes}/visual/link3.dae (100%)
rename {franka_description/meshes => meshes}/visual/link4.dae (100%)
rename {franka_description/meshes => meshes}/visual/link5.dae (100%)
rename {franka_description/meshes => meshes}/visual/link6.dae (100%)
rename {franka_description/meshes => meshes}/visual/link7.dae (100%)
rename franka_description/package.xml => package.xml (100%)
rename {franka_description/robots => robots}/dual_panda_example.urdf.xacro (100%)
rename {franka_description/robots => robots}/hand.urdf.xacro (100%)
rename {franka_description/robots => robots}/hand.xacro (100%)
rename {franka_description/robots => robots}/panda.gazebo.xacro (100%)
rename {franka_description/robots => robots}/panda.transmission.xacro (100%)
rename {franka_description/robots => robots}/panda_arm.urdf.xacro (100%)
rename {franka_description/robots => robots}/panda_arm.xacro (100%)
rename {franka_description/robots => robots}/panda_arm_hand.urdf.xacro (100%)
rename franka_control/rosdoc.yaml => rosdoc.yaml (100%)
diff --git a/CHANGELOG.md b/CHANGELOG.md
deleted file mode 100644
index e610347..0000000
--- a/CHANGELOG.md
+++ /dev/null
@@ -1,136 +0,0 @@
-# CHANGELOG
-
-## 0.7.0 - UNRELEASED
-
-Requires `libfranka` >= 0.7.0
-
- * **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.
- * **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`.
- * Add rosparam to choose value of `franka::RealtimeConfig`.
- * Fix unused parameter bugs in `FrankaModelHandle` (#78).
-
-## 0.6.0 - 2018-08-08
-
-Requires `libfranka` >= 0.5.0
-
- * **BREAKING** Fixes for MoveIt, improving robot performance:
- * Fixed joint velocity and acceleration limits in `joint_limits.yaml`
- * Use desired joint state for move group
- * **BREAKING** Updated joint limits in URDF
- * **BREAKING** Fixed velocity, acceleration and jerk limits in `franka_hw`
- * **BREAKING** Start `franka_gripper_node` when giving `load_gripper:=true` to `franka_control.launch`
- * Allow to configure rate limiting, filtering and internal controller in `franka_control_node`
- * **BREAKING** `FrankaHW::FrankaHW` takes additional parameters.
- * **BREAKING** Enabled rate limiting and low-pass filtering by default (`franka_control_node.yaml`)
- * Publish desired joint state in `/joint_state_desired`
- * Removed `effort_joint_trajectory_controller` from `default_controllers.yaml`
- * Fixed a bug when switching between controllers using the same `libfranka` interface
-
-## 0.5.0 - 2018-06-28
-
-Requires `libfranka` >= 0.4.0
-
- * **BREAKING** Updated URDF:
- * Adjusted maximum joint velocity
- * Updated axis 4 hard and soft limits
-
-## 0.4.1 - 2018-06-21
-
-Requires `libfranka` >= 0.3.0
-
- * Added some missing includes to `franka_hw`
- * Add support for commanding elbow in Cartesian pose and Cartesian velocity interfaces
-
-## 0.4.0 - 2018-03-26
-
-Requires `libfranka` >= 0.3.0
-
- * **BREAKING** Removed `arm_id` and default `robot_ip` from launchfiles
- * **BREAKING** Changed namespace of `franka_control` controller manager
- * **BREAKING** Changed behavior of `gripper_action` for compatibility with MoveIt
- * Changes in `panda_moveit_config`:
- * Updated joint limits from URDF
- * Removed `home` poses
- * Fixed fake execution
- * Add `load_gripper` argument (default: `true`) to `panda_moveit.launch`
- * Conditionally load controllers/SRDFs based on `load_gripper`
- * Add gripper controller configuration (requires running `franka_gripper_node`)
- * Added `mimic` tag for gripper fingers to URDF and fixed velocity limits
-
-## 0.3.0 - 2018-02-22
-
-Requires `libfranka` >= 0.3.0
-
- * **BREAKING** Changed signatures in `franka_hw::FrankaModelHandle`
- * **BREAKING** Added epsilon parameters to `franka_gripper/Grasp` action
- * Added Collada meshes for Panda and Hand
- * Added missing dependencies to `panda_moveit_config` and `franka_example_controllers`
- * Fixed linker errors when building with `-DFranka_DIR` while an older version of
- `ros-kinetic-libfranka` is installed
- * Added gripper joint state publisher to `franka_visualization`
- * Moved `move_to_start.py` example script to `franka_example_controllers`
-
-## 0.2.2 - 2018-01-31
-
-Requires `libfranka` >= 0.2.0
-
- * Catkin-related fixes for `franka_example_controllers`
- * Added missing `<build_export_depend>` for `message_runtime`
-
-## 0.2.1 - 2018-01-30
-
-Requires `libfranka` >= 0.2.0
-
- * Added missing dependency to `franka_example_controllers`
- * Lowered rotational gains for Cartesian impedance example controller
-
-## 0.2.0 - 2018-01-29
-
-Requires `libfranka` >= 0.2.0
-
- * Added missing run-time dependencies to `franka_description` and `franka_control`
- * Added `tau_J_d`, `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal`, `I_total`,
- `theta` and `dtheta` to `franka_msgs/FrankaState`
- * Added new errors to `franka_msgs/Errors`
- * Updated and improved examples in `franka_example_controllers`
- * Fixed includes for Eigen3 in `franka_example_controllers`
- * Fixed gripper state publishing in `franka_gripper_node`
-
-## 0.1.2 - 2017-10-10
-
- * Fixed out-of-workspace build
-
-## 0.1.1 - 2017-10-09
-
- * Integrated `franka_description` as subdirectory
- * Fixed dependencies on libfranka
- * Fixed RViz config file paths
- * Added missing `test_depend` to `franka_hw`
- * Added missing CMake install rules
-
-## 0.1.0 - 2017-09-15
-
- * Initial release
diff --git a/franka_description/CMakeLists.txt b/CMakeLists.txt
similarity index 100%
rename from franka_description/CMakeLists.txt
rename to CMakeLists.txt
diff --git a/Jenkinsfile b/Jenkinsfile
deleted file mode 100644
index e0cb318..0000000
--- a/Jenkinsfile
+++ /dev/null
@@ -1,73 +0,0 @@
-#!groovy
-
-buildResult = 'NOT_BUILT'
-
-def getStages(rosDistribution, ubuntuVersion) {
- return {
- node('docker') {
- step([$class: 'StashNotifier'])
-
- try {
- dir('src/franka_ros') {
- checkout scm
- }
-
- sh 'rm -rf dist'
- dir('dist') {
- try {
- step([$class: 'CopyArtifact',
- filter: "libfranka-*-amd64-${ubuntuVersion}.tar.gz",
- fingerprintArtifacts: true,
- projectName: "SWDEV/libfranka/${java.net.URLEncoder.encode(env.BRANCH_NAME, "UTF-8")}",
- selector: [$class: 'StatusBuildSelector', stable: false]])
- } catch (e) {
- // Fall back to develop branch.
- step([$class: 'CopyArtifact',
- filter: "libfranka-*-amd64-${ubuntuVersion}.tar.gz",
- fingerprintArtifacts: true,
- projectName: "SWDEV/libfranka/develop",
- selector: [$class: 'StatusBuildSelector', stable: false]])
- }
- sh """
- tar xfz libfranka-*-amd64-${ubuntuVersion}.tar.gz
- ln -sf libfranka-*-amd64 libfranka
- """
- }
-
- docker.build("franka_ros-ci-worker:${rosDistribution}",
- "-f src/franka_ros/.ci/Dockerfile.${rosDistribution} src/franka_ros/.ci").inside('-e MAKEFLAGS') {
- withEnv(["CMAKE_PREFIX_PATH+=${env.WORKSPACE}/dist/libfranka/lib/cmake/Franka",
- "ROS_HOME=${env.WORKSPACE}/ros-home"]) {
- stage("${rosDistribution}: Build & Lint (Debug)") {
- sh """
- . /opt/ros/${rosDistribution}/setup.sh
- src/franka_ros/.ci/debug.sh
- """
- junit 'build-debug/test_results/**/*.xml'
- }
- }
- }
-
- if (buildResult != 'FAILED') {
- buildResult = 'SUCCESS'
- }
- } catch (e) {
- buildResult = 'FAILED'
- }
- }
- }
-}
-
-node {
- step([$class: 'StashNotifier'])
-}
-
-parallel(
- 'kinetic': getStages('kinetic', 'xenial'),
- 'melodic': getStages('melodic', 'bionic'),
-)
-
-node {
- currentBuild.result = buildResult
- step([$class: 'StashNotifier'])
-}
diff --git a/README.md b/README.md
index 1df34a2..1ddf2be 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,5 @@
# ROS integration for Franka Emika research robots
-[![Build Status][travis-status]][travis]
-
See the [Franka Control Interface (FCI) documentation][fci-docs] for more information.
## License
diff --git a/cmake/ClangTools.cmake b/cmake/ClangTools.cmake
deleted file mode 100644
index fbc80d6..0000000
--- a/cmake/ClangTools.cmake
+++ /dev/null
@@ -1,65 +0,0 @@
-include(CMakeParseArguments)
-
-find_program(CLANG_FORMAT_PROG clang-format-6.0 DOC "'clang-format' executable")
-if(CLANG_FORMAT_PROG AND NOT TARGET format)
- add_custom_target(format)
- add_custom_target(check-format)
-endif()
-find_program(CLANG_TIDY_PROG clang-tidy-6.0 DOC "'clang-tidy' executable")
-if(CLANG_TIDY_PROG AND NOT TARGET tidy)
- if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)
- message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'
- to generate compilation database for 'clang-tidy'.")
- endif()
-
- add_custom_target(tidy)
- add_custom_target(check-tidy)
-endif()
-
-function(add_format_target _target)
- if(NOT CLANG_FORMAT_PROG)
- return()
- endif()
- cmake_parse_arguments(ARG "" "" "FILES" ${ARGN})
-
- add_custom_target(format-${_target}
- COMMAND ${CLANG_FORMAT_PROG} -i ${ARG_FILES}
- WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
- COMMENT "Formatting ${_target} source code with clang-format"
- VERBATIM
- )
- add_dependencies(format format-${_target})
-
- add_custom_target(check-format-${_target}
- 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
- )
- add_dependencies(check-format check-format-${_target})
-endfunction()
-
-function(add_tidy_target _target)
- if(NOT CLANG_TIDY_PROG)
- return()
- endif()
- cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN})
-
- add_custom_target(tidy-${_target}
- COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
- WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
- DEPENDS ${ARG_DEPENDS}
- COMMENT "Running clang-tidy for ${_target}"
- VERBATIM
- )
- add_dependencies(tidy tidy-${_target})
-
- add_custom_target(check-tidy-${_target}
- 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}"
- VERBATIM
- )
- add_dependencies(check-tidy check-tidy-${_target})
-endfunction()
diff --git a/franka_control/CMakeLists.txt b/franka_control/CMakeLists.txt
deleted file mode 100644
index 2abf7e5..0000000
--- a/franka_control/CMakeLists.txt
+++ /dev/null
@@ -1,136 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_control)
-
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-find_package(catkin REQUIRED COMPONENTS
- controller_interface
- controller_manager
- franka_hw
- franka_msgs
- geometry_msgs
- message_generation
- pluginlib
- realtime_tools
- roscpp
- sensor_msgs
- tf
- tf2_msgs
-)
-
-find_package(Franka 0.7.0 REQUIRED)
-
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES franka_state_controller
- CATKIN_DEPENDS
- controller_interface
- franka_hw
- franka_msgs
- geometry_msgs
- pluginlib
- realtime_tools
- roscpp
- sensor_msgs
- tf2_msgs
- DEPENDS Franka
-)
-
-## franka_state_controller
-add_library(franka_state_controller
- src/franka_state_controller.cpp
-)
-
-add_dependencies(franka_state_controller
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_state_controller
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_state_controller SYSTEM PUBLIC
- ${Franka_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
-)
-target_include_directories(franka_state_controller PUBLIC
- include
-)
-
-## franka_control_node
-add_executable(franka_control_node
- src/franka_control_node.cpp
-)
-
-add_dependencies(franka_control_node
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_control_node
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_control_node SYSTEM PUBLIC
- ${Franka_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
-)
-
-add_executable(franka_combined_control_node
- src/franka_combined_control_node.cpp
-)
-
-add_dependencies(franka_combined_control_node
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_combined_control_node
- ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_combined_control_node SYSTEM PUBLIC
- ${catkin_INCLUDE_DIRS}
-)
-
-
-## Installation
-install(TARGETS franka_state_controller
- 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}
-)
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-install(DIRECTORY launch
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY config
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(FILES franka_controller_plugins.xml
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
- RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_TOOLS)
- file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
- file(GLOB_RECURSE HEADERS
- ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
- ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
- )
- add_format_target(franka_control FILES ${SOURCES} ${HEADERS})
- add_tidy_target(franka_control
- FILES ${SOURCES}
- 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
deleted file mode 100644
index e7ef492..0000000
--- a/franka_control/config/default_combined_controllers.yaml
+++ /dev/null
@@ -1,88 +0,0 @@
-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/default_controllers.yaml b/franka_control/config/default_controllers.yaml
deleted file mode 100644
index 28b4805..0000000
--- a/franka_control/config/default_controllers.yaml
+++ /dev/null
@@ -1,39 +0,0 @@
-position_joint_trajectory_controller:
- type: position_controllers/JointTrajectoryController
- joints:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
- constraints:
- goal_time: 0.5
- panda_joint1:
- goal: 0.05
- panda_joint2:
- goal: 0.05
- panda_joint3:
- goal: 0.05
- panda_joint4:
- goal: 0.05
- panda_joint5:
- goal: 0.05
- panda_joint6:
- goal: 0.05
- panda_joint7:
- goal: 0.05
-
-franka_state_controller:
- type: franka_control/FrankaStateController
- publish_rate: 30 # [Hz]
- joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
- arm_id: panda
diff --git a/franka_control/config/franka_combined_control_node.yaml b/franka_control/config/franka_combined_control_node.yaml
deleted file mode 100644
index fa103fa..0000000
--- a/franka_control/config/franka_combined_control_node.yaml
+++ /dev/null
@@ -1,67 +0,0 @@
-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
deleted file mode 100644
index b27d570..0000000
--- a/franka_control/config/franka_control_node.yaml
+++ /dev/null
@@ -1,29 +0,0 @@
-joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
-arm_id: panda
-# Configure the threshold angle for printing joint limit warnings.
-joint_limit_warning_threshold: 0.1 # [rad]
-# Activate rate limiter? [true|false]
-rate_limiting: true
-# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
-cutoff_frequency: 100
-# Internal controller for motion generators [joint_impedance|cartesian_impedance]
-internal_controller: joint_impedance
-# 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/franka_controller_plugins.xml b/franka_control/franka_controller_plugins.xml
deleted file mode 100644
index d7ffbfc..0000000
--- a/franka_control/franka_controller_plugins.xml
+++ /dev/null
@@ -1,5 +0,0 @@
-<library path="lib/libfranka_state_controller">
- <class name="franka_control/FrankaStateController" type="franka_control::FrankaStateController" base_class_type="controller_interface::ControllerBase">
- <description>A controller that publishes the complete robot state</description>
- </class>
-</library>
diff --git a/franka_control/include/franka_control/franka_state_controller.h b/franka_control/include/franka_control/franka_state_controller.h
deleted file mode 100644
index 6fe4294..0000000
--- a/franka_control/include/franka_control/franka_state_controller.h
+++ /dev/null
@@ -1,72 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#pragma once
-
-#include <cstdint>
-#include <memory>
-#include <vector>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <franka_hw/franka_state_interface.h>
-#include <franka_hw/trigger_rate.h>
-#include <franka_msgs/FrankaState.h>
-#include <geometry_msgs/WrenchStamped.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <sensor_msgs/JointState.h>
-#include <tf2_msgs/TFMessage.h>
-
-namespace franka_control {
-
-/**
- * Controller to publish the robot state as ROS topics.
- */
-class FrankaStateController
- : public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface> {
- public:
- /**
- * Creates an instance of a FrankaStateController.
- */
- FrankaStateController() = default;
-
- /**
- * Initializes the controller with interfaces and publishers.
- *
- * @param[in] robot_hardware RobotHW instance to get a franka_hw::FrankaStateInterface from.
- * @param[in] root_node_handle Node handle in the controller_manager namespace.
- * @param[in] controller_node_handle Node handle in the controller namespace.
- */
- bool init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& controller_node_handle) override;
-
- /**
- * Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.
- *
- * @param[in] time Current ROS time.
- * @param[in] period Time since the last update.
- */
- void update(const ros::Time& time, const ros::Duration& period) override;
-
- private:
- void publishFrankaStates(const ros::Time& time);
- void publishJointStates(const ros::Time& time);
- void publishTransforms(const ros::Time& time);
- void publishExternalWrench(const ros::Time& time);
-
- std::string arm_id_;
-
- franka_hw::FrankaStateInterface* franka_state_interface_{};
- std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_{};
-
- realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
- realtime_tools::RealtimePublisher<franka_msgs::FrankaState> publisher_franka_states_;
- realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_;
- realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_desired_;
- realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> publisher_external_wrench_;
- franka_hw::TriggerRate trigger_publish_;
- franka::RobotState robot_state_;
- uint64_t sequence_number_ = 0;
- std::vector<std::string> joint_names_;
-};
-
-} // namespace franka_control
diff --git a/franka_control/launch/franka_combined_control.launch b/franka_control/launch/franka_combined_control.launch
deleted file mode 100644
index bfdf673..0000000
--- a/franka_control/launch/franka_combined_control.launch
+++ /dev/null
@@ -1,53 +0,0 @@
-<?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/launch/franka_control.launch b/franka_control/launch/franka_control.launch
deleted file mode 100644
index 37adda8..0000000
--- a/franka_control/launch/franka_control.launch
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
-
- <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
- <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
-
- <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
- <arg name="robot_ip" value="$(arg robot_ip)" />
- </include>
-
- <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
- <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
- <param name="robot_ip" value="$(arg robot_ip)" />
- </node>
-
- <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
- <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
- <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">
- <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
- <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
- <param name="rate" value="30"/>
- </node>
- <node name="joint_state_desired_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
- <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam>
- <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states_desired] </rosparam>
- <param name="rate" value="30"/>
- <remap from="/joint_states" to="/joint_states_desired" />
- </node>
-</launch>
diff --git a/franka_control/package.xml b/franka_control/package.xml
deleted file mode 100644
index af060e9..0000000
--- a/franka_control/package.xml
+++ /dev/null
@@ -1,37 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_control</name>
- <version>0.7.0</version>
- <description>franka_control provides a hardware node to control a Franka Emika research robot</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_control</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <depend>libfranka</depend>
- <depend>controller_interface</depend>
- <depend>controller_manager</depend>
- <depend>franka_hw</depend>
- <depend>franka_msgs</depend>
- <depend>geometry_msgs</depend>
- <depend>pluginlib</depend>
- <depend>realtime_tools</depend>
- <depend>roscpp</depend>
- <depend>sensor_msgs</depend>
- <depend>tf2_msgs</depend>
- <depend>tf</depend>
-
- <exec_depend>franka_description</exec_depend>
- <exec_depend>franka_gripper</exec_depend>
- <exec_depend>joint_state_publisher</exec_depend>
- <exec_depend>robot_state_publisher</exec_depend>
-
- <export>
- <controller_interface plugin="${prefix}/franka_controller_plugins.xml"/>
- </export>
-</package>
diff --git a/franka_control/src/franka_combined_control_node.cpp b/franka_control/src/franka_combined_control_node.cpp
deleted file mode 100644
index dfcb9ba..0000000
--- a/franka_control/src/franka_combined_control_node.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-// 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
deleted file mode 100644
index 9562a1a..0000000
--- a/franka_control/src/franka_control_node.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <algorithm>
-#include <atomic>
-
-#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>
-
-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("~");
-
- 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;
- }
-
- franka::Robot& robot = franka_control.robot();
-
- std::atomic_bool has_error(false);
-
- ServiceContainer services;
- franka_hw::setupServices(robot, node_handle, services);
-
- actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction> recovery_action_server(
- node_handle, "error_recovery",
- [&](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_msgs::ErrorRecoveryResult(), ex.what());
- }
- },
- false);
-
- // Initialize robot state before loading any controller
- franka_control.update(robot.readOnce());
-
- controller_manager::ControllerManager control_manager(&franka_control, public_node_handle);
-
- recovery_action_server.start();
-
- // Start background threads for message handling
- ros::AsyncSpinner spinner(4);
- spinner.start();
-
- while (ros::ok()) {
- ros::Time last_time = ros::Time::now();
-
- // Wait until controller has been activated or error has been recovered
- while (!franka_control.controllerActive() || has_error) {
- franka_control.update(robot.readOnce());
-
- ros::Time now = ros::Time::now();
- control_manager.update(now, now - last_time);
- franka_control.checkJointLimits();
- last_time = now;
-
- if (!ros::ok()) {
- return 0;
- }
- }
-
- try {
- // Run control loop. Will exit if the controller is switched.
- 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();
- });
- } catch (const franka::ControlException& e) {
- ROS_ERROR("%s", e.what());
- has_error = true;
- }
- }
-
- return 0;
-}
diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp
deleted file mode 100644
index 5d90e08..0000000
--- a/franka_control/src/franka_state_controller.cpp
+++ /dev/null
@@ -1,461 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_control/franka_state_controller.h>
-
-#include <cmath>
-#include <memory>
-#include <mutex>
-#include <string>
-
-#include <franka/errors.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_msgs/Errors.h>
-#include <hardware_interface/hardware_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-#include <tf/tf.h>
-#include <tf/transform_datatypes.h>
-
-namespace {
-
-tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
- tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],
- transform[9], transform[2], transform[6], transform[10]);
- tf::Vector3 translation(transform[12], transform[13], transform[14]);
- return tf::Transform(rotation, translation);
-}
-
-franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
- franka_msgs::Errors message;
- message.joint_position_limits_violation =
- static_cast<decltype(message.joint_position_limits_violation)>(
- error.joint_position_limits_violation);
- message.cartesian_position_limits_violation =
- static_cast<decltype(message.cartesian_position_limits_violation)>(
- error.cartesian_position_limits_violation);
- message.self_collision_avoidance_violation =
- static_cast<decltype(message.self_collision_avoidance_violation)>(
- error.self_collision_avoidance_violation);
- message.joint_velocity_violation =
- static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
- message.cartesian_velocity_violation =
- static_cast<decltype(message.cartesian_velocity_violation)>(
- error.cartesian_velocity_violation);
- message.force_control_safety_violation =
- static_cast<decltype(message.force_control_safety_violation)>(
- error.force_control_safety_violation);
- message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
- message.cartesian_reflex =
- static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
- message.max_goal_pose_deviation_violation =
- static_cast<decltype(message.max_goal_pose_deviation_violation)>(
- error.max_goal_pose_deviation_violation);
- message.max_path_pose_deviation_violation =
- static_cast<decltype(message.max_path_pose_deviation_violation)>(
- error.max_path_pose_deviation_violation);
- message.cartesian_velocity_profile_safety_violation =
- static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
- error.cartesian_velocity_profile_safety_violation);
- message.joint_position_motion_generator_start_pose_invalid =
- static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
- error.joint_position_motion_generator_start_pose_invalid);
- message.joint_motion_generator_position_limits_violation =
- static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
- error.joint_motion_generator_position_limits_violation);
- message.joint_motion_generator_velocity_limits_violation =
- static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
- error.joint_motion_generator_velocity_limits_violation);
- message.joint_motion_generator_velocity_discontinuity =
- static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
- error.joint_motion_generator_velocity_discontinuity);
- message.joint_motion_generator_acceleration_discontinuity =
- static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
- error.joint_motion_generator_acceleration_discontinuity);
- message.cartesian_position_motion_generator_start_pose_invalid =
- static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
- error.cartesian_position_motion_generator_start_pose_invalid);
- message.cartesian_motion_generator_elbow_limit_violation =
- static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
- error.cartesian_motion_generator_elbow_limit_violation);
- message.cartesian_motion_generator_velocity_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
- error.cartesian_motion_generator_velocity_limits_violation);
- message.cartesian_motion_generator_velocity_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
- error.cartesian_motion_generator_velocity_discontinuity);
- message.cartesian_motion_generator_acceleration_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
- error.cartesian_motion_generator_acceleration_discontinuity);
- message.cartesian_motion_generator_elbow_sign_inconsistent =
- static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
- error.cartesian_motion_generator_elbow_sign_inconsistent);
- message.cartesian_motion_generator_start_elbow_invalid =
- static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
- error.cartesian_motion_generator_start_elbow_invalid);
- message.cartesian_motion_generator_joint_position_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
- error.cartesian_motion_generator_joint_position_limits_violation);
- message.cartesian_motion_generator_joint_velocity_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
- error.cartesian_motion_generator_joint_velocity_limits_violation);
- message.cartesian_motion_generator_joint_velocity_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
- error.cartesian_motion_generator_joint_velocity_discontinuity);
- message.cartesian_motion_generator_joint_acceleration_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
- error.cartesian_motion_generator_joint_acceleration_discontinuity);
- message.cartesian_position_motion_generator_invalid_frame =
- static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
- error.cartesian_position_motion_generator_invalid_frame);
- message.force_controller_desired_force_tolerance_violation =
- static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
- error.force_controller_desired_force_tolerance_violation);
- message.controller_torque_discontinuity =
- static_cast<decltype(message.controller_torque_discontinuity)>(
- error.controller_torque_discontinuity);
- message.start_elbow_sign_inconsistent =
- static_cast<decltype(message.start_elbow_sign_inconsistent)>(
- error.start_elbow_sign_inconsistent);
- message.communication_constraints_violation =
- static_cast<decltype(message.communication_constraints_violation)>(
- error.communication_constraints_violation);
- message.power_limit_violation =
- static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
- message.joint_p2p_insufficient_torque_for_planning =
- static_cast<decltype(message.joint_p2p_insufficient_torque_for_planning)>(
- error.joint_p2p_insufficient_torque_for_planning);
- message.tau_j_range_violation =
- static_cast<decltype(message.tau_j_range_violation)>(error.tau_j_range_violation);
- message.instability_detected =
- static_cast<decltype(message.instability_detected)>(error.instability_detected);
- return message;
-}
-
-} // anonymous namespace
-
-namespace franka_control {
-
-bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& controller_node_handle) {
- franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
- if (franka_state_interface_ == nullptr) {
- ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
- return false;
- }
- if (!controller_node_handle.getParam("arm_id", arm_id_)) {
- ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
- return false;
- }
- double publish_rate(30.0);
- if (!controller_node_handle.getParam("publish_rate", publish_rate)) {
- ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
- << publish_rate << " [Hz].");
- }
- trigger_publish_ = franka_hw::TriggerRate(publish_rate);
-
- if (!controller_node_handle.getParam("joint_names", joint_names_) ||
- joint_names_.size() != robot_state_.q.size()) {
- ROS_ERROR(
- "FrankaStateController: Invalid or no joint_names parameters provided, aborting "
- "controller init!");
- return false;
- }
-
- try {
- franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
- franka_state_interface_->getHandle(arm_id_ + "_robot"));
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM("FrankaStateController: Exception getting franka state handle: " << ex.what());
- return false;
- }
-
- publisher_transforms_.init(root_node_handle, "/tf", 1);
- publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
- publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
- publisher_joint_states_desired_.init(controller_node_handle, "joint_states_desired", 1);
- publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
-
- {
- std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
- publisher_joint_states_);
- publisher_joint_states_.msg_.name.resize(joint_names_.size());
- publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
- publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
- publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
- }
- {
- std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
- publisher_joint_states_desired_);
- publisher_joint_states_desired_.msg_.name.resize(joint_names_.size());
- publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size());
- publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size());
- publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size());
- }
- {
- std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> lock(
- publisher_transforms_);
- publisher_transforms_.msg_.transforms.resize(2);
- tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
- tf::Vector3 translation(0.0, 0.0, 0.05);
- tf::Transform transform(quaternion, translation);
- tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
- arm_id_ + "_EE");
- geometry_msgs::TransformStamped transform_message;
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[0] = transform_message;
- translation = tf::Vector3(0.0, 0.0, 0.0);
- transform = tf::Transform(quaternion, translation);
- stamped_transform =
- tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[1] = transform_message;
- }
- {
- std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>> lock(
- publisher_external_wrench_);
- publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
- publisher_external_wrench_.msg_.wrench.force.x = 0.0;
- publisher_external_wrench_.msg_.wrench.force.y = 0.0;
- publisher_external_wrench_.msg_.wrench.force.z = 0.0;
- publisher_external_wrench_.msg_.wrench.torque.x = 0.0;
- publisher_external_wrench_.msg_.wrench.torque.y = 0.0;
- publisher_external_wrench_.msg_.wrench.torque.z = 0.0;
- }
- return true;
-}
-
-void FrankaStateController::update(const ros::Time& time, const ros::Duration& /* period */) {
- if (trigger_publish_()) {
- robot_state_ = franka_state_handle_->getRobotState();
- publishFrankaStates(time);
- publishTransforms(time);
- publishExternalWrench(time);
- publishJointStates(time);
- sequence_number_++;
- }
-}
-
-void FrankaStateController::publishFrankaStates(const ros::Time& time) {
- if (publisher_franka_states_.trylock()) {
- static_assert(
- sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.K_F_ext_hat_K),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_F_ext_hat_K),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_d),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_c),
- "Robot state Cartesian members do not have same size");
- static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_ddP_EE_c),
- "Robot state Cartesian members do not have same size");
- for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
- publisher_franka_states_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
- publisher_franka_states_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
- publisher_franka_states_.msg_.K_F_ext_hat_K[i] = robot_state_.K_F_ext_hat_K[i];
- publisher_franka_states_.msg_.O_F_ext_hat_K[i] = robot_state_.O_F_ext_hat_K[i];
- publisher_franka_states_.msg_.O_dP_EE_d[i] = robot_state_.O_dP_EE_d[i];
- publisher_franka_states_.msg_.O_dP_EE_c[i] = robot_state_.O_dP_EE_c[i];
- publisher_franka_states_.msg_.O_ddP_EE_c[i] = robot_state_.O_ddP_EE_c[i];
- }
-
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.ddq_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
- "Robot state joint members do not have same size");
- for (size_t i = 0; i < robot_state_.q.size(); i++) {
- publisher_franka_states_.msg_.q[i] = robot_state_.q[i];
- publisher_franka_states_.msg_.q_d[i] = robot_state_.q_d[i];
- publisher_franka_states_.msg_.dq[i] = robot_state_.dq[i];
- publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
- publisher_franka_states_.msg_.ddq_d[i] = robot_state_.ddq_d[i];
- publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
- publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
- publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
- publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i];
- publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i];
- publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
- publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
- publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
- }
-
- static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.elbow_d),
- "Robot state elbow configuration members do not have same size");
- static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.elbow_c),
- "Robot state elbow configuration members do not have same size");
- static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.delbow_c),
- "Robot state elbow configuration members do not have same size");
- static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.ddelbow_c),
- "Robot state elbow configuration members do not have same size");
-
- for (size_t i = 0; i < robot_state_.elbow.size(); i++) {
- publisher_franka_states_.msg_.elbow[i] = robot_state_.elbow[i];
- publisher_franka_states_.msg_.elbow_d[i] = robot_state_.elbow_d[i];
- publisher_franka_states_.msg_.elbow_c[i] = robot_state_.elbow_c[i];
- publisher_franka_states_.msg_.delbow_c[i] = robot_state_.delbow_c[i];
- publisher_franka_states_.msg_.ddelbow_c[i] = robot_state_.ddelbow_c[i];
- }
-
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
- "Robot state transforms do not have same size");
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
- "Robot state transforms do not have same size");
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
- "Robot state transforms do not have same size");
- static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_c),
- "Robot state transforms do not have same size");
- for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {
- publisher_franka_states_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];
- publisher_franka_states_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];
- publisher_franka_states_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];
- publisher_franka_states_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];
- publisher_franka_states_.msg_.O_T_EE_c[i] = robot_state_.O_T_EE_c[i];
- }
- publisher_franka_states_.msg_.m_ee = robot_state_.m_ee;
- publisher_franka_states_.msg_.m_load = robot_state_.m_load;
- publisher_franka_states_.msg_.m_total = robot_state_.m_total;
-
- for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
- publisher_franka_states_.msg_.I_ee[i] = robot_state_.I_ee[i];
- publisher_franka_states_.msg_.I_load[i] = robot_state_.I_load[i];
- publisher_franka_states_.msg_.I_total[i] = robot_state_.I_total[i];
- }
-
- for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
- publisher_franka_states_.msg_.F_x_Cee[i] = robot_state_.F_x_Cee[i];
- publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
- publisher_franka_states_.msg_.F_x_Ctotal[i] = robot_state_.F_x_Ctotal[i];
- }
-
- publisher_franka_states_.msg_.time = robot_state_.time.toSec();
- publisher_franka_states_.msg_.control_command_success_rate =
- robot_state_.control_command_success_rate;
- publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
- publisher_franka_states_.msg_.last_motion_errors =
- errorsToMessage(robot_state_.last_motion_errors);
-
- switch (robot_state_.robot_mode) {
- case franka::RobotMode::kOther:
- publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_OTHER;
- break;
-
- case franka::RobotMode::kIdle:
- publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_IDLE;
- break;
-
- case franka::RobotMode::kMove:
- publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_MOVE;
- break;
-
- case franka::RobotMode::kGuiding:
- publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_GUIDING;
- break;
-
- case franka::RobotMode::kReflex:
- publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_REFLEX;
- break;
-
- case franka::RobotMode::kUserStopped:
- publisher_franka_states_.msg_.robot_mode =
- franka_msgs::FrankaState::ROBOT_MODE_USER_STOPPED;
- break;
-
- case franka::RobotMode::kAutomaticErrorRecovery:
- publisher_franka_states_.msg_.robot_mode =
- franka_msgs::FrankaState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY;
- break;
- }
-
- publisher_franka_states_.msg_.header.seq = sequence_number_;
- publisher_franka_states_.msg_.header.stamp = time;
- publisher_franka_states_.unlockAndPublish();
- }
-}
-
-void FrankaStateController::publishJointStates(const ros::Time& time) {
- if (publisher_joint_states_.trylock()) {
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
- "Robot state joint members do not have same size");
- for (size_t i = 0; i < robot_state_.q.size(); i++) {
- publisher_joint_states_.msg_.name[i] = joint_names_[i];
- publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
- publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
- publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];
- }
- publisher_joint_states_.msg_.header.stamp = time;
- publisher_joint_states_.msg_.header.seq = sequence_number_;
- publisher_joint_states_.unlockAndPublish();
- }
- if (publisher_joint_states_desired_.trylock()) {
- static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.dq_d),
- "Robot state joint members do not have same size");
- static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.tau_J_d),
- "Robot state joint members do not have same size");
- for (size_t i = 0; i < robot_state_.q_d.size(); i++) {
- publisher_joint_states_desired_.msg_.name[i] = joint_names_[i];
- publisher_joint_states_desired_.msg_.position[i] = robot_state_.q_d[i];
- publisher_joint_states_desired_.msg_.velocity[i] = robot_state_.dq_d[i];
- publisher_joint_states_desired_.msg_.effort[i] = robot_state_.tau_J_d[i];
- }
- publisher_joint_states_desired_.msg_.header.stamp = time;
- publisher_joint_states_desired_.msg_.header.seq = sequence_number_;
- publisher_joint_states_desired_.unlockAndPublish();
- }
-}
-
-void FrankaStateController::publishTransforms(const ros::Time& time) {
- if (publisher_transforms_.trylock()) {
- tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_EE), time,
- arm_id_ + "_link8", arm_id_ + "_EE");
- geometry_msgs::TransformStamped transform_message;
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[0] = transform_message;
- stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,
- arm_id_ + "_EE", arm_id_ + "_K");
- transformStampedTFToMsg(stamped_transform, transform_message);
- publisher_transforms_.msg_.transforms[1] = transform_message;
- publisher_transforms_.unlockAndPublish();
- }
-}
-
-void FrankaStateController::publishExternalWrench(const ros::Time& time) {
- if (publisher_external_wrench_.trylock()) {
- publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
- publisher_external_wrench_.msg_.header.stamp = time;
- publisher_external_wrench_.msg_.wrench.force.x = robot_state_.K_F_ext_hat_K[0];
- publisher_external_wrench_.msg_.wrench.force.y = robot_state_.K_F_ext_hat_K[1];
- publisher_external_wrench_.msg_.wrench.force.z = robot_state_.K_F_ext_hat_K[2];
- publisher_external_wrench_.msg_.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];
- publisher_external_wrench_.msg_.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];
- publisher_external_wrench_.msg_.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];
- publisher_external_wrench_.unlockAndPublish();
- }
-}
-
-} // namespace franka_control
-
-PLUGINLIB_EXPORT_CLASS(franka_control::FrankaStateController, controller_interface::ControllerBase)
diff --git a/franka_description/mainpage.dox b/franka_description/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_description/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_description/rosdoc.yaml b/franka_description/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_description/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
deleted file mode 100644
index 3f935f9..0000000
--- a/franka_example_controllers/CMakeLists.txt
+++ /dev/null
@@ -1,128 +0,0 @@
-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
- roscpp
- rospy
-)
-
-find_package(Eigen3 REQUIRED)
-find_package(Franka 0.7.0 REQUIRED)
-
-add_message_files(FILES
- JointTorqueComparison.msg
-)
-
-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
- roscpp
- DEPENDS Franka
-)
-
-add_library(franka_example_controllers
- src/elbow_example_controller.cpp
- src/cartesian_pose_example_controller.cpp
- src/cartesian_velocity_example_controller.cpp
- src/joint_position_example_controller.cpp
- src/joint_velocity_example_controller.cpp
- src/model_example_controller.cpp
- 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
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- ${PROJECT_NAME}_generate_messages_cpp
- ${PROJECT_NAME}_gencpp
- ${PROJECT_NAME}_gencfg
-)
-
-target_link_libraries(franka_example_controllers PUBLIC
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_example_controllers SYSTEM PUBLIC
- ${Franka_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
-)
-target_include_directories(franka_example_controllers PUBLIC
- include
-)
-
-## Installation
-install(TARGETS franka_example_controllers
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY launch
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY config
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(FILES franka_example_controllers_plugin.xml
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-catkin_install_python(
- PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py scripts/dual_arm_interactive_marker.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
- RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_TOOLS)
- file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
- file(GLOB_RECURSE HEADERS
- ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
- ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
- )
- add_format_target(franka_example_controllers FILES ${SOURCES} ${HEADERS})
- add_tidy_target(franka_example_controllers
- FILES ${SOURCES}
- DEPENDS franka_example_controllers
- )
-endif()
diff --git a/franka_example_controllers/cfg/compliance_param.cfg b/franka_example_controllers/cfg/compliance_param.cfg
deleted file mode 100755
index 3a891b0..0000000
--- a/franka_example_controllers/cfg/compliance_param.cfg
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/usr/bin/env python
-PACKAGE = "franka_example_controllers"
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
-gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
-gen.add("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", "compliance_param"))
diff --git a/franka_example_controllers/cfg/desired_mass_param.cfg b/franka_example_controllers/cfg/desired_mass_param.cfg
deleted file mode 100755
index 2fb2f0a..0000000
--- a/franka_example_controllers/cfg/desired_mass_param.cfg
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/usr/bin/env python
-PACKAGE = "franka_example_controllers"
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0, 0.0, 2.0)
-gen.add("k_p", double_t, 0, "force P gain", 0.0, 0.0, 2.0)
-gen.add("k_i", double_t, 0, "force I gain", 0.0, 0.0, 2.0)
-
-exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param"))
diff --git a/franka_example_controllers/cfg/dual_arm_compliance_param.cfg b/franka_example_controllers/cfg/dual_arm_compliance_param.cfg
deleted file mode 100755
index ef80c41..0000000
--- a/franka_example_controllers/cfg/dual_arm_compliance_param.cfg
+++ /dev/null
@@ -1,16 +0,0 @@
-#!/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
deleted file mode 100644
index b82b7ef..0000000
--- a/franka_example_controllers/config/franka_example_controllers.yaml
+++ /dev/null
@@ -1,117 +0,0 @@
-joint_velocity_example_controller:
- type: franka_example_controllers/JointVelocityExampleController
- joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
-
-joint_position_example_controller:
- type: franka_example_controllers/JointPositionExampleController
- joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
-
-cartesian_pose_example_controller:
- type: franka_example_controllers/CartesianPoseExampleController
- arm_id: panda
-
-elbow_example_controller:
- type: franka_example_controllers/ElbowExampleController
- arm_id: panda
-
-cartesian_velocity_example_controller:
- type: franka_example_controllers/CartesianVelocityExampleController
- arm_id: panda
-
-model_example_controller:
- type: franka_example_controllers/ModelExampleController
- arm_id: panda
-
-force_example_controller:
- type: franka_example_controllers/ForceExampleController
- arm_id: panda
- joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
-
-joint_impedance_example_controller:
- type: franka_example_controllers/JointImpedanceExampleController
- arm_id: panda
- joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
- k_gains:
- - 600.0
- - 600.0
- - 600.0
- - 600.0
- - 250.0
- - 150.0
- - 50.0
- d_gains:
- - 50.0
- - 50.0
- - 50.0
- - 20.0
- - 20.0
- - 20.0
- - 10.0
- radius: 0.1
- acceleration_time: 2.0
- vel_max: 0.15
- publish_rate: 10.0
- coriolis_factor: 1.0
-
-cartesian_impedance_example_controller:
- type: franka_example_controllers/CartesianImpedanceExampleController
- arm_id: panda
- joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - 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
deleted file mode 100644
index e770f04..0000000
--- a/franka_example_controllers/franka_example_controllers_plugin.xml
+++ /dev/null
@@ -1,52 +0,0 @@
-<library path="lib/libfranka_example_controllers">
- <class name="franka_example_controllers/JointVelocityExampleController" type="franka_example_controllers::JointVelocityExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that executes a short motion based on joint velocities to demonstrate correct usage
- </description>
- </class>
- <class name="franka_example_controllers/JointPositionExampleController" type="franka_example_controllers::JointPositionExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that executes a short motion based on joint positions to demonstrate correct usage
- </description>
- </class>
- <class name="franka_example_controllers/CartesianPoseExampleController" type="franka_example_controllers::CartesianPoseExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that executes a short motion based on cartesian poses to demonstrate correct usage
- </description>
- </class>
- <class name="franka_example_controllers/CartesianVelocityExampleController" type="franka_example_controllers::CartesianVelocityExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that executes a short motion based on cartesian velocities to demonstrate correct usage
- </description>
- </class>
- <class name="franka_example_controllers/ElbowExampleController" type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that executes a short motion based on cartesian poses and elbow to demonstrate correct usage
- </description>
- </class>
- <class name="franka_example_controllers/ModelExampleController" type="franka_example_controllers::ModelExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that evaluates and prints the dynamic model of Franka
- </description>
- </class>
- <class name="franka_example_controllers/JointImpedanceExampleController" type="franka_example_controllers::JointImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
- <description>
- A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs.
- </description>
- </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.
- </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.
- </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/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
deleted file mode 100644
index 2e5d634..0000000
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
+++ /dev/null
@@ -1,69 +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 <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 <ros/node_handle.h>
-#include <ros/time.h>
-#include <Eigen/Dense>
-
-#include <franka_example_controllers/compliance_paramConfig.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
-
-namespace franka_example_controllers {
-
-class CartesianImpedanceExampleController : public controller_interface::MultiInterfaceController<
- franka_hw::FrankaModelInterface,
- hardware_interface::EffortJointInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
- void starting(const ros::Time&) override;
- void update(const ros::Time&, const ros::Duration& period) override;
-
- private:
- // Saturation
- Eigen::Matrix<double, 7, 1> saturateTorqueRate(
- const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
- const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming)
-
- std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
- std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
- std::vector<hardware_interface::JointHandle> joint_handles_;
-
- double filter_params_{0.005};
- double nullspace_stiffness_{20.0};
- double nullspace_stiffness_target_{20.0};
- const double delta_tau_max_{1.0};
- Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
- Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
- Eigen::Matrix<double, 6, 6> cartesian_damping_;
- Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
- Eigen::Matrix<double, 7, 1> q_d_nullspace_;
- Eigen::Vector3d position_d_;
- Eigen::Quaterniond orientation_d_;
- Eigen::Vector3d position_d_target_;
- Eigen::Quaterniond orientation_d_target_;
-
- // Dynamic reconfigure
- std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
- dynamic_server_compliance_param_;
- ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
- void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config,
- uint32_t level);
-
- // Equilibrium pose subscriber
- ros::Subscriber sub_equilibrium_pose_;
- void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
deleted file mode 100644
index e712c7a..0000000
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
+++ /dev/null
@@ -1,34 +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 <array>
-#include <memory>
-#include <string>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <franka_hw/franka_state_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-
-#include <franka_hw/franka_cartesian_command_interface.h>
-
-namespace franka_example_controllers {
-
-class CartesianPoseExampleController
- : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
- void starting(const ros::Time&) override;
- void update(const ros::Time&, const ros::Duration& period) override;
-
- private:
- franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
- std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
- ros::Duration elapsed_time_;
- std::array<double, 16> initial_pose_{};
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
deleted file mode 100644
index 95d4ace..0000000
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
+++ /dev/null
@@ -1,32 +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 <memory>
-#include <string>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_state_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-
-namespace franka_example_controllers {
-
-class CartesianVelocityExampleController : public controller_interface::MultiInterfaceController<
- franka_hw::FrankaVelocityCartesianInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
- void update(const ros::Time&, const ros::Duration& period) override;
- void starting(const ros::Time&) override;
- void stopping(const ros::Time&) override;
-
- private:
- franka_hw::FrankaVelocityCartesianInterface* velocity_cartesian_interface_;
- std::unique_ptr<franka_hw::FrankaCartesianVelocityHandle> velocity_cartesian_handle_;
- ros::Duration elapsed_time_;
-};
-
-} // namespace franka_example_controllers
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
deleted file mode 100644
index 3aab56f..0000000
--- a/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h
+++ /dev/null
@@ -1,182 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#pragma once
-
-#include <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/include/franka_example_controllers/elbow_example_controller.h b/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h
deleted file mode 100644
index 70a8cfb..0000000
--- a/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h
+++ /dev/null
@@ -1,35 +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 <array>
-#include <memory>
-#include <string>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <franka_hw/franka_state_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-
-#include <franka_hw/franka_cartesian_command_interface.h>
-
-namespace franka_example_controllers {
-
-class ElbowExampleController
- : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
- void starting(const ros::Time&) override;
- void update(const ros::Time&, const ros::Duration& period) override;
-
- private:
- franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
- std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
- ros::Duration elapsed_time_;
- std::array<double, 16> initial_pose_{};
- std::array<double, 2> initial_elbow_{};
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
deleted file mode 100644
index 1b7eb7d..0000000
--- a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
+++ /dev/null
@@ -1,61 +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 <memory>
-#include <string>
-#include <vector>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <dynamic_reconfigure/server.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/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-#include <Eigen/Core>
-
-#include <franka_example_controllers/desired_mass_paramConfig.h>
-
-namespace franka_example_controllers {
-
-class ForceExampleController : public controller_interface::MultiInterfaceController<
- franka_hw::FrankaModelInterface,
- hardware_interface::EffortJointInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
- void starting(const ros::Time&) override;
- void update(const ros::Time&, const ros::Duration& period) override;
-
- private:
- // Saturation
- Eigen::Matrix<double, 7, 1> saturateTorqueRate(
- const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
- const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming)
-
- std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
- std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
- std::vector<hardware_interface::JointHandle> joint_handles_;
-
- double desired_mass_{0.0};
- double target_mass_{0.0};
- double k_p_{0.0};
- double k_i_{0.0};
- double target_k_p_{0.0};
- double target_k_i_{0.0};
- double filter_gain_{0.001};
- Eigen::Matrix<double, 7, 1> tau_ext_initial_;
- Eigen::Matrix<double, 7, 1> tau_error_;
- static constexpr double kDeltaTauMax{1.0};
-
- // Dynamic reconfigure
- std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>
- dynamic_server_desired_mass_param_;
- ros::NodeHandle dynamic_reconfigure_desired_mass_param_node_;
- void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig& config,
- uint32_t level);
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
deleted file mode 100644
index 526629a..0000000
--- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
+++ /dev/null
@@ -1,60 +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 <memory>
-#include <string>
-#include <vector>
-
-#include <controller_interface/multi_interface_controller.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 <franka_example_controllers/JointTorqueComparison.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/trigger_rate.h>
-
-namespace franka_example_controllers {
-
-class JointImpedanceExampleController : public controller_interface::MultiInterfaceController<
- franka_hw::FrankaModelInterface,
- hardware_interface::EffortJointInterface,
- franka_hw::FrankaPoseCartesianInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
- void starting(const ros::Time&) override;
- void update(const ros::Time&, const ros::Duration& period) override;
-
- private:
- // Saturation
- std::array<double, 7> saturateTorqueRate(
- const std::array<double, 7>& tau_d_calculated,
- const std::array<double, 7>& tau_J_d); // NOLINT (readability-identifier-naming)
-
- std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
- std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
- std::vector<hardware_interface::JointHandle> joint_handles_;
-
- static constexpr double kDeltaTauMax{1.0};
- double radius_{0.1};
- double acceleration_time_{2.0};
- double vel_max_{0.05};
- double angle_{0.0};
- double vel_current_{0.0};
-
- std::vector<double> k_gains_;
- std::vector<double> d_gains_;
- double coriolis_factor_{1.0};
- std::array<double, 7> dq_filtered_;
- std::array<double, 16> initial_pose_;
-
- franka_hw::TriggerRate rate_trigger_{1.0};
- std::array<double, 7> last_tau_d_{};
- realtime_tools::RealtimePublisher<JointTorqueComparison> torques_publisher_;
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
deleted file mode 100644
index 4148851..0000000
--- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
+++ /dev/null
@@ -1,31 +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 <array>
-#include <string>
-#include <vector>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-
-namespace franka_example_controllers {
-
-class JointPositionExampleController : public controller_interface::MultiInterfaceController<
- hardware_interface::PositionJointInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
- void starting(const ros::Time&) override;
- void update(const ros::Time&, const ros::Duration& period) override;
-
- private:
- hardware_interface::PositionJointInterface* position_joint_interface_;
- std::vector<hardware_interface::JointHandle> position_joint_handles_;
- ros::Duration elapsed_time_;
- std::array<double, 7> initial_pose_{};
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
deleted file mode 100644
index 3c20999..0000000
--- a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
+++ /dev/null
@@ -1,32 +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 <string>
-#include <vector>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <franka_hw/franka_state_interface.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.h>
-
-namespace franka_example_controllers {
-
-class JointVelocityExampleController : public controller_interface::MultiInterfaceController<
- hardware_interface::VelocityJointInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
- void update(const ros::Time&, const ros::Duration& period) override;
- void starting(const ros::Time&) override;
- void stopping(const ros::Time&) override;
-
- private:
- hardware_interface::VelocityJointInterface* velocity_joint_interface_;
- std::vector<hardware_interface::JointHandle> velocity_joint_handles_;
- ros::Duration elapsed_time_;
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h
deleted file mode 100644
index e1b4aef..0000000
--- a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h
+++ /dev/null
@@ -1,34 +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 <memory>
-#include <string>
-
-#include <controller_interface/multi_interface_controller.h>
-#include <hardware_interface/robot_hw.h>
-#include <ros/node_handle.h>
-#include <ros/time.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 {
-
-class ModelExampleController
- : public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface,
- franka_hw::FrankaStateInterface> {
- public:
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
- void update(const ros::Time&, const ros::Duration&) override;
-
- private:
- franka_hw::FrankaStateInterface* franka_state_interface_;
- std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
- franka_hw::FrankaModelInterface* model_interface_;
- std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
- franka_hw::TriggerRate rate_trigger_{1.0};
-};
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
deleted file mode 100644
index c820927..0000000
--- a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
+++ /dev/null
@@ -1,30 +0,0 @@
-// Author: Enrico Corvaglia
-// https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_controllers/include/utils/pseudo_inversion.h
-// File provided under public domain
-// pseudo_inverse() computes the pseudo inverse of matrix M_ using SVD decomposition (can choose
-// between damped and not)
-// returns the pseudo inverted matrix M_pinv_
-
-#pragma once
-
-#include <Eigen/Core>
-#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;
-
- Eigen::JacobiSVD<Eigen::MatrixXd> svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV);
- Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
- Eigen::MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed.
- S_.setZero();
-
- for (int i = 0; i < sing_vals_.size(); i++)
- S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_);
-
- M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
-}
-
-} // namespace franka_example_controllers
diff --git a/franka_example_controllers/launch/cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/cartesian_impedance_example_controller.launch
deleted file mode 100644
index 590329c..0000000
--- a/franka_example_controllers/launch/cartesian_impedance_example_controller.launch
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <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="cartesian_impedance_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
- <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen">
- <param name="link_name" value="panda_link0" />
- </node>
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
-</launch>
diff --git a/franka_example_controllers/launch/cartesian_pose_example_controller.launch b/franka_example_controllers/launch/cartesian_pose_example_controller.launch
deleted file mode 100644
index 364c160..0000000
--- a/franka_example_controllers/launch/cartesian_pose_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <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="cartesian_pose_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/cartesian_velocity_example_controller.launch b/franka_example_controllers/launch/cartesian_velocity_example_controller.launch
deleted file mode 100644
index ee99d73..0000000
--- a/franka_example_controllers/launch/cartesian_velocity_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <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="cartesian_velocity_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
deleted file mode 100644
index 9dafe44..0000000
--- a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<?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/elbow_example_controller.launch b/franka_example_controllers/launch/elbow_example_controller.launch
deleted file mode 100644
index a883d6c..0000000
--- a/franka_example_controllers/launch/elbow_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <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="elbow_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/force_example_controller.launch b/franka_example_controllers/launch/force_example_controller.launch
deleted file mode 100644
index f01aa64..0000000
--- a/franka_example_controllers/launch/force_example_controller.launch
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <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="force_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
-</launch>
diff --git a/franka_example_controllers/launch/joint_impedance_example_controller.launch b/franka_example_controllers/launch/joint_impedance_example_controller.launch
deleted file mode 100644
index 380c5e1..0000000
--- a/franka_example_controllers/launch/joint_impedance_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_impedance_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/joint_position_example_controller.launch b/franka_example_controllers/launch/joint_position_example_controller.launch
deleted file mode 100644
index f3dacb6..0000000
--- a/franka_example_controllers/launch/joint_position_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_position_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/joint_velocity_example_controller.launch b/franka_example_controllers/launch/joint_velocity_example_controller.launch
deleted file mode 100644
index 15c276d..0000000
--- a/franka_example_controllers/launch/joint_velocity_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_velocity_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/model_example_controller.launch b/franka_example_controllers/launch/model_example_controller.launch
deleted file mode 100644
index 4bf3e7e..0000000
--- a/franka_example_controllers/launch/model_example_controller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
- <arg name="load_gripper" default="true" />
- <include file="$(find franka_control)/launch/franka_control.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="$(arg load_gripper)" />
- </include>
-
- <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="model_example_controller"/>
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-</launch>
diff --git a/franka_example_controllers/launch/move_to_start.launch b/franka_example_controllers/launch/move_to_start.launch
deleted file mode 100644
index d27edf4..0000000
--- a/franka_example_controllers/launch/move_to_start.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
- <arg name="robot_ip" />
-
- <include file="$(find franka_control)/launch/franka_control.launch">
- <arg name="robot_ip" value="$(arg robot_ip)" />
- <arg name="load_gripper" value="false" />
- </include>
- <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
- <arg name="load_gripper" value="false" />
- </include>
- <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
-</launch>
diff --git a/franka_example_controllers/launch/robot.rviz b/franka_example_controllers/launch/robot.rviz
deleted file mode 100644
index 07c4f7f..0000000
--- a/franka_example_controllers/launch/robot.rviz
+++ /dev/null
@@ -1,187 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 89
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- Splitter Ratio: 0.5
- Tree Height: 746
- - 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: ""
-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
- - 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
- panda_hand:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_leftfinger:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link0:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link1:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link2:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link3:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link4:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link5:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link6:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link7:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link8:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- panda_rightfinger:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- Name: RobotModel
- Robot Description: robot_description
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: panda_link0
- 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: 3.39362764
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: -0.139062241
- Y: 0.151414081
- Z: 0.0437288694
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.0500000007
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.00999999978
- Pitch: 0.485397696
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 0.500398219
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 1059
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016a00000384fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000384000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000384fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000384000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000004f90000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1918
- X: 1920
- Y: 19
diff --git a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz
deleted file mode 100644
index eacde25..0000000
--- a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz
+++ /dev/null
@@ -1,181 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- Splitter Ratio: 0.5
- Tree Height: 573
- - Class: rviz/Selection
- Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: ""
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- 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
- - 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
- panda_link0:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link1:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link2:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link3:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link4:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link5:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link6:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link7:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link8:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Name: RobotModel
- Robot Description: robot_description
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- - Class: rviz/InteractiveMarkers
- Enable Transparency: true
- Enabled: true
- Name: InteractiveMarkers
- Show Axes: false
- Show Descriptions: true
- Show Visual Aids: false
- Update Topic: /equilibrium_pose_marker/update
- Value: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: panda_link0
- 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: 4.644041061401367
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 0
- Y: 0
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.785398006439209
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 0.785398006439209
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 846
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c5000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000027000002c50000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000042fc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1200
- X: 295
- Y: 39
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
deleted file mode 100644
index bdd4b59..0000000
--- a/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz
+++ /dev/null
@@ -1,281 +0,0 @@
-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/mainpage.dox b/franka_example_controllers/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_example_controllers/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_example_controllers/msg/JointTorqueComparison.msg b/franka_example_controllers/msg/JointTorqueComparison.msg
deleted file mode 100644
index c519732..0000000
--- a/franka_example_controllers/msg/JointTorqueComparison.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-float64[7] tau_error
-float64[7] tau_commanded
-float64[7] tau_measured
-float64 root_mean_square_error
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
deleted file mode 100644
index 65636db..0000000
--- a/franka_example_controllers/package.xml
+++ /dev/null
@@ -1,43 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_example_controllers</name>
- <version>0.7.0</version>
- <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros_control</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_example_controllers</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <build_depend>message_generation</build_depend>
- <build_depend>eigen</build_depend>
-
- <build_export_depend>message_runtime</build_export_depend>
-
- <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>
- <depend>roscpp</depend>
-
- <exec_depend>franka_control</exec_depend>
- <exec_depend>franka_description</exec_depend>
- <exec_depend>message_runtime</exec_depend>
- <exec_depend>panda_moveit_config</exec_depend>
- <exec_depend>rospy</exec_depend>
-
- <export>
- <controller_interface plugin="${prefix}/franka_example_controllers_plugin.xml"/>
- </export>
-</package>
diff --git a/franka_example_controllers/rosdoc.yaml b/franka_example_controllers/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_example_controllers/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
deleted file mode 100755
index d971629..0000000
--- a/franka_example_controllers/scripts/dual_arm_interactive_marker.py
+++ /dev/null
@@ -1,253 +0,0 @@
-#!/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/scripts/interactive_marker.py b/franka_example_controllers/scripts/interactive_marker.py
deleted file mode 100755
index 376ac18..0000000
--- a/franka_example_controllers/scripts/interactive_marker.py
+++ /dev/null
@@ -1,141 +0,0 @@
-#!/usr/bin/python
-
-import rospy
-import tf.transformations
-import numpy as np
-
-from interactive_markers.interactive_marker_server import \
- InteractiveMarkerServer, InteractiveMarkerFeedback
-from visualization_msgs.msg import InteractiveMarker, \
- InteractiveMarkerControl
-from geometry_msgs.msg import PoseStamped
-from franka_msgs.msg import FrankaState
-
-marker_pose = PoseStamped()
-initial_pose_found = False
-pose_pub = None
-# [[min_x, max_x], [min_y, max_y], [min_z, max_z]]
-position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
-
-
-def publisherCallback(msg, link_name):
- marker_pose.header.frame_id = link_name
- marker_pose.header.stamp = rospy.Time(0)
- pose_pub.publish(marker_pose)
-
-
-def franka_state_callback(msg):
- initial_quaternion = \
- tf.transformations.quaternion_from_matrix(
- np.transpose(np.reshape(msg.O_T_EE,
- (4, 4))))
- initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
- marker_pose.pose.orientation.x = initial_quaternion[0]
- marker_pose.pose.orientation.y = initial_quaternion[1]
- marker_pose.pose.orientation.z = initial_quaternion[2]
- marker_pose.pose.orientation.w = initial_quaternion[3]
- marker_pose.pose.position.x = msg.O_T_EE[12]
- marker_pose.pose.position.y = msg.O_T_EE[13]
- marker_pose.pose.position.z = msg.O_T_EE[14]
- global initial_pose_found
- initial_pose_found = True
-
-
-def processFeedback(feedback):
- if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
- marker_pose.pose.position.x = max([min([feedback.pose.position.x,
- position_limits[0][1]]),
- position_limits[0][0]])
- marker_pose.pose.position.y = max([min([feedback.pose.position.y,
- position_limits[1][1]]),
- position_limits[1][0]])
- marker_pose.pose.position.z = max([min([feedback.pose.position.z,
- position_limits[2][1]]),
- position_limits[2][0]])
- marker_pose.pose.orientation = feedback.pose.orientation
- server.applyChanges()
-
-
-if __name__ == "__main__":
- rospy.init_node("equilibrium_pose_node")
- state_sub = rospy.Subscriber("franka_state_controller/franka_states",
- FrankaState, franka_state_callback)
- listener = tf.TransformListener()
- link_name = rospy.get_param("~link_name")
-
- # Get initial pose for the interactive marker
- while not initial_pose_found:
- rospy.sleep(1)
- state_sub.unregister()
-
- pose_pub = rospy.Publisher(
- "equilibrium_pose", PoseStamped, queue_size=10)
- server = InteractiveMarkerServer("equilibrium_pose_marker")
- int_marker = InteractiveMarker()
- int_marker.header.frame_id = link_name
- int_marker.scale = 0.3
- int_marker.name = "equilibrium_pose"
- int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
- "If you move the \nequilibrium "
- "pose the robot will follow it\n"
- "so be aware of potential collisions")
- int_marker.pose = marker_pose.pose
- # run pose publisher
- rospy.Timer(rospy.Duration(0.005),
- lambda msg: publisherCallback(msg, link_name))
-
- # 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)
- server.insert(int_marker, processFeedback)
-
- server.applyChanges()
-
- rospy.spin()
diff --git a/franka_example_controllers/scripts/move_to_start.py b/franka_example_controllers/scripts/move_to_start.py
deleted file mode 100755
index f6d5221..0000000
--- a/franka_example_controllers/scripts/move_to_start.py
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-from moveit_commander import MoveGroupCommander
-from actionlib_msgs.msg import GoalStatusArray
-
-if __name__ == '__main__':
- rospy.init_node('move_to_start')
- rospy.wait_for_message('move_group/status', GoalStatusArray)
- commander = MoveGroupCommander('panda_arm')
- commander.set_named_target('ready')
- commander.go()
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
deleted file mode 100644
index 525b785..0000000
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/cartesian_impedance_example_controller.h>
-
-#include <cmath>
-#include <memory>
-
-#include <controller_interface/controller_base.h>
-#include <franka/robot_state.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-#include <franka_example_controllers/pseudo_inversion.h>
-
-namespace franka_example_controllers {
-
-bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
- ros::NodeHandle& node_handle) {
- std::vector<double> cartesian_stiffness_vector;
- std::vector<double> cartesian_damping_vector;
-
- sub_equilibrium_pose_ = node_handle.subscribe(
- "/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
- ros::TransportHints().reliable().tcpNoDelay());
-
- std::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
- return false;
- }
- std::vector<std::string> joint_names;
- if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
- ROS_ERROR(
- "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
- "aborting controller init!");
- return false;
- }
-
- auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
- if (model_interface == nullptr) {
- ROS_ERROR_STREAM(
- "CartesianImpedanceExampleController: Error getting model interface from hardware");
- return false;
- }
- try {
- model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
- model_interface->getHandle(arm_id + "_model"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "CartesianImpedanceExampleController: 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(
- "CartesianImpedanceExampleController: Error getting state interface from hardware");
- return false;
- }
- try {
- state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
- state_interface->getHandle(arm_id + "_robot"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "CartesianImpedanceExampleController: 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(
- "CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
- return false;
- }
- for (size_t i = 0; i < 7; ++i) {
- try {
- joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
- return false;
- }
- }
-
- dynamic_reconfigure_compliance_param_node_ =
- ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
-
- dynamic_server_compliance_param_ = std::make_unique<
- dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>(
-
- dynamic_reconfigure_compliance_param_node_);
- dynamic_server_compliance_param_->setCallback(
- boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
-
- position_d_.setZero();
- orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
- position_d_target_.setZero();
- orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
-
- cartesian_stiffness_.setZero();
- cartesian_damping_.setZero();
-
- return true;
-}
-
-void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
- // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
- // to initial configuration
- franka::RobotState initial_state = state_handle_->getRobotState();
- // get jacobian
- std::array<double, 42> jacobian_array =
- model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
- // convert to eigen
- 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 equilibrium point to current state
- position_d_ = initial_transform.translation();
- orientation_d_ = Eigen::Quaterniond(initial_transform.linear());
- position_d_target_ = initial_transform.translation();
- orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear());
-
- // set nullspace equilibrium configuration to initial q
- q_d_nullspace_ = q_initial;
-}
-
-void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
- const ros::Duration& /*period*/) {
- // get state variables
- franka::RobotState robot_state = state_handle_->getRobotState();
- std::array<double, 7> coriolis_array = model_handle_->getCoriolis();
- std::array<double, 42> jacobian_array =
- 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 - position_d_;
-
- // orientation error
- if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) {
- orientation.coeffs() << -orientation.coeffs();
- }
- // "difference" quaternion
- Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_);
- error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
- // Transform to base frame
- error.tail(3) << -transform.linear() * error.tail(3);
-
- // 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;
- pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);
-
- // Cartesian PD control with damping ratio = 1
- tau_task << jacobian.transpose() *
- (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq));
- // nullspace PD control with damping ratio = 1
- tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
- jacobian.transpose() * jacobian_transpose_pinv) *
- (nullspace_stiffness_ * (q_d_nullspace_ - q) -
- (2.0 * sqrt(nullspace_stiffness_)) * dq);
- // Desired torque
- tau_d << tau_task + tau_nullspace + coriolis;
- // Saturate torque rate to avoid discontinuities
- tau_d << saturateTorqueRate(tau_d, tau_J_d);
- for (size_t i = 0; i < 7; ++i) {
- joint_handles_[i].setCommand(tau_d(i));
- }
-
- // update parameters changed online either through dynamic reconfigure or through the interactive
- // target by filtering
- cartesian_stiffness_ =
- filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_;
- cartesian_damping_ =
- filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_;
- nullspace_stiffness_ =
- filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_;
- position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_;
- orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_);
-}
-
-Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
- 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, delta_tau_max_), -delta_tau_max_);
- }
- return tau_d_saturated;
-}
-
-void CartesianImpedanceExampleController::complianceParamCallback(
- franka_example_controllers::compliance_paramConfig& config,
- uint32_t /*level*/) {
- cartesian_stiffness_target_.setIdentity();
- cartesian_stiffness_target_.topLeftCorner(3, 3)
- << config.translational_stiffness * Eigen::Matrix3d::Identity();
- cartesian_stiffness_target_.bottomRightCorner(3, 3)
- << config.rotational_stiffness * Eigen::Matrix3d::Identity();
- cartesian_damping_target_.setIdentity();
- // Damping ratio = 1
- cartesian_damping_target_.topLeftCorner(3, 3)
- << 2.0 * sqrt(config.translational_stiffness) * Eigen::Matrix3d::Identity();
- cartesian_damping_target_.bottomRightCorner(3, 3)
- << 2.0 * sqrt(config.rotational_stiffness) * Eigen::Matrix3d::Identity();
- nullspace_stiffness_target_ = config.nullspace_stiffness;
-}
-
-void CartesianImpedanceExampleController::equilibriumPoseCallback(
- const geometry_msgs::PoseStampedConstPtr& msg) {
- position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
- Eigen::Quaterniond last_orientation_d_target(orientation_d_target_);
- orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y,
- msg->pose.orientation.z, msg->pose.orientation.w;
- if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) {
- orientation_d_target_.coeffs() << -orientation_d_target_.coeffs();
- }
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
deleted file mode 100644
index af69500..0000000
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/cartesian_pose_example_controller.h>
-
-#include <cmath>
-#include <memory>
-#include <stdexcept>
-#include <string>
-
-#include <controller_interface/controller_base.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <hardware_interface/hardware_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-namespace franka_example_controllers {
-
-bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& node_handle) {
- cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
- if (cartesian_pose_interface_ == nullptr) {
- ROS_ERROR(
- "CartesianPoseExampleController: Could not get Cartesian Pose "
- "interface from hardware");
- return false;
- }
-
- std::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
- return false;
- }
-
- try {
- cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
- cartesian_pose_interface_->getHandle(arm_id + "_robot"));
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM(
- "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
- return false;
- }
-
- auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
- if (state_interface == nullptr) {
- ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
- return false;
- }
-
- try {
- auto state_handle = state_interface->getHandle(arm_id + "_robot");
-
- std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- for (size_t i = 0; i < q_start.size(); i++) {
- if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
- ROS_ERROR_STREAM(
- "CartesianPoseExampleController: Robot is not in the expected starting position for "
- "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
- "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
- return false;
- }
- }
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM(
- "CartesianPoseExampleController: Exception getting state handle: " << e.what());
- return false;
- }
-
- return true;
-}
-
-void CartesianPoseExampleController::starting(const ros::Time& /* time */) {
- initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
- elapsed_time_ = ros::Duration(0.0);
-}
-
-void CartesianPoseExampleController::update(const ros::Time& /* time */,
- const ros::Duration& period) {
- elapsed_time_ += period;
-
- double radius = 0.3;
- double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
- double delta_x = radius * std::sin(angle);
- double delta_z = radius * (std::cos(angle) - 1);
- std::array<double, 16> new_pose = initial_pose_;
- new_pose[12] -= delta_x;
- new_pose[14] -= delta_z;
- cartesian_pose_handle_->setCommand(new_pose);
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianPoseExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
deleted file mode 100644
index 67c34a2..0000000
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/cartesian_velocity_example_controller.h>
-
-#include <array>
-#include <cmath>
-#include <memory>
-#include <string>
-
-#include <controller_interface/controller_base.h>
-#include <hardware_interface/hardware_interface.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-namespace franka_example_controllers {
-
-bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& node_handle) {
- std::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id");
- return false;
- }
-
- velocity_cartesian_interface_ =
- robot_hardware->get<franka_hw::FrankaVelocityCartesianInterface>();
- if (velocity_cartesian_interface_ == nullptr) {
- ROS_ERROR(
- "CartesianVelocityExampleController: Could not get Cartesian velocity interface from "
- "hardware");
- return false;
- }
- try {
- velocity_cartesian_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>(
- velocity_cartesian_interface_->getHandle(arm_id + "_robot"));
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM(
- "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());
- return false;
- }
-
- auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
- if (state_interface == nullptr) {
- ROS_ERROR("CartesianVelocityExampleController: Could not get state interface from hardware");
- return false;
- }
-
- try {
- auto state_handle = state_interface->getHandle(arm_id + "_robot");
-
- std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- for (size_t i = 0; i < q_start.size(); i++) {
- if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
- ROS_ERROR_STREAM(
- "CartesianVelocityExampleController: Robot is not in the expected starting position "
- "for running this example. Run `roslaunch franka_example_controllers "
- "move_to_start.launch robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` "
- "first.");
- return false;
- }
- }
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM(
- "CartesianVelocityExampleController: Exception getting state handle: " << e.what());
- return false;
- }
-
- return true;
-}
-
-void CartesianVelocityExampleController::starting(const ros::Time& /* time */) {
- elapsed_time_ = ros::Duration(0.0);
-}
-
-void CartesianVelocityExampleController::update(const ros::Time& /* time */,
- const ros::Duration& period) {
- elapsed_time_ += period;
-
- double time_max = 4.0;
- double v_max = 0.05;
- double angle = M_PI / 4.0;
- double cycle = std::floor(
- pow(-1.0, (elapsed_time_.toSec() - std::fmod(elapsed_time_.toSec(), time_max)) / time_max));
- double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * elapsed_time_.toSec()));
- double v_x = std::cos(angle) * v;
- double v_z = -std::sin(angle) * v;
- std::array<double, 6> command = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}};
- velocity_cartesian_handle_->setCommand(command);
-}
-
-void CartesianVelocityExampleController::stopping(const ros::Time& /*time*/) {
- // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION
- // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT
- // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianVelocityExampleController,
- controller_interface::ControllerBase)
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
deleted file mode 100644
index aee01b6..0000000
--- a/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp
+++ /dev/null
@@ -1,412 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_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_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp
deleted file mode 100644
index c988cb5..0000000
--- a/franka_example_controllers/src/elbow_example_controller.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/elbow_example_controller.h>
-
-#include <cmath>
-#include <memory>
-#include <stdexcept>
-#include <string>
-
-#include <controller_interface/controller_base.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <hardware_interface/hardware_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-namespace franka_example_controllers {
-
-bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& node_handle) {
- cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
- if (cartesian_pose_interface_ == nullptr) {
- ROS_ERROR("ElbowExampleController: Could not get Cartesian Pose interface from hardware");
- return false;
- }
-
- std::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("ElbowExampleController: Could not get parameter arm_id");
- return false;
- }
-
- try {
- cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
- cartesian_pose_interface_->getHandle(arm_id + "_robot"));
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what());
- return false;
- }
-
- auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
- if (state_interface == nullptr) {
- ROS_ERROR("ElbowExampleController: Could not get state interface from hardware");
- return false;
- }
-
- try {
- auto state_handle = state_interface->getHandle(arm_id + "_robot");
-
- std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- for (size_t i = 0; i < q_start.size(); i++) {
- if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
- ROS_ERROR_STREAM(
- "ElbowExampleController: Robot is not in the expected starting position for "
- "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
- "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
- return false;
- }
- }
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM("ElbowExampleController: Exception getting state handle: " << e.what());
- return false;
- }
-
- return true;
-}
-
-void ElbowExampleController::starting(const ros::Time& /* time */) {
- initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
- initial_elbow_ = cartesian_pose_handle_->getRobotState().elbow_d;
- elapsed_time_ = ros::Duration(0.0);
-}
-
-void ElbowExampleController::update(const ros::Time& /* time */, const ros::Duration& period) {
- elapsed_time_ += period;
-
- double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
- auto elbow = initial_elbow_;
- elbow[0] += angle;
-
- cartesian_pose_handle_->setCommand(initial_pose_, elbow);
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ElbowExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp
deleted file mode 100644
index 36cce5d..0000000
--- a/franka_example_controllers/src/force_example_controller.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/force_example_controller.h>
-
-#include <cmath>
-#include <memory>
-
-#include <controller_interface/controller_base.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-#include <franka/robot_state.h>
-
-namespace franka_example_controllers {
-
-bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
- ros::NodeHandle& node_handle) {
- std::vector<std::string> joint_names;
- std::string arm_id;
- ROS_WARN(
- "ForceExampleController: Make sure your robot's endeffector is in contact "
- "with a horizontal surface before starting the controller!");
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("ForceExampleController: Could not read parameter arm_id");
- return false;
- }
- if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
- ROS_ERROR(
- "ForceExampleController: Invalid or no joint_names parameters provided, aborting "
- "controller init!");
- return false;
- }
-
- auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
- if (model_interface == nullptr) {
- ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware");
- return false;
- }
- try {
- model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
- model_interface->getHandle(arm_id + "_model"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "ForceExampleController: 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("ForceExampleController: Error getting state interface from hardware");
- return false;
- }
- try {
- state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
- state_interface->getHandle(arm_id + "_robot"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "ForceExampleController: 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("ForceExampleController: Error getting effort joint interface from hardware");
- return false;
- }
- for (size_t i = 0; i < 7; ++i) {
- try {
- joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM("ForceExampleController: Exception getting joint handles: " << ex.what());
- return false;
- }
- }
-
- dynamic_reconfigure_desired_mass_param_node_ =
- ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
- dynamic_server_desired_mass_param_ = std::make_unique<
- dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>(
-
- dynamic_reconfigure_desired_mass_param_node_);
- dynamic_server_desired_mass_param_->setCallback(
- boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
-
- return true;
-}
-
-void ForceExampleController::starting(const ros::Time& /*time*/) {
- franka::RobotState robot_state = state_handle_->getRobotState();
- std::array<double, 7> gravity_array = model_handle_->getGravity();
- Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
- // Bias correction for the current external torque
- tau_ext_initial_ = tau_measured - gravity;
- tau_error_.setZero();
-}
-
-void ForceExampleController::update(const ros::Time& /*time*/, const ros::Duration& period) {
- franka::RobotState robot_state = state_handle_->getRobotState();
- std::array<double, 42> jacobian_array =
- model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
- std::array<double, 7> gravity_array = model_handle_->getGravity();
- Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
- robot_state.tau_J_d.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
-
- Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
- desired_force_torque.setZero();
- desired_force_torque(2) = desired_mass_ * -9.81;
- tau_ext = tau_measured - gravity - tau_ext_initial_;
- tau_d << jacobian.transpose() * desired_force_torque;
- tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
- // FF + PI control (PI gains are initially all 0)
- tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
- tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d);
-
- for (size_t i = 0; i < 7; ++i) {
- joint_handles_[i].setCommand(tau_cmd(i));
- }
-
- // Update signals changed online through dynamic reconfigure
- desired_mass_ = filter_gain_ * target_mass_ + (1 - filter_gain_) * desired_mass_;
- k_p_ = filter_gain_ * target_k_p_ + (1 - filter_gain_) * k_p_;
- k_i_ = filter_gain_ * target_k_i_ + (1 - filter_gain_) * k_i_;
-}
-
-void ForceExampleController::desiredMassParamCallback(
- franka_example_controllers::desired_mass_paramConfig& config,
- uint32_t /*level*/) {
- target_mass_ = config.desired_mass;
- target_k_p_ = config.k_p;
- target_k_i_ = config.k_i;
-}
-
-Eigen::Matrix<double, 7, 1> ForceExampleController::saturateTorqueRate(
- 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, kDeltaTauMax), -kDeltaTauMax);
- }
- return tau_d_saturated;
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ForceExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
deleted file mode 100644
index 607e87d..0000000
--- a/franka_example_controllers/src/joint_impedance_example_controller.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/joint_impedance_example_controller.h>
-
-#include <cmath>
-#include <memory>
-
-#include <controller_interface/controller_base.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-#include <franka/robot_state.h>
-
-namespace franka_example_controllers {
-
-bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
- ros::NodeHandle& node_handle) {
- std::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("JointImpedanceExampleController: Could not read parameter arm_id");
- return false;
- }
- if (!node_handle.getParam("radius", radius_)) {
- ROS_INFO_STREAM(
- "JointImpedanceExampleController: No parameter radius, defaulting to: " << radius_);
- }
- if (std::fabs(radius_) < 0.005) {
- ROS_INFO_STREAM("JointImpedanceExampleController: Set radius to small, defaulting to: " << 0.1);
- radius_ = 0.1;
- }
-
- if (!node_handle.getParam("vel_max", vel_max_)) {
- ROS_INFO_STREAM(
- "JointImpedanceExampleController: No parameter vel_max, defaulting to: " << vel_max_);
- }
- if (!node_handle.getParam("acceleration_time", acceleration_time_)) {
- ROS_INFO_STREAM(
- "JointImpedanceExampleController: No parameter acceleration_time, defaulting to: "
- << acceleration_time_);
- }
-
- std::vector<std::string> joint_names;
- if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
- ROS_ERROR(
- "JointImpedanceExampleController: Invalid or no joint_names parameters provided, aborting "
- "controller init!");
- return false;
- }
-
- if (!node_handle.getParam("k_gains", k_gains_) || k_gains_.size() != 7) {
- ROS_ERROR(
- "JointImpedanceExampleController: Invalid or no k_gain parameters provided, aborting "
- "controller init!");
- return false;
- }
-
- if (!node_handle.getParam("d_gains", d_gains_) || d_gains_.size() != 7) {
- ROS_ERROR(
- "JointImpedanceExampleController: Invalid or no d_gain parameters provided, aborting "
- "controller init!");
- return false;
- }
-
- double publish_rate(30.0);
- if (!node_handle.getParam("publish_rate", publish_rate)) {
- ROS_INFO_STREAM("JointImpedanceExampleController: publish_rate not found. Defaulting to "
- << publish_rate);
- }
- rate_trigger_ = franka_hw::TriggerRate(publish_rate);
-
- if (!node_handle.getParam("coriolis_factor", coriolis_factor_)) {
- ROS_INFO_STREAM("JointImpedanceExampleController: coriolis_factor not found. Defaulting to "
- << coriolis_factor_);
- }
-
- auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
- if (model_interface == nullptr) {
- ROS_ERROR_STREAM(
- "JointImpedanceExampleController: Error getting model interface from hardware");
- return false;
- }
- try {
- model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
- model_interface->getHandle(arm_id + "_model"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "JointImpedanceExampleController: Exception getting model handle from interface: "
- << ex.what());
- return false;
- }
-
- auto* cartesian_pose_interface = robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
- if (cartesian_pose_interface == nullptr) {
- ROS_ERROR_STREAM(
- "JointImpedanceExampleController: Error getting cartesian pose interface from hardware");
- return false;
- }
- try {
- cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
- cartesian_pose_interface->getHandle(arm_id + "_robot"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "JointImpedanceExampleController: Exception getting cartesian pose 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(
- "JointImpedanceExampleController: Error getting effort joint interface from hardware");
- return false;
- }
- for (size_t i = 0; i < 7; ++i) {
- try {
- joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "JointImpedanceExampleController: Exception getting joint handles: " << ex.what());
- return false;
- }
- }
- torques_publisher_.init(node_handle, "torque_comparison", 1);
-
- std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0);
-
- return true;
-}
-
-void JointImpedanceExampleController::starting(const ros::Time& /*time*/) {
- initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
-}
-
-void JointImpedanceExampleController::update(const ros::Time& /*time*/,
- const ros::Duration& period) {
- if (vel_current_ < vel_max_) {
- vel_current_ += period.toSec() * std::fabs(vel_max_ / acceleration_time_);
- }
- vel_current_ = std::fmin(vel_current_, vel_max_);
-
- angle_ += period.toSec() * vel_current_ / std::fabs(radius_);
- if (angle_ > 2 * M_PI) {
- angle_ -= 2 * M_PI;
- }
-
- double delta_y = radius_ * (1 - std::cos(angle_));
- double delta_z = radius_ * std::sin(angle_);
-
- std::array<double, 16> pose_desired = initial_pose_;
- pose_desired[13] += delta_y;
- pose_desired[14] += delta_z;
- cartesian_pose_handle_->setCommand(pose_desired);
-
- franka::RobotState robot_state = cartesian_pose_handle_->getRobotState();
- std::array<double, 7> coriolis = model_handle_->getCoriolis();
- std::array<double, 7> gravity = model_handle_->getGravity();
-
- double alpha = 0.99;
- for (size_t i = 0; i < 7; i++) {
- dq_filtered_[i] = (1 - alpha) * dq_filtered_[i] + alpha * robot_state.dq[i];
- }
-
- std::array<double, 7> tau_d_calculated;
- for (size_t i = 0; i < 7; ++i) {
- tau_d_calculated[i] = coriolis_factor_ * coriolis[i] +
- k_gains_[i] * (robot_state.q_d[i] - robot_state.q[i]) +
- d_gains_[i] * (robot_state.dq_d[i] - dq_filtered_[i]);
- }
-
- // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is
- // 1000 * (1 / sampling_time).
- std::array<double, 7> tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d);
-
- for (size_t i = 0; i < 7; ++i) {
- joint_handles_[i].setCommand(tau_d_saturated[i]);
- }
-
- if (rate_trigger_() && torques_publisher_.trylock()) {
- std::array<double, 7> tau_j = robot_state.tau_J;
- std::array<double, 7> tau_error;
- double error_rms(0.0);
- for (size_t i = 0; i < 7; ++i) {
- tau_error[i] = last_tau_d_[i] - tau_j[i];
- error_rms += std::sqrt(std::pow(tau_error[i], 2.0)) / 7.0;
- }
- torques_publisher_.msg_.root_mean_square_error = error_rms;
- for (size_t i = 0; i < 7; ++i) {
- torques_publisher_.msg_.tau_commanded[i] = last_tau_d_[i];
- torques_publisher_.msg_.tau_error[i] = tau_error[i];
- torques_publisher_.msg_.tau_measured[i] = tau_j[i];
- }
- torques_publisher_.unlockAndPublish();
- }
-
- for (size_t i = 0; i < 7; ++i) {
- last_tau_d_[i] = tau_d_saturated[i] + gravity[i];
- }
-}
-
-std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
- const std::array<double, 7>& tau_d_calculated,
- const std::array<double, 7>& tau_J_d) { // NOLINT (readability-identifier-naming)
- std::array<double, 7> tau_d_saturated{};
- for (size_t i = 0; i < 7; i++) {
- double difference = tau_d_calculated[i] - tau_J_d[i];
- tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
- }
- return tau_d_saturated;
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointImpedanceExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp
deleted file mode 100644
index 3ba12b8..0000000
--- a/franka_example_controllers/src/joint_position_example_controller.cpp
+++ /dev/null
@@ -1,81 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/joint_position_example_controller.h>
-
-#include <cmath>
-
-#include <controller_interface/controller_base.h>
-#include <hardware_interface/hardware_interface.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-namespace franka_example_controllers {
-
-bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& node_handle) {
- position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>();
- if (position_joint_interface_ == nullptr) {
- ROS_ERROR(
- "JointPositionExampleController: Error getting position joint interface from hardware!");
- return false;
- }
- std::vector<std::string> joint_names;
- if (!node_handle.getParam("joint_names", joint_names)) {
- ROS_ERROR("JointPositionExampleController: Could not parse joint names");
- }
- if (joint_names.size() != 7) {
- ROS_ERROR_STREAM("JointPositionExampleController: Wrong number of joint names, got "
- << joint_names.size() << " instead of 7 names!");
- return false;
- }
- position_joint_handles_.resize(7);
- for (size_t i = 0; i < 7; ++i) {
- try {
- position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]);
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM(
- "JointPositionExampleController: Exception getting joint handles: " << e.what());
- return false;
- }
- }
-
- std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- for (size_t i = 0; i < q_start.size(); i++) {
- if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) {
- ROS_ERROR_STREAM(
- "JointPositionExampleController: Robot is not in the expected starting position for "
- "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
- "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
- return false;
- }
- }
-
- return true;
-}
-
-void JointPositionExampleController::starting(const ros::Time& /* time */) {
- for (size_t i = 0; i < 7; ++i) {
- initial_pose_[i] = position_joint_handles_[i].getPosition();
- }
- elapsed_time_ = ros::Duration(0.0);
-}
-
-void JointPositionExampleController::update(const ros::Time& /*time*/,
- const ros::Duration& period) {
- elapsed_time_ += period;
-
- double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec())) * 0.2;
- for (size_t i = 0; i < 7; ++i) {
- if (i == 4) {
- position_joint_handles_[i].setCommand(initial_pose_[i] - delta_angle);
- } else {
- position_joint_handles_[i].setCommand(initial_pose_[i] + delta_angle);
- }
- }
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointPositionExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp
deleted file mode 100644
index ad01ad6..0000000
--- a/franka_example_controllers/src/joint_velocity_example_controller.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_example_controllers/joint_velocity_example_controller.h>
-
-#include <cmath>
-
-#include <controller_interface/controller_base.h>
-#include <hardware_interface/hardware_interface.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-namespace franka_example_controllers {
-
-bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& node_handle) {
- velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>();
- if (velocity_joint_interface_ == nullptr) {
- ROS_ERROR(
- "JointVelocityExampleController: Error getting velocity joint interface from hardware!");
- return false;
- }
- std::vector<std::string> joint_names;
- if (!node_handle.getParam("joint_names", joint_names)) {
- ROS_ERROR("JointVelocityExampleController: Could not parse joint names");
- }
- if (joint_names.size() != 7) {
- ROS_ERROR_STREAM("JointVelocityExampleController: Wrong number of joint names, got "
- << joint_names.size() << " instead of 7 names!");
- return false;
- }
- velocity_joint_handles_.resize(7);
- for (size_t i = 0; i < 7; ++i) {
- try {
- velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]);
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "JointVelocityExampleController: Exception getting joint handles: " << ex.what());
- return false;
- }
- }
-
- auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
- if (state_interface == nullptr) {
- ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware");
- return false;
- }
-
- try {
- auto state_handle = state_interface->getHandle("panda_robot");
-
- std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- for (size_t i = 0; i < q_start.size(); i++) {
- if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
- ROS_ERROR_STREAM(
- "JointVelocityExampleController: Robot is not in the expected starting position for "
- "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
- "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
- return false;
- }
- }
- } catch (const hardware_interface::HardwareInterfaceException& e) {
- ROS_ERROR_STREAM(
- "JointVelocityExampleController: Exception getting state handle: " << e.what());
- return false;
- }
-
- return true;
-}
-
-void JointVelocityExampleController::starting(const ros::Time& /* time */) {
- elapsed_time_ = ros::Duration(0.0);
-}
-
-void JointVelocityExampleController::update(const ros::Time& /* time */,
- const ros::Duration& period) {
- elapsed_time_ += period;
-
- ros::Duration time_max(8.0);
- double omega_max = 0.1;
- double cycle = std::floor(
- std::pow(-1.0, (elapsed_time_.toSec() - std::fmod(elapsed_time_.toSec(), time_max.toSec())) /
- time_max.toSec()));
- double omega = cycle * omega_max / 2.0 *
- (1.0 - std::cos(2.0 * M_PI / time_max.toSec() * elapsed_time_.toSec()));
-
- for (auto joint_handle : velocity_joint_handles_) {
- joint_handle.setCommand(omega);
- }
-}
-
-void JointVelocityExampleController::stopping(const ros::Time& /*time*/) {
- // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION
- // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT
- // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointVelocityExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp
deleted file mode 100644
index b67f241..0000000
--- a/franka_example_controllers/src/model_example_controller.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-
-#include <franka_example_controllers/model_example_controller.h>
-
-#include <algorithm>
-#include <array>
-#include <cstring>
-#include <iterator>
-#include <memory>
-
-#include <controller_interface/controller_base.h>
-#include <hardware_interface/hardware_interface.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
-
-namespace {
-template <class T, size_t N>
-std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
- ostream << "[";
- std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
- std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
- ostream << "]";
- return ostream;
-}
-} // anonymous namespace
-
-namespace franka_example_controllers {
-
-bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
- ros::NodeHandle& node_handle) {
- franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>();
- if (franka_state_interface_ == nullptr) {
- ROS_ERROR("ModelExampleController: Could not get Franka state interface from hardware");
- return false;
- }
- std::string arm_id;
- if (!node_handle.getParam("arm_id", arm_id)) {
- ROS_ERROR("ModelExampleController: Could not read parameter arm_id");
- return false;
- }
- model_interface_ = robot_hw->get<franka_hw::FrankaModelInterface>();
- if (model_interface_ == nullptr) {
- ROS_ERROR_STREAM("ModelExampleController: Error getting model interface from hardware");
- return false;
- }
-
- try {
- franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
- franka_state_interface_->getHandle(arm_id + "_robot"));
- } catch (const hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "ModelExampleController: Exception getting franka state handle: " << ex.what());
- return false;
- }
-
- try {
- model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
- model_interface_->getHandle(arm_id + "_model"));
- } catch (hardware_interface::HardwareInterfaceException& ex) {
- ROS_ERROR_STREAM(
- "ModelExampleController: Exception getting model handle from interface: " << ex.what());
- return false;
- }
- return true;
-}
-
-void ModelExampleController::update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {
- if (rate_trigger_()) {
- std::array<double, 49> mass = model_handle_->getMass();
- std::array<double, 7> coriolis = model_handle_->getCoriolis();
- std::array<double, 7> gravity = model_handle_->getGravity();
- std::array<double, 16> pose = model_handle_->getPose(franka::Frame::kJoint4);
- std::array<double, 42> joint4_body_jacobian =
- model_handle_->getBodyJacobian(franka::Frame::kJoint4);
- std::array<double, 42> endeffector_zero_jacobian =
- model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
-
- ROS_INFO("--------------------------------------------------");
- ROS_INFO_STREAM("mass :" << mass);
- ROS_INFO_STREAM("coriolis: " << coriolis);
- ROS_INFO_STREAM("gravity :" << gravity);
- ROS_INFO_STREAM("joint_pose :" << pose);
- ROS_INFO_STREAM("joint4_body_jacobian :" << joint4_body_jacobian);
- ROS_INFO_STREAM("joint_zero_jacobian :" << endeffector_zero_jacobian);
- }
-}
-
-} // namespace franka_example_controllers
-
-PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ModelExampleController,
- controller_interface::ControllerBase)
diff --git a/franka_gripper/CMakeLists.txt b/franka_gripper/CMakeLists.txt
deleted file mode 100644
index 14dd852..0000000
--- a/franka_gripper/CMakeLists.txt
+++ /dev/null
@@ -1,121 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_gripper)
-
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- message_generation
- control_msgs
- actionlib
- sensor_msgs
- xmlrpcpp
- actionlib_msgs
-)
-
-find_package(Franka 0.7.0 REQUIRED)
-
-add_action_files(
- DIRECTORY action
- FILES Grasp.action
- Homing.action
- Stop.action
- Move.action
-)
-
-add_message_files(
- DIRECTORY msg
- FILES GraspEpsilon.msg
-)
-
-generate_messages(DEPENDENCIES actionlib_msgs)
-
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES franka_gripper
- CATKIN_DEPENDS roscpp
- message_runtime
- control_msgs
- actionlib
- sensor_msgs
- xmlrpcpp
- actionlib_msgs
- DEPENDS Franka
-)
-
-add_library(franka_gripper
- src/franka_gripper.cpp
-)
-
-add_dependencies(franka_gripper
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- ${PROJECT_NAME}_generate_messages_cpp
-)
-
-target_link_libraries(franka_gripper
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_gripper SYSTEM PUBLIC
- ${Franka_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
-)
-target_include_directories(franka_gripper PUBLIC
- include
-)
-
-add_executable(franka_gripper_node
- src/franka_gripper_node.cpp
-)
-
-add_dependencies(franka_gripper_node
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- franka_gripper
-)
-
-target_link_libraries(franka_gripper_node
- ${catkin_LIBRARIES}
- franka_gripper
-)
-
-target_include_directories(franka_gripper_node SYSTEM PUBLIC
- ${catkin_INCLUDE_DIRS}
-)
-
-## Installation
-install(TARGETS franka_gripper
- franka_gripper_node
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-install(DIRECTORY launch
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY config
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
- RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_TOOLS)
- file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
- file(GLOB_RECURSE HEADERS
- ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
- ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
- )
- add_format_target(franka_gripper FILES ${SOURCES} ${HEADERS})
- add_tidy_target(franka_gripper
- FILES ${SOURCES}
- DEPENDS franka_gripper franka_gripper_node
- )
-endif()
diff --git a/franka_gripper/action/Grasp.action b/franka_gripper/action/Grasp.action
deleted file mode 100644
index b462efa..0000000
--- a/franka_gripper/action/Grasp.action
+++ /dev/null
@@ -1,8 +0,0 @@
-float64 width # [m]
-GraspEpsilon epsilon
-float64 speed # [m/s]
-float64 force # [N]
----
-bool success
-string error
----
diff --git a/franka_gripper/action/Homing.action b/franka_gripper/action/Homing.action
deleted file mode 100644
index a3b2826..0000000
--- a/franka_gripper/action/Homing.action
+++ /dev/null
@@ -1,4 +0,0 @@
----
-bool success
-string error
----
diff --git a/franka_gripper/action/Move.action b/franka_gripper/action/Move.action
deleted file mode 100644
index d908a46..0000000
--- a/franka_gripper/action/Move.action
+++ /dev/null
@@ -1,6 +0,0 @@
-float64 width # [m]
-float64 speed # [m/s]
----
-bool success
-string error
----
diff --git a/franka_gripper/action/Stop.action b/franka_gripper/action/Stop.action
deleted file mode 100644
index a3b2826..0000000
--- a/franka_gripper/action/Stop.action
+++ /dev/null
@@ -1,4 +0,0 @@
----
-bool success
-string error
----
diff --git a/franka_gripper/config/franka_gripper_node.yaml b/franka_gripper/config/franka_gripper_node.yaml
deleted file mode 100644
index 687e5d3..0000000
--- a/franka_gripper/config/franka_gripper_node.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-publish_rate: 30 # [Hz]
-default_speed: 0.1 # [m/s]
-default_grasp_epsilon:
- inner: 0.005 # [m]
- outer: 0.005 # [m]
diff --git a/franka_gripper/include/franka_gripper/franka_gripper.h b/franka_gripper/include/franka_gripper/franka_gripper.h
deleted file mode 100644
index 305d069..0000000
--- a/franka_gripper/include/franka_gripper/franka_gripper.h
+++ /dev/null
@@ -1,93 +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 <cmath>
-#include <string>
-
-#include <actionlib/server/simple_action_server.h>
-#include <control_msgs/GripperCommandAction.h>
-#include <ros/node_handle.h>
-
-#include <franka/exception.h>
-#include <franka/gripper.h>
-#include <franka/gripper_state.h>
-#include <franka_gripper/GraspAction.h>
-#include <franka_gripper/HomingAction.h>
-#include <franka_gripper/MoveAction.h>
-#include <franka_gripper/StopAction.h>
-
-namespace franka_gripper {
-
-/**
- * Reads a gripper state if possible
- *
- * @param[in] state A gripper state to update
- * @param[in] gripper A pointer to a franka gripper
- *
- * @return True if update was successful, false otherwise.
- */
-bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* state);
-
-/**
- * Wraps the execution of a gripper command action to catch exceptions and
- * report results
- *
- * @note
- * For compatibility with current MoveIt! behavior, the given goal's command position is
- * multiplied by a factor of 2 before being commanded to the gripper!
- *
- * @param[in] gripper A pointer to a franka gripper
- * @param[in] default_speed The default speed for a gripper action
- * @param[in] grasp_epsilon The epsilon window of the grasp.
- * @param[in] action_server A pointer to a gripper action server
- * @param[in] goal A gripper action goal
- */
-void gripperCommandExecuteCallback(
- const franka::Gripper& gripper,
- const GraspEpsilon& grasp_epsilon,
- double default_speed,
- actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
- const control_msgs::GripperCommandGoalConstPtr& goal);
-
-/**
- * Calls the libfranka move service of the gripper
- *
- * @param[in] gripper A gripper instance to execute the command
- * @param[in] goal A move goal with target width and velocity
- *
- * @return True if command was successful, false otherwise.
- */
-bool move(const franka::Gripper& gripper, const MoveGoalConstPtr& goal);
-
-/**
- * Calls the libfranka homing service of the gripper
- *
- * @param[in] gripper A gripper instance to execute the command
- *
- * @return True if command was successful, false otherwise.
- */
-bool homing(const franka::Gripper& gripper, const HomingGoalConstPtr& /*goal*/);
-
-/**
- * Calls the libfranka stop service of the gripper to stop applying force
- *
- * @param[in] gripper A gripper instance to execute the command
- *
- * @return True if command was successful, false otherwise.
- */
-bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/);
-
-/**
- * Calls the libfranka grasp service of the gripper
- *
- * An object is considered grasped if the distance \f$d\f$ between the gripper fingers satisfies
- * \f$(\text{width} - \text{epsilon_inner}) < d < (\text{width} + \text{epsilon_outer})\f$.
- *
- * @param[in] gripper A gripper instance to execute the command
- * @param[in] goal A grasp goal with target width, epsilon_inner, epsilon_outer, velocity and effort
- * @return True if an object has been grasped, false otherwise.
- */
-bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal);
-
-} // namespace franka_gripper
diff --git a/franka_gripper/launch/franka_gripper.launch b/franka_gripper/launch/franka_gripper.launch
deleted file mode 100644
index d566563..0000000
--- a/franka_gripper/launch/franka_gripper.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?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_gripper/mainpage.dox b/franka_gripper/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_gripper/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_gripper/msg/GraspEpsilon.msg b/franka_gripper/msg/GraspEpsilon.msg
deleted file mode 100644
index b750cf6..0000000
--- a/franka_gripper/msg/GraspEpsilon.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-float64 inner # [m]
-float64 outer # [m]
diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml
deleted file mode 100644
index cf1e314..0000000
--- a/franka_gripper/package.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_gripper</name>
- <version>0.7.0</version>
- <description>This package implements the franka gripper of type Franka Hand for the use in ros</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_gripper</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <depend>roscpp</depend>
- <depend>message_generation</depend>
- <depend>libfranka</depend>
- <depend>control_msgs</depend>
- <depend>actionlib</depend>
- <depend>sensor_msgs</depend>
- <depend>xmlrpcpp</depend>
- <depend>actionlib_msgs</depend>
-
- <exec_depend>message_runtime</exec_depend>
-
- <build_export_depend>message_runtime</build_export_depend>
-</package>
diff --git a/franka_gripper/rosdoc.yaml b/franka_gripper/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_gripper/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp
deleted file mode 100644
index 53fc6d1..0000000
--- a/franka_gripper/src/franka_gripper.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_gripper/franka_gripper.h>
-
-#include <cmath>
-#include <functional>
-#include <thread>
-
-#include <ros/node_handle.h>
-
-#include <franka/exception.h>
-#include <franka/gripper_state.h>
-#include <franka_gripper/GraspAction.h>
-#include <franka_gripper/HomingAction.h>
-#include <franka_gripper/MoveAction.h>
-#include <franka_gripper/StopAction.h>
-
-namespace franka_gripper {
-
-bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* state) {
- try {
- *state = gripper.readOnce();
- } catch (const franka::Exception& ex) {
- ROS_ERROR_STREAM("GripperServer: Exception reading gripper state: " << ex.what());
- return false;
- }
- return true;
-}
-
-void gripperCommandExecuteCallback(
- const franka::Gripper& gripper,
- const GraspEpsilon& grasp_epsilon,
- double default_speed,
- actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
- const control_msgs::GripperCommandGoalConstPtr& goal) {
- auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() {
- // HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
- // only sends us the width of one finger. Multiply by 2 to get the intended width.
- double target_width = 2 * goal->command.position;
-
- franka::GripperState state = gripper.readOnce();
- if (target_width > state.max_width || target_width < 0.0) {
- ROS_ERROR_STREAM("GripperServer: Commanding out of range width! max_width = "
- << state.max_width << " command = " << target_width);
- return false;
- }
- constexpr double kSamePositionThreshold = 1e-4;
- if (std::abs(target_width - state.width) < kSamePositionThreshold) {
- return true;
- }
- if (target_width >= state.width) {
- return gripper.move(target_width, default_speed);
- }
- return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
- grasp_epsilon.outer);
- };
-
- try {
- if (gripper_command_handler()) {
- franka::GripperState state;
- if (updateGripperState(gripper, &state)) {
- control_msgs::GripperCommandResult result;
- result.effort = 0.0;
- result.position = state.width;
- result.reached_goal = static_cast<decltype(result.reached_goal)>(true);
- result.stalled = static_cast<decltype(result.stalled)>(false);
- action_server->setSucceeded(result);
- return;
- }
- }
- } catch (const franka::Exception& ex) {
- ROS_ERROR_STREAM("" << ex.what());
- }
- action_server->setAborted();
-}
-
-bool move(const franka::Gripper& gripper, const MoveGoalConstPtr& goal) {
- return gripper.move(goal->width, goal->speed);
-}
-
-bool homing(const franka::Gripper& gripper, const HomingGoalConstPtr& /*goal*/) {
- return gripper.homing();
-}
-
-bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
- return gripper.stop();
-}
-
-bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
- return gripper.grasp(goal->width, goal->speed, goal->force, goal->epsilon.inner,
- goal->epsilon.outer);
-}
-
-} // namespace franka_gripper
diff --git a/franka_gripper/src/franka_gripper_node.cpp b/franka_gripper/src/franka_gripper_node.cpp
deleted file mode 100644
index 3b48a19..0000000
--- a/franka_gripper/src/franka_gripper_node.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <functional>
-#include <mutex>
-#include <string>
-#include <thread>
-#include <vector>
-
-#include <ros/init.h>
-#include <ros/node_handle.h>
-#include <ros/rate.h>
-#include <ros/spinner.h>
-#include <sensor_msgs/JointState.h>
-
-#include <franka/gripper_state.h>
-#include <franka_gripper/franka_gripper.h>
-
-namespace {
-
-template <typename T_action, typename T_goal, typename T_result>
-void handleErrors(actionlib::SimpleActionServer<T_action>* server,
- std::function<bool(const T_goal&)> handler,
- const T_goal& goal) {
- T_result result;
- try {
- result.success = handler(goal);
- server->setSucceeded(result);
- } catch (const franka::Exception& ex) {
- ROS_ERROR_STREAM("" << ex.what());
- result.success = false;
- result.error = ex.what();
- server->setAborted(result);
- }
-}
-
-} // anonymous namespace
-
-using actionlib::SimpleActionServer;
-using control_msgs::GripperCommandAction;
-using franka_gripper::grasp;
-using franka_gripper::GraspAction;
-using franka_gripper::GraspEpsilon;
-using franka_gripper::GraspGoalConstPtr;
-using franka_gripper::GraspResult;
-using franka_gripper::gripperCommandExecuteCallback;
-using franka_gripper::homing;
-using franka_gripper::HomingAction;
-using franka_gripper::HomingGoalConstPtr;
-using franka_gripper::HomingResult;
-using franka_gripper::move;
-using franka_gripper::MoveAction;
-using franka_gripper::MoveGoalConstPtr;
-using franka_gripper::MoveResult;
-using franka_gripper::stop;
-using franka_gripper::StopAction;
-using franka_gripper::StopGoalConstPtr;
-using franka_gripper::StopResult;
-using franka_gripper::updateGripperState;
-
-int main(int argc, char** argv) {
- ros::init(argc, argv, "franka_gripper_node");
- ros::NodeHandle node_handle("~");
- std::string robot_ip;
- if (!node_handle.getParam("robot_ip", robot_ip)) {
- ROS_ERROR("franka_gripper_node: Could not parse robot_ip parameter");
- return -1;
- }
-
- double default_speed(0.1);
- if (node_handle.getParam("default_speed", default_speed)) {
- ROS_INFO_STREAM("franka_gripper_node: Found default_speed " << default_speed);
- }
-
- GraspEpsilon default_grasp_epsilon;
- default_grasp_epsilon.inner = 0.005;
- default_grasp_epsilon.outer = 0.005;
- std::map<std::string, double> epsilon_map;
- if (node_handle.getParam("default_grasp_epsilon", epsilon_map)) {
- ROS_INFO_STREAM("franka_gripper_node: Found default_grasp_epsilon "
- << "inner: " << epsilon_map["inner"] << ", outer: " << epsilon_map["outer"]);
- default_grasp_epsilon.inner = epsilon_map["inner"];
- default_grasp_epsilon.outer = epsilon_map["outer"];
- }
-
- franka::Gripper gripper(robot_ip);
-
- auto homing_handler = [&gripper](auto&& goal) { return homing(gripper, goal); };
- auto stop_handler = [&gripper](auto&& goal) { return stop(gripper, goal); };
- auto grasp_handler = [&gripper](auto&& goal) { return grasp(gripper, goal); };
- auto move_handler = [&gripper](auto&& goal) { return move(gripper, goal); };
-
- SimpleActionServer<HomingAction> homing_action_server(
- node_handle, "homing",
- [=, &homing_action_server](auto&& goal) {
- return handleErrors<franka_gripper::HomingAction, franka_gripper::HomingGoalConstPtr,
- franka_gripper::HomingResult>(&homing_action_server, homing_handler,
- goal);
- },
- false);
-
- SimpleActionServer<StopAction> stop_action_server(
- node_handle, "stop",
- [=, &stop_action_server](auto&& goal) {
- return handleErrors<franka_gripper::StopAction, franka_gripper::StopGoalConstPtr,
- franka_gripper::StopResult>(&stop_action_server, stop_handler, goal);
- },
- false);
-
- SimpleActionServer<MoveAction> move_action_server(
- node_handle, "move",
- [=, &move_action_server](auto&& goal) {
- return handleErrors<franka_gripper::MoveAction, franka_gripper::MoveGoalConstPtr,
- franka_gripper::MoveResult>(&move_action_server, move_handler, goal);
- },
- false);
-
- SimpleActionServer<GraspAction> grasp_action_server(
- node_handle, "grasp",
- [=, &grasp_action_server](auto&& goal) {
- return handleErrors<franka_gripper::GraspAction, franka_gripper::GraspGoalConstPtr,
- franka_gripper::GraspResult>(&grasp_action_server, grasp_handler, goal);
- },
- false);
-
- SimpleActionServer<GripperCommandAction> gripper_command_action_server(
- node_handle, "gripper_action",
- [=, &gripper, &gripper_command_action_server](auto&& goal) {
- return gripperCommandExecuteCallback(gripper, default_grasp_epsilon, default_speed,
- &gripper_command_action_server, goal);
- },
- false);
-
- homing_action_server.start();
- stop_action_server.start();
- move_action_server.start();
- grasp_action_server.start();
- gripper_command_action_server.start();
-
- double publish_rate(30.0);
- if (!node_handle.getParam("publish_rate", publish_rate)) {
- ROS_INFO_STREAM("franka_gripper_node: Could not find parameter publish_rate. Defaulting to "
- << publish_rate);
- }
-
- std::vector<std::string> joint_names;
- if (!node_handle.getParam("joint_names", joint_names)) {
- ROS_ERROR("franka_gripper_node: Could not parse joint_names!");
- return -1;
- }
- if (joint_names.size() != 2) {
- ROS_ERROR("franka_gripper_node: Got wrong number of joint_names!");
- return -1;
- }
-
- franka::GripperState gripper_state;
- std::mutex gripper_state_mutex;
- std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() {
- ros::Rate read_rate(10);
- while (ros::ok()) {
- {
- std::lock_guard<std::mutex> _(gripper_state_mutex);
- updateGripperState(gripper, &gripper_state);
- }
- read_rate.sleep();
- }
- });
-
- ros::Publisher gripper_state_publisher =
- node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
- ros::AsyncSpinner spinner(2);
- spinner.start();
- ros::Rate rate(publish_rate);
- while (ros::ok()) {
- if (gripper_state_mutex.try_lock()) {
- sensor_msgs::JointState joint_states;
- joint_states.header.stamp = ros::Time::now();
- joint_states.name.push_back(joint_names[0]);
- joint_states.name.push_back(joint_names[1]);
- joint_states.position.push_back(gripper_state.width * 0.5);
- joint_states.position.push_back(gripper_state.width * 0.5);
- joint_states.velocity.push_back(0.0);
- joint_states.velocity.push_back(0.0);
- joint_states.effort.push_back(0.0);
- joint_states.effort.push_back(0.0);
- gripper_state_publisher.publish(joint_states);
- gripper_state_mutex.unlock();
- }
- rate.sleep();
- }
- read_thread.join();
- return 0;
-}
diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
deleted file mode 100644
index 417b816..0000000
--- a/franka_hw/CMakeLists.txt
+++ /dev/null
@@ -1,119 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_hw)
-
-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.7.0 REQUIRED)
-
-catkin_package(
- INCLUDE_DIRS include
- 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
-)
-
-add_dependencies(franka_hw
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_hw
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
- franka_control_services
-)
-
-target_include_directories(franka_hw SYSTEM PUBLIC
- ${Franka_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
-)
-target_include_directories(franka_hw PUBLIC
- include
-)
-
-## Installation
-install(TARGETS franka_hw
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-## 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()
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
- RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_TOOLS)
- file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
- file(GLOB_RECURSE HEADERS
- ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
- ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
- )
- add_format_target(franka_hw FILES ${SOURCES} ${HEADERS})
- add_tidy_target(franka_hw
- FILES ${SOURCES}
- DEPENDS franka_hw
- )
-endif()
diff --git a/franka_hw/franka_combinable_hw_plugin.xml b/franka_hw/franka_combinable_hw_plugin.xml
deleted file mode 100644
index 12763ff..0000000
--- a/franka_hw/franka_combinable_hw_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<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/control_mode.h b/franka_hw/include/franka_hw/control_mode.h
deleted file mode 100644
index ccf6915..0000000
--- a/franka_hw/include/franka_hw/control_mode.h
+++ /dev/null
@@ -1,49 +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 <ostream>
-#include <type_traits>
-
-namespace franka_hw {
-
-enum class ControlMode {
- None = 0,
- JointTorque = (1 << 0),
- JointPosition = (1 << 1),
- JointVelocity = (1 << 2),
- CartesianVelocity = (1 << 3),
- CartesianPose = (1 << 4),
-};
-
-std::ostream& operator<<(std::ostream& ostream, ControlMode mode);
-
-// Implement operators for BitmaskType concept
-constexpr ControlMode operator&(ControlMode left, ControlMode right) {
- return static_cast<ControlMode>(static_cast<std::underlying_type_t<ControlMode>>(left) &
- static_cast<std::underlying_type_t<ControlMode>>(right));
-}
-
-constexpr ControlMode operator|(ControlMode left, ControlMode right) {
- return static_cast<ControlMode>(static_cast<std::underlying_type_t<ControlMode>>(left) |
- static_cast<std::underlying_type_t<ControlMode>>(right));
-}
-
-constexpr ControlMode operator^(ControlMode left, ControlMode right) {
- return static_cast<ControlMode>(static_cast<std::underlying_type_t<ControlMode>>(left) ^
- static_cast<std::underlying_type_t<ControlMode>>(right));
-}
-
-constexpr ControlMode operator~(ControlMode mode) {
- return static_cast<ControlMode>(~static_cast<std::underlying_type_t<ControlMode>>(mode));
-}
-
-constexpr ControlMode& operator&=(ControlMode& left, ControlMode right) {
- return left = left & right;
-}
-
-constexpr ControlMode& operator|=(ControlMode& left, ControlMode right) {
- return left = left | right;
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/franka_cartesian_command_interface.h b/franka_hw/include/franka_hw/franka_cartesian_command_interface.h
deleted file mode 100644
index df4a98c..0000000
--- a/franka_hw/include/franka_hw/franka_cartesian_command_interface.h
+++ /dev/null
@@ -1,130 +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 <array>
-
-#include <franka_hw/franka_state_interface.h>
-#include <hardware_interface/internal/hardware_resource_manager.h>
-
-namespace franka_hw {
-
-/**
- * Handle to read and command a Cartesian pose.
- */
-class FrankaCartesianPoseHandle : public FrankaStateHandle {
- public:
- FrankaCartesianPoseHandle() = delete;
-
- /**
- * Creates an instance of a FrankaCartesianPoseHandle.
- *
- * @param[in] franka_state_handle Robot state handle.
- * @param[in] command A reference to the Cartesian pose command wrapped by this handle.
- * @param[in] elbow A reference to the elbow wrapped by this handle.
- */
- FrankaCartesianPoseHandle(const FrankaStateHandle& franka_state_handle,
- std::array<double, 16>& command,
- std::array<double, 2>& elbow)
- : FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}
-
- /**
- * Sets the given command.
- *
- * @param[in] command Command to set.
- */
- void setCommand(const std::array<double, 16>& command) noexcept {
- *command_ = command;
- *elbow_ = {};
- }
-
- /**
- * Sets the given command.
- *
- * @param[in] command Command to set.
- * @param[in] elbow Elbow to set.
- */
- void setCommand(const std::array<double, 16>& command,
- const std::array<double, 2>& elbow) noexcept {
- *command_ = command;
- *elbow_ = elbow;
- }
-
- /**
- * Gets the current command.
- *
- * @return Current command.
- */
- const std::array<double, 16>& getCommand() const noexcept { return *command_; }
-
- private:
- std::array<double, 16>* command_;
- std::array<double, 2>* elbow_;
-};
-
-/**
- * Hardware interface to command Cartesian poses.
- */
-class FrankaPoseCartesianInterface
- : public hardware_interface::HardwareResourceManager<FrankaCartesianPoseHandle,
- hardware_interface::ClaimResources> {};
-
-/**
- * Handle to read and command a Cartesian velocity.
- */
-class FrankaCartesianVelocityHandle : public FrankaStateHandle {
- public:
- FrankaCartesianVelocityHandle() = delete;
-
- /**
- * @param[in] franka_state_handle Robot state handle.
- * @param[in] command A reference to the Cartesian velocity command wrapped by this handle.
- * @param[in] elbow A reference to the elbow wrapped by this handle.
- */
- FrankaCartesianVelocityHandle(const FrankaStateHandle& franka_state_handle,
- std::array<double, 6>& command,
- std::array<double, 2>& elbow)
- : FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}
-
- /**
- * Sets the given command.
- *
- * @param[in] command Command to set.
- */
- void setCommand(std::array<double, 6>& command) noexcept {
- *command_ = command;
- *elbow_ = {};
- }
-
- /**
- * Sets the given command.
- *
- * @param[in] command Command to set.
- * @param[in] elbow Elbow to set.
- */
- void setCommand(const std::array<double, 6>& command,
- const std::array<double, 2>& elbow) noexcept {
- *command_ = command;
- *elbow_ = elbow;
- }
-
- /**
- * Gets the current command.
- *
- * @return Current command.
- */
- const std::array<double, 6>& getCommand() const noexcept { return *command_; }
-
- private:
- std::array<double, 6>* command_;
- std::array<double, 2>* elbow_;
-};
-
-/**
- * Hardware interface to command Cartesian velocities.
- */
-class FrankaVelocityCartesianInterface
- : public hardware_interface::HardwareResourceManager<FrankaCartesianVelocityHandle,
- hardware_interface::ClaimResources> {};
-
-} // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/franka_combinable_hw.h b/franka_hw/include/franka_hw/franka_combinable_hw.h
deleted file mode 100644
index bc2dffd..0000000
--- a/franka_hw/include/franka_hw/franka_combinable_hw.h
+++ /dev/null
@@ -1,170 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#pragma once
-
-#include <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
deleted file mode 100644
index 3072f31..0000000
--- a/franka_hw/include/franka_hw/franka_combined_hw.h
+++ /dev/null
@@ -1,64 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#pragma once
-
-#include <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
deleted file mode 100644
index 2b6ee5e..0000000
--- a/franka_hw/include/franka_hw/franka_hw.h
+++ /dev/null
@@ -1,516 +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 <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 <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:
- /**
- * 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.
- *
- * If no controller is active, the function immediately exits. When running a controller,
- * the function only exits when ros_callback returns false.
- *
- * @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.
- *
- * @throw franka::ControlException if an error related to torque control or motion generation
- * occurred.
- * @throw franka::InvalidOperationException if a conflicting operation is already running.
- * @throw franka::NetworkException if the connection is lost, e.g. after a timeout.
- * @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
- */
- 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.
- */
- 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.
- */
- virtual bool controllerActive() const noexcept;
-
- /**
- * Checks whether a requested controller can be run, based on the resources
- * and interfaces it claims.
- *
- * @param[in] info Controllers to be running at the same time.
- *
- * @return True in case of a conflict, false in case of valid controllers.
- */
- virtual bool checkForConflict(
- const std::list<hardware_interface::ControllerInfo>& info) const override;
-
- /**
- * Performs controller switching (real-time capable).
- */
- 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).
- *
- * @param[in] start_list Controllers requested to be started.
- * @param[in] stop_list Controllers requested to be stopped.
- *
- * @return True if the preparation has been successful, false otherwise.
- */
- 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 The current joint position command.
- */
- virtual std::array<double, 7> getJointPositionCommand() const noexcept;
-
- /**
- * Gets the current joint velocity command.
- *
- * @return The current joint velocity command.
- */
- virtual std::array<double, 7> getJointVelocityCommand() const noexcept;
-
- /**
- * Gets the current joint torque command.
- *
- * @return The current joint torque command.
- */
- virtual std::array<double, 7> getJointEffortCommand() const noexcept;
-
- /**
- * Enforces limits on position, velocity, and torque level.
- *
- * @param[in] period The duration of the current cycle.
- */
- virtual void enforceLimits(const ros::Duration& period);
-
- /**
- * Resets the limit interfaces.
- */
- 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); });
- }
-
- 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_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_{};
- FrankaStateInterface franka_state_interface_{};
- hardware_interface::PositionJointInterface position_joint_interface_{};
- hardware_interface::VelocityJointInterface velocity_joint_interface_{};
- hardware_interface::EffortJointInterface effort_joint_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_{};
-
- std::mutex libfranka_state_mutex_;
- std::mutex ros_state_mutex_;
- franka::RobotState robot_state_libfranka_{};
- franka::RobotState robot_state_ros_{};
-
- 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_;
-
- 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::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/include/franka_hw/franka_model_interface.h b/franka_hw/include/franka_hw/franka_model_interface.h
deleted file mode 100644
index 2bec190..0000000
--- a/franka_hw/include/franka_hw/franka_model_interface.h
+++ /dev/null
@@ -1,279 +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 <array>
-#include <string>
-
-#include <franka/model.h>
-#include <franka/robot_state.h>
-#include <hardware_interface/internal/hardware_resource_manager.h>
-
-namespace franka_hw {
-
-/**
- * Handle to perform calculations using the dynamic model of a robot.
- */
-class FrankaModelHandle {
- public:
- FrankaModelHandle() = delete;
-
- /**
- * Creates an instance of a FrankaModelHandle.
- *
- * @param[in] name The name of the model handle.
- * @param[in] model A reference to the franka::Model instance wrapped by this handle.
- * @param[in] robot_state A reference to the current robot state.
- */
- FrankaModelHandle(const std::string& name, franka::Model& model, franka::RobotState& robot_state)
- : name_(name), model_(&model), robot_state_(&robot_state) {}
-
- /**
- * Gets the name of the model handle.
- *
- * @return Name of the model handle.
- */
- std::string getName() const noexcept { return name_; }
-
- /**
- * Calculates the 7x7 mass matrix from the current robot state. Unit: \f$[kg \times m^2]\f$.
- *
- * @return Vectorized 7x7 mass matrix, column-major.
- *
- * @see franka::Model::mass
- */
- std::array<double, 49> getMass() const { return model_->mass(*robot_state_); }
-
- /**
- * Calculates the 7x7 mass matrix from a given robot state. Unit: \f$[kg \times m^2]\f$.
- *
- * @param[in] q Joint position. Unit: \f$[rad]\f$.
- * @param[in] total_inertia Inertia of the attached total load including end effector, relative to
- * center of mass, given as vectorized 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
- * @param[in] total_mass Weight of the attached total load including end effector.
- * Unit: \f$[kg]\f$.
- * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load
- * including end effector.
- * Unit: \f$[m]\f$.
- *
- * @return Vectorized 7x7 mass matrix, column-major.
- *
- * @see franka::Model::mass
- */
- std::array<double, 49> getMass(
- const std::array<double, 7>& q,
- const std::array<double, 9>& total_inertia,
- double total_mass,
- const std::array<double, 3>& F_x_Ctotal) const { // NOLINT (readability-identifier-naming)
- return model_->mass(q, total_inertia, total_mass, F_x_Ctotal);
- }
-
- /**
- * Calculates the Coriolis force vector (state-space equation) from the current robot state:
- * \f$ c= C \times dq\f$, in \f$[Nm]\f$.
- *
- * @return Coriolis force vector.
- *
- * @see franka::Model::coriolis
- */
- std::array<double, 7> getCoriolis() const { return model_->coriolis(*robot_state_); }
-
- /**
- * Calculates the Coriolis force vector (state-space equation) from the given robot state:
- * \f$ c= C \times dq\f$, in \f$[Nm]\f$.
- *
- * @param[in] q Joint position. Unit: \f$[rad]\f$.
- * @param[in] dq Joint velocity. Unit: \f$[\frac{rad}{s}]\f$.
- * @param[in] total_inertia Inertia of the attached total load including end effector, relative to
- * center of mass, given as vectorized 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
- * @param[in] total_mass Weight of the attached total load including end effector.
- * Unit: \f$[kg]\f$.
- * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load
- * including end effector.
- * Unit: \f$[m]\f$.
- *
- * @return Coriolis force vector.
- *
- * @see franka::Model::coriolis
- */
- std::array<double, 7> getCoriolis(
- const std::array<double, 7>& q,
- const std::array<double, 7>& dq,
- const std::array<double, 9>& total_inertia,
- double total_mass,
- const std::array<double, 3>& F_x_Ctotal) const { // NOLINT (readability-identifier-naming)
- return model_->coriolis(q, dq, total_inertia, total_mass, F_x_Ctotal);
- }
-
- /**
- * Calculates the gravity vector from the current robot state. Unit: \f$[Nm]\f$.
- *
- * @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
- * Default to {0.0, 0.0, -9.81}.
- *
- * @return Gravity vector.
- *
- * @see franka::Model::gravity
- */
- std::array<double, 7> getGravity(const std::array<double, 3>& gravity_earth = {
- {0., 0., -9.81}}) const {
- return model_->gravity(*robot_state_, gravity_earth);
- }
-
- /**
- * Calculates the gravity vector from the given robot state. Unit: \f$[Nm]\f$.
- *
- * @param[in] q Joint position. Unit: \f$[rad]\f$.
- * @param[in] total_mass Weight of the attached total load including end effector.
- * Unit: \f$[kg]\f$.
- * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load
- * including end effector.
- * Unit: \f$[m]\f$.
- * @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
- * Default to {0.0, 0.0, -9.81}.
- *
- * @return Gravity vector.
- *
- * @see franka::Model::gravity
- */
- std::array<double, 7> getGravity(
- const std::array<double, 7>& q,
- double total_mass,
- const std::array<double, 3>& F_x_Ctotal, // NOLINT (readability-identifier-naming)
- const std::array<double, 3>& gravity_earth = {{0., 0., -9.81}}) const {
- return model_->gravity(q, total_mass, F_x_Ctotal, gravity_earth);
- }
-
- /**
- * Gets the 4x4 pose matrix for the given frame in base frame, calculated from the current
- * robot state.
- *
- * The pose is represented as a 4x4 matrix in column-major format.
- *
- * @param[in] frame The desired frame.
- *
- * @return Vectorized 4x4 pose matrix, column-major.
- *
- * @see franka::Model::pose
- */
- std::array<double, 16> getPose(const franka::Frame& frame) const {
- return model_->pose(frame, *robot_state_);
- }
-
- /**
- * Gets the 4x4 pose matrix for the given frame in base frame, calculated from the given
- * robot state.
- *
- * The pose is represented as a 4x4 matrix in column-major format.
- *
- * @param[in] frame The desired frame.
- * @param[in] q Joint position. Unit: \f$[rad]\f$.
- * @param[in] F_T_EE End effector in flange frame.
- * @param[in] EE_T_K Stiffness frame K in the end effector frame.
- *
- * @return Vectorized 4x4 pose matrix, column-major.
- *
- * @see franka::Model::pose
- */
- std::array<double, 16> getPose(
- const franka::Frame& frame,
- const std::array<double, 7>& q,
- const std::array<double, 16>& F_T_EE, // NOLINT (readability-identifier-naming)
- const std::array<double, 16>& EE_T_K) // NOLINT (readability-identifier-naming)
- const {
- return model_->pose(frame, q, F_T_EE, EE_T_K);
- }
-
- /**
- * Gets the 6x7 Jacobian for the given frame, relative to that frame.
- *
- * The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
- * the current robot state.
- *
- * @param[in] frame The desired frame.
- *
- * @return Vectorized 6x7 Jacobian, column-major.
- *
- * @see franka::Model::bodyJacobian
- */
- std::array<double, 42> getBodyJacobian(const franka::Frame& frame) const {
- return model_->bodyJacobian(frame, *robot_state_);
- }
-
- /**
- * Gets the 6x7 Jacobian for the given frame, relative to that frame.
- *
- * The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
- * the given robot state.
- *
- * @param[in] frame The desired frame.
- * @param[in] q Joint position. Unit: \f$[rad]\f$.
- * @param[in] F_T_EE End effector in flange frame.
- * @param[in] EE_T_K Stiffness frame K in the end effector frame.
- *
- * @return Vectorized 6x7 Jacobian, column-major.
- *
- * @see franka::Model::bodyJacobian
- */
- std::array<double, 42> getBodyJacobian(
- const franka::Frame& frame,
- const std::array<double, 7>& q,
- const std::array<double, 16>& F_T_EE, // NOLINT (readability-identifier-naming)
- const std::array<double, 16>& EE_T_K) // NOLINT (readability-identifier-naming)
- const {
- return model_->bodyJacobian(frame, q, F_T_EE, EE_T_K);
- }
-
- /**
- * Gets the 6x7 Jacobian for the given joint relative to the base frame.
- *
- * The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
- * the current robot state.
- *
- * @param[in] frame The desired frame.
- *
- * @return Vectorized 6x7 Jacobian, column-major.
- *
- * @see franka::Model::zeroJacobian
- */
- std::array<double, 42> getZeroJacobian(const franka::Frame& frame) const {
- return model_->zeroJacobian(frame, *robot_state_);
- }
-
- /**
- * Gets the 6x7 Jacobian for the given joint relative to the base frame.
- *
- * The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
- * the given robot state.
- *
- * @param[in] frame The desired frame.
- * @param[in] q Joint position. Unit: \f$[rad]\f$.
- * @param[in] F_T_EE End effector in flange frame.
- * @param[in] EE_T_K Stiffness frame K in the end effector frame.
- *
- * @return Vectorized 6x7 Jacobian, column-major.
- *
- * @see franka::Model::zeroJacobian
- */
- std::array<double, 42> getZeroJacobian(
- const franka::Frame& frame,
- const std::array<double, 7>& q,
- const std::array<double, 16>& F_T_EE, // NOLINT (readability-identifier-naming)
- const std::array<double, 16>& EE_T_K) // NOLINT (readability-identifier-naming)
- const {
- return model_->zeroJacobian(frame, q, F_T_EE, EE_T_K);
- }
-
- private:
- std::string name_;
- const franka::Model* model_;
- const franka::RobotState* robot_state_;
-};
-
-/**
- * Hardware interface to perform calculations using the dynamic model of a robot.
- */
-class FrankaModelInterface : public hardware_interface::HardwareResourceManager<FrankaModelHandle> {
-};
-
-} // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/franka_state_interface.h b/franka_hw/include/franka_hw/franka_state_interface.h
deleted file mode 100644
index 0e3be4c..0000000
--- a/franka_hw/include/franka_hw/franka_state_interface.h
+++ /dev/null
@@ -1,55 +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 <string>
-
-#include <franka/robot_state.h>
-#include <hardware_interface/internal/hardware_resource_manager.h>
-
-namespace franka_hw {
-
-/**
- * Handle to read the complete state of a robot.
- */
-class FrankaStateHandle {
- public:
- FrankaStateHandle() = delete;
-
- /**
- * Creates an instance of a FrankaStateHandle.
- *
- * @param[in] name The name of the state handle.
- * @param[in] robot_state A reference to the robot state wrapped by this handle.
- */
- FrankaStateHandle(const std::string& name, franka::RobotState& robot_state)
- : name_(name), robot_state_(&robot_state) {}
-
- /**
- * Gets the name of the state handle.
- *
- * @return Name of the state handle.
- */
- const std::string& getName() const noexcept { return name_; }
-
- /**
- * Gets the current robot state.
- *
- * @return Current robot state.
- */
- const franka::RobotState& getRobotState() const noexcept { return *robot_state_; }
-
- private:
- std::string name_;
- const franka::RobotState* robot_state_;
-};
-
-/**
- * Hardware interface to read the complete robot state.
- *
- * @see franka::RobotState for a description of the values included in the robot state.
- */
-class FrankaStateInterface : public hardware_interface::HardwareResourceManager<FrankaStateHandle> {
-};
-
-} // namespace franka_hw
diff --git a/franka_hw/include/franka_hw/resource_helpers.h b/franka_hw/include/franka_hw/resource_helpers.h
deleted file mode 100644
index f2231ba..0000000
--- a/franka_hw/include/franka_hw/resource_helpers.h
+++ /dev/null
@@ -1,45 +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 <list>
-#include <map>
-#include <string>
-#include <vector>
-
-#include <hardware_interface/controller_info.h>
-
-#include <franka_hw/control_mode.h>
-
-namespace franka_hw {
-
-using ResourceWithClaimsMap = std::map<std::string, std::vector<std::vector<std::string>>>;
-
-struct ResourceClaims {
- uint8_t joint_position_claims = 0;
- uint8_t joint_velocity_claims = 0;
- uint8_t joint_torque_claims = 0;
- uint8_t cartesian_velocity_claims = 0;
- uint8_t cartesian_pose_claims = 0;
-};
-
-using ArmClaimedMap = std::map<std::string, ResourceClaims>;
-
-bool findArmIdInResourceId(const std::string& resource_id, std::string* arm_id);
-
-ResourceWithClaimsMap getResourceMap(const std::list<hardware_interface::ControllerInfo>& info);
-
-bool getArmClaimedMap(ResourceWithClaimsMap& resource_map, ArmClaimedMap& arm_claim_map);
-
-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
deleted file mode 100644
index 5d010b3..0000000
--- a/franka_hw/include/franka_hw/services.h
+++ /dev/null
@@ -1,160 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#pragma once
-#include <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/include/franka_hw/trigger_rate.h b/franka_hw/include/franka_hw/trigger_rate.h
deleted file mode 100644
index add514c..0000000
--- a/franka_hw/include/franka_hw/trigger_rate.h
+++ /dev/null
@@ -1,19 +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 <ros/time.h>
-
-namespace franka_hw {
-
-class TriggerRate {
- public:
- explicit TriggerRate(double rate = 30.0);
- bool operator()();
-
- private:
- ros::Time time_stamp_;
- double period_;
-};
-
-}; // namespace franka_hw
diff --git a/franka_hw/mainpage.dox b/franka_hw/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_hw/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_hw/package.xml b/franka_hw/package.xml
deleted file mode 100644
index a61cec9..0000000
--- a/franka_hw/package.xml
+++ /dev/null
@@ -1,37 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_hw</name>
- <version>0.7.0</version>
- <description>franka_hw provides hardware interfaces for using Franka Emika research robots with ros_control</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_hw</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <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/rosdoc.yaml b/franka_hw/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_hw/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_hw/src/control_mode.cpp b/franka_hw/src/control_mode.cpp
deleted file mode 100644
index cfd89c2..0000000
--- a/franka_hw/src/control_mode.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_hw/control_mode.h>
-
-#include <algorithm>
-#include <iterator>
-#include <string>
-#include <vector>
-
-namespace franka_hw {
-
-std::ostream& operator<<(std::ostream& ostream, ControlMode mode) {
- if (mode == ControlMode::None) {
- ostream << "<none>";
- } else {
- std::vector<std::string> names;
- if ((mode & ControlMode::JointTorque) != ControlMode::None) {
- names.emplace_back("joint_torque");
- }
- if ((mode & ControlMode::JointPosition) != ControlMode::None) {
- names.emplace_back("joint_position");
- }
- if ((mode & ControlMode::JointVelocity) != ControlMode::None) {
- names.emplace_back("joint_velocity");
- }
- if ((mode & ControlMode::CartesianVelocity) != ControlMode::None) {
- names.emplace_back("cartesian_velocity");
- }
- if ((mode & ControlMode::CartesianPose) != ControlMode::None) {
- names.emplace_back("cartesian_pose");
- }
- std::copy(names.cbegin(), names.cend() - 1, std::ostream_iterator<std::string>(ostream, ", "));
- ostream << names.back();
- }
- return ostream;
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/src/franka_combinable_hw.cpp b/franka_hw/src/franka_combinable_hw.cpp
deleted file mode 100644
index 3b2b165..0000000
--- a/franka_hw/src/franka_combinable_hw.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_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
deleted file mode 100644
index 357b5bb..0000000
--- a/franka_hw/src/franka_combined_hw.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_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
deleted file mode 100644
index 2fce719..0000000
--- a/franka_hw/src/franka_hw.cpp
+++ /dev/null
@@ -1,590 +0,0 @@
-// 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>
-
-namespace franka_hw {
-
-namespace {
-std::ostream& operator<<(std::ostream& ostream, franka::ControllerMode mode) {
- if (mode == franka::ControllerMode::kJointImpedance) {
- ostream << "joint_impedance";
- } else if (mode == franka::ControllerMode::kCartesianImpedance) {
- ostream << "cartesian_impedance";
- } else {
- ostream << "<unknown>";
- }
- 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()
- : 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}),
- pose_cartesian_command_libfranka_(
- {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
- velocity_cartesian_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
- velocity_cartesian_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) {}
-
-bool FrankaHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
- if (initialized_) {
- ROS_ERROR("FrankaHW: Cannot be initialized twice.");
- return false;
- }
-
- if (!initParameters(root_nh, robot_hw_nh)) {
- ROS_ERROR("FrankaHW: Failed to parse all required parameters.");
- return false;
- }
- try {
- initRobot();
- } catch (const std::runtime_error& error) {
- ROS_ERROR("FrankaHW: Failed to initialize libfranka robot. %s", error.what());
- return false;
- }
- initROSInterfaces(robot_hw_nh);
- setupParameterCallbacks(robot_hw_nh);
-
- initialized_ = true;
- return true;
-}
-
-bool FrankaHW::initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
- std::vector<std::string> joint_names_vector;
- if (!robot_hw_nh.getParam("joint_names", joint_names_vector) || joint_names_vector.size() != 7) {
- ROS_ERROR("Invalid or no joint_names parameters provided");
- return false;
- }
- std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names_.begin());
-
- bool rate_limiting;
- if (!robot_hw_nh.getParamCached("rate_limiting", rate_limiting)) {
- ROS_ERROR("Invalid or no rate_limiting parameter provided");
- return false;
- }
-
- double cutoff_frequency;
- if (!robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency)) {
- ROS_ERROR("Invalid or no cutoff_frequency parameter provided");
- return false;
- }
-
- std::string internal_controller;
- if (!robot_hw_nh.getParam("internal_controller", internal_controller)) {
- ROS_ERROR("No internal_controller parameter provided");
- return false;
- }
-
- if (!robot_hw_nh.getParam("arm_id", arm_id_)) {
- ROS_ERROR("Invalid or no arm_id parameter provided");
- return false;
- }
-
- if (!urdf_model_.initParamWithNodeHandle("robot_description", root_nh)) {
- ROS_ERROR("Could not initialize URDF model from robot_description");
- return false;
- }
-
- if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) {
- ROS_ERROR("Invalid or no robot_ip parameter provided");
- return false;
- }
-
- if (!robot_hw_nh.getParam("joint_limit_warning_threshold", joint_limit_warning_threshold_)) {
- ROS_INFO(
- "No parameter joint_limit_warning_threshold is found, using default "
- "value %f",
- joint_limit_warning_threshold_);
- }
-
- 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;
- }
-
- // Get full collision behavior config from the parameter server.
- std::vector<double> thresholds =
- getCollisionThresholds("lower_torque_thresholds_acceleration", robot_hw_nh,
- {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.lower_torque_thresholds_acceleration.begin());
- thresholds = getCollisionThresholds("upper_torque_thresholds_acceleration", robot_hw_nh,
- {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.upper_torque_thresholds_acceleration.begin());
- thresholds = getCollisionThresholds("lower_torque_thresholds_nominal", robot_hw_nh,
- {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.lower_torque_thresholds_nominal.begin());
- thresholds = getCollisionThresholds("upper_torque_thresholds_nominal", robot_hw_nh,
- {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.upper_torque_thresholds_nominal.begin());
- thresholds.resize(6);
- thresholds = getCollisionThresholds("lower_force_thresholds_acceleration", robot_hw_nh,
- {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.lower_force_thresholds_acceleration.begin());
- thresholds = getCollisionThresholds("upper_force_thresholds_acceleration", robot_hw_nh,
- {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.upper_force_thresholds_acceleration.begin());
- thresholds = getCollisionThresholds("lower_force_thresholds_nominal", robot_hw_nh,
- {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.lower_force_thresholds_nominal.begin());
- thresholds = getCollisionThresholds("upper_force_thresholds_nominal", robot_hw_nh,
- {20.0, 20.0, 20.0, 25.0, 25.0, 25.0});
- std::copy(thresholds.begin(), thresholds.end(),
- collision_config_.upper_force_thresholds_nominal.begin());
-
- return true;
-}
-
-void FrankaHW::update(const franka::RobotState& robot_state) {
- std::lock_guard<std::mutex> ros_lock(ros_state_mutex_);
- robot_state_ros_ = robot_state;
-}
-
-bool FrankaHW::controllerActive() const noexcept {
- return controller_active_;
-}
-
-void FrankaHW::control(
- 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_ros_.time;
-
- 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;
- });
-}
-
-void FrankaHW::enforceLimits(const ros::Duration& period) {
- if (period.toSec() > 0.0) {
- position_joint_limit_interface_.enforceLimits(period);
- velocity_joint_limit_interface_.enforceLimits(period);
- effort_joint_limit_interface_.enforceLimits(period);
- }
-}
-
-bool FrankaHW::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_STREAM("FrankaHW: Unknown interface claimed. Conflict!");
- return true;
- }
- return hasConflictingJointAndCartesianClaim(arm_claim_map, arm_id_) ||
- partiallyClaimsArmJoints(arm_claim_map, arm_id_);
-}
-
-// 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) {
- reset();
- controller_active_ = true;
- }
-}
-
-// 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);
- ArmClaimedMap start_arm_claim_map;
- if (!getArmClaimedMap(start_resource_map, start_arm_claim_map)) {
- 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);
- ArmClaimedMap stop_arm_claim_map;
- if (!getArmClaimedMap(stop_resource_map, stop_arm_claim_map)) {
- ROS_ERROR("FrankaHW: Unknown interface claimed for stopping!");
- return false;
- }
- ControlMode stop_control_mode = getControlMode(arm_id_, stop_arm_claim_map);
-
- ControlMode requested_control_mode = current_control_mode_;
- requested_control_mode &= ~stop_control_mode;
- requested_control_mode |= start_control_mode;
-
- 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:
- break;
- 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_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_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_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_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_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_libfranka_), ros_callback, _1, _2),
- std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
- 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_libfranka_), ros_callback, _1, _2),
- std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
- 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_libfranka_), ros_callback, _1, _2),
- std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
- 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_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;
-}
-
-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_);
-}
-
-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());
-}
-
-void FrankaHW::setupParameterCallbacks(ros::NodeHandle& robot_hw_nh) {
- get_limit_rate_ = [robot_hw_nh]() {
- bool rate_limiting;
- robot_hw_nh.getParamCached("rate_limiting", rate_limiting);
- return rate_limiting;
- };
-
- get_internal_controller_ = [robot_hw_nh]() {
- std::string internal_controller;
- robot_hw_nh.getParamCached("internal_controller", internal_controller);
- franka::ControllerMode controller_mode;
- if (internal_controller == "joint_impedance") {
- controller_mode = franka::ControllerMode::kJointImpedance;
- } else if (internal_controller == "cartesian_impedance") {
- controller_mode = franka::ControllerMode::kCartesianImpedance;
- } else {
- ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance");
- controller_mode = franka::ControllerMode::kJointImpedance;
- }
- return controller_mode;
- };
-
- get_cutoff_frequency_ = [robot_hw_nh]() {
- double cutoff_frequency;
- robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency);
- return cutoff_frequency;
- };
-}
-
-bool FrankaHW::commandHasNaN(const franka::Torques& command) {
- return arrayHasNaN(command.tau_J);
-}
-
-bool FrankaHW::commandHasNaN(const franka::JointPositions& command) {
- return arrayHasNaN(command.q);
-}
-
-bool FrankaHW::commandHasNaN(const franka::JointVelocities& command) {
- return arrayHasNaN(command.dq);
-}
-
-bool FrankaHW::commandHasNaN(const franka::CartesianPose& command) {
- return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_T_EE);
-}
-
-bool FrankaHW::commandHasNaN(const franka::CartesianVelocities& command) {
- return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_dP_EE);
-}
-
-std::vector<double> FrankaHW::getCollisionThresholds(const std::string& name,
- ros::NodeHandle& robot_hw_nh,
- const std::vector<double>& defaults) {
- std::vector<double> thresholds;
- if (!robot_hw_nh.getParam("collision_config/" + name, thresholds) ||
- thresholds.size() != defaults.size()) {
- std::string message;
- for (const double& threshold : defaults) {
- message += std::to_string(threshold);
- message += " ";
- }
- ROS_INFO("No parameter %s found, using default values: %s", name.c_str(), message.c_str());
- return defaults;
- }
- return thresholds;
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/src/resource_helpers.cpp b/franka_hw/src/resource_helpers.cpp
deleted file mode 100644
index f921996..0000000
--- a/franka_hw/src/resource_helpers.cpp
+++ /dev/null
@@ -1,219 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_hw/resource_helpers.h>
-
-#include <ros/console.h>
-
-namespace franka_hw {
-
-bool findArmIdInResourceId(const std::string& resource_id, std::string* arm_id) {
- size_t position = resource_id.rfind("_joint");
- if (position != std::string::npos && position > 0) {
- *arm_id = resource_id.substr(0, position);
- return true;
- }
- position = resource_id.rfind("_robot");
- if (position != std::string::npos && position > 0) {
- *arm_id = resource_id.substr(0, position);
- return true;
- }
- return false;
-}
-
-ResourceWithClaimsMap getResourceMap(const std::list<hardware_interface::ControllerInfo>& info) {
- ResourceWithClaimsMap resource_map;
- for (auto& item : info) {
- const std::vector<hardware_interface::InterfaceResources>& c_res = item.claimed_resources;
- for (auto& resource_set : c_res) {
- const std::set<std::string>& iface_resources = resource_set.resources;
- for (auto& resource : iface_resources) {
- std::vector<std::string> claiming_controller(3);
- claiming_controller[0] = item.name;
- claiming_controller[1] = item.type;
- claiming_controller[2] = resource_set.hardware_interface;
- resource_map[resource].push_back(claiming_controller);
- }
- }
- }
- return resource_map;
-}
-
-bool getArmClaimedMap(ResourceWithClaimsMap& resource_map, ArmClaimedMap& arm_claim_map) {
- std::string current_arm_id;
-
- // 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.
- 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("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;
- for (auto claimed_by : map_it->second) {
- if (claimed_by[2] == "hardware_interface::EffortJointInterface") {
- new_claim.joint_torque_claims++;
- } else if (claimed_by[2] == "hardware_interface::PositionJointInterface") {
- new_claim.joint_position_claims++;
- } else if (claimed_by[2] == "hardware_interface::VelocityJointInterface") {
- new_claim.joint_velocity_claims++;
- } else if (claimed_by[2] == "franka_hw::FrankaPoseCartesianInterface") {
- new_claim.cartesian_pose_claims++;
- } else if (claimed_by[2] == "franka_hw::FrankaVelocityCartesianInterface") {
- new_claim.cartesian_velocity_claims++;
- } else {
- return false;
- }
- }
- arm_claim_map[current_arm_id].joint_position_claims += new_claim.joint_position_claims;
- arm_claim_map[current_arm_id].joint_velocity_claims += new_claim.joint_velocity_claims;
- arm_claim_map[current_arm_id].joint_torque_claims += new_claim.joint_torque_claims;
- arm_claim_map[current_arm_id].cartesian_velocity_claims += new_claim.cartesian_velocity_claims;
- arm_claim_map[current_arm_id].cartesian_pose_claims += new_claim.cartesian_pose_claims;
- }
- return true;
-}
-
-ControlMode getControlMode(const std::string& arm_id, ArmClaimedMap& arm_claim_map) {
- ControlMode control_mode = ControlMode::None;
- if (arm_claim_map[arm_id].joint_position_claims > 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims == 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::JointPosition;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims > 0 &&
- arm_claim_map[arm_id].joint_torque_claims == 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::JointVelocity;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims > 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::JointTorque;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims == 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims > 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::CartesianPose;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims == 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims > 0) {
- control_mode = ControlMode::CartesianVelocity;
- } else if (arm_claim_map[arm_id].joint_position_claims > 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims > 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::JointTorque | ControlMode::JointPosition;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims > 0 &&
- arm_claim_map[arm_id].joint_torque_claims > 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::JointTorque | ControlMode::JointVelocity;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims > 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims > 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims == 0) {
- control_mode = ControlMode::JointTorque | ControlMode::CartesianPose;
- } else if (arm_claim_map[arm_id].joint_position_claims == 0 &&
- arm_claim_map[arm_id].joint_velocity_claims == 0 &&
- arm_claim_map[arm_id].joint_torque_claims > 0 &&
- arm_claim_map[arm_id].cartesian_pose_claims == 0 &&
- arm_claim_map[arm_id].cartesian_velocity_claims > 0) {
- control_mode = ControlMode::JointTorque | ControlMode::CartesianVelocity;
- }
- 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_hw/src/services.cpp b/franka_hw/src/services.cpp
deleted file mode 100644
index 3557977..0000000
--- a/franka_hw/src/services.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_hw/services.h>
-
-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 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());
- robot.setCartesianImpedance(cartesian_stiffness);
-}
-
-void setJointImpedance(franka::Robot& robot,
- 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 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 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 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(),
- req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
- std::array<double, 7> upper_torque_thresholds_nominal;
- std::copy(req.upper_torque_thresholds_nominal.cbegin(),
- req.upper_torque_thresholds_nominal.cend(), upper_torque_thresholds_nominal.begin());
- std::array<double, 6> lower_force_thresholds_nominal;
- std::copy(req.lower_force_thresholds_nominal.cbegin(), req.lower_force_thresholds_nominal.cend(),
- lower_force_thresholds_nominal.begin());
- std::array<double, 6> upper_force_thresholds_nominal;
- std::copy(req.upper_force_thresholds_nominal.cbegin(), req.upper_force_thresholds_nominal.cend(),
- upper_force_thresholds_nominal.begin());
-
- robot.setCollisionBehavior(lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
- lower_force_thresholds_nominal, upper_force_thresholds_nominal);
-}
-
-void setFullCollisionBehavior(franka::Robot& robot,
- const 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(),
- lower_torque_thresholds_acceleration.begin());
- std::array<double, 7> upper_torque_thresholds_acceleration;
- std::copy(req.upper_torque_thresholds_acceleration.cbegin(),
- req.upper_torque_thresholds_acceleration.cend(),
- upper_torque_thresholds_acceleration.begin());
- std::array<double, 7> lower_torque_thresholds_nominal;
- std::copy(req.lower_torque_thresholds_nominal.cbegin(),
- req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
- std::array<double, 7> upper_torque_thresholds_nominal;
- std::copy(req.upper_torque_thresholds_nominal.cbegin(),
- req.upper_torque_thresholds_nominal.cend(), upper_torque_thresholds_nominal.begin());
- std::array<double, 6> lower_force_thresholds_acceleration;
- std::copy(req.lower_force_thresholds_acceleration.cbegin(),
- req.lower_force_thresholds_acceleration.cend(),
- lower_force_thresholds_acceleration.begin());
- std::array<double, 6> upper_force_thresholds_acceleration;
- std::copy(req.upper_force_thresholds_acceleration.cbegin(),
- req.upper_force_thresholds_acceleration.cend(),
- upper_force_thresholds_acceleration.begin());
- std::array<double, 6> lower_force_thresholds_nominal;
- std::copy(req.lower_force_thresholds_nominal.cbegin(), req.lower_force_thresholds_nominal.cend(),
- lower_force_thresholds_nominal.begin());
- std::array<double, 6> upper_force_thresholds_nominal;
- std::copy(req.upper_force_thresholds_nominal.cbegin(), req.upper_force_thresholds_nominal.cend(),
- upper_force_thresholds_nominal.begin());
- robot.setCollisionBehavior(lower_torque_thresholds_acceleration,
- upper_torque_thresholds_acceleration, lower_torque_thresholds_nominal,
- upper_torque_thresholds_nominal, lower_force_thresholds_acceleration,
- upper_force_thresholds_acceleration, lower_force_thresholds_nominal,
- upper_force_thresholds_nominal);
-}
-
-void setLoad(franka::Robot& robot,
- const 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());
- std::array<double, 9> load_inertia;
- std::copy(req.load_inertia.cbegin(), req.load_inertia.cend(), load_inertia.begin());
-
- robot.setLoad(mass, F_x_center_load, load_inertia);
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/src/trigger_rate.cpp b/franka_hw/src/trigger_rate.cpp
deleted file mode 100644
index 4b5458f..0000000
--- a/franka_hw/src/trigger_rate.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <franka_hw/trigger_rate.h>
-
-namespace franka_hw {
-
-TriggerRate::TriggerRate(double rate) : period_(1.0 / rate), time_stamp_(ros::Time::now()) {}
-
-bool TriggerRate::operator()() {
- if ((ros::Time::now() - time_stamp_).toSec() > period_) {
- time_stamp_ = ros::Time::now();
- return true;
- }
- return false;
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/test/CMakeLists.txt b/franka_hw/test/CMakeLists.txt
deleted file mode 100644
index 3601d19..0000000
--- a/franka_hw/test/CMakeLists.txt
+++ /dev/null
@@ -1,24 +0,0 @@
-find_package(rostest REQUIRED)
-
-add_rostest_gtest(franka_hw_test
- launch/franka_hw_test.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
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_hw_test
- ${catkin_LIBRARIES}
- Franka::Franka
- franka_hw
-)
-
-target_include_directories(franka_hw_test PUBLIC
- ${catkin_INCLUDE_DIRS}
-)
diff --git a/franka_hw/test/config/ros_console_settings_for_tests.conf b/franka_hw/test/config/ros_console_settings_for_tests.conf
deleted file mode 100644
index 82c3553..0000000
--- a/franka_hw/test/config/ros_console_settings_for_tests.conf
+++ /dev/null
@@ -1,4 +0,0 @@
-# Set the default ros output to warning and higher
-log4j.logger.ros=WARN
-# Override my package to output everything
-log4j.logger.ros.franka_hw=FATAL
diff --git a/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp b/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp
deleted file mode 100644
index 0f2837f..0000000
--- a/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp
+++ /dev/null
@@ -1,198 +0,0 @@
-// Copyright (c) 2019 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <array>
-#include <list>
-#include <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
deleted file mode 100644
index 59646b7..0000000
--- a/franka_hw/test/franka_hw_controller_switching_test.cpp
+++ /dev/null
@@ -1,196 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <array>
-#include <list>
-#include <random>
-#include <set>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#include <hardware_interface/controller_info.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <joint_limits_interface/joint_limits.h>
-#include <joint_limits_interface/joint_limits_urdf.h>
-#include <ros/ros.h>
-#include <urdf/model.h>
-
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_hw.h>
-
-using std::string;
-using std::set;
-using hardware_interface::InterfaceResources;
-using hardware_interface::ControllerInfo;
-
-std::string arm_id("panda");
-std::array<std::string, 7> joint_names = {
- arm_id + "_joint1", arm_id + "_joint2", arm_id + "_joint3", arm_id + "_joint4",
- arm_id + "_joint5", arm_id + "_joint6", arm_id + "_joint7"};
-
-namespace franka_hw {
-
-hardware_interface::ControllerInfo newInfo(
- const std::string& name,
- const std::string& type,
- const hardware_interface::InterfaceResources& resource1) {
- hardware_interface::ControllerInfo info;
- info.claimed_resources.clear();
- info.name = name;
- info.type = type;
- info.claimed_resources.push_back(resource1);
- return info;
-}
-
-hardware_interface::ControllerInfo newInfo(
- const std::string& name,
- const std::string& type,
- const hardware_interface::InterfaceResources& resource1,
- const hardware_interface::InterfaceResources& resource2) {
- hardware_interface::ControllerInfo info = newInfo(name, type, resource1);
- info.claimed_resources.push_back(resource2);
- return info;
-}
-
-hardware_interface::ControllerInfo newInfo(
- const std::string& name,
- const std::string& type,
- const hardware_interface::InterfaceResources& resource1,
- const hardware_interface::InterfaceResources& resource2,
- const hardware_interface::InterfaceResources& resource3) {
- hardware_interface::ControllerInfo info = newInfo(name, type, resource1, resource2);
- info.claimed_resources.push_back(resource3);
- return info;
-}
-
-class ControllerConflict
- : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
- public:
- ControllerConflict() : robot_(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_;
-};
-
-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_;
-};
-
-string arm_id2("panda2");
-string jp_iface_str("hardware_interface::PositionJointInterface");
-string jv_iface_str("hardware_interface::VelocityJointInterface");
-string jt_iface_str("hardware_interface::EffortJointInterface");
-string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface");
-string cp_iface_str("franka_hw::FrankaPoseCartesianInterface");
-string unknown_iface_str("hardware_interface::UnknownInterface");
-string name_str("some_controller");
-string type_str("SomeControllerClass");
-set<string> joints_set = {joint_names[0], joint_names[1], joint_names[2], joint_names[3],
- joint_names[4], joint_names[5], joint_names[6]};
-set<string> cartesian_set = {arm_id + "_robot"};
-set<string> cartesian_arm2_set = {arm_id2 + "_robot"};
-set<string> no_id_set = {"joint1"};
-InterfaceResources no_id_res(jp_iface_str, no_id_set);
-InterfaceResources unknown_iface_res(unknown_iface_str, joints_set);
-InterfaceResources jp_res(jp_iface_str, joints_set);
-InterfaceResources jv_res(jv_iface_str, joints_set);
-InterfaceResources jt_res(jt_iface_str, joints_set);
-InterfaceResources cv_res(cv_iface_str, cartesian_set);
-InterfaceResources cp_res(cp_iface_str, cartesian_set);
-InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set);
-ControllerInfo cp_arm2_info = newInfo(name_str, type_str, cp_arm2_res);
-ControllerInfo no_id_info = newInfo(name_str, type_str, no_id_res);
-ControllerInfo unknown_iface_info = newInfo(name_str, type_str, unknown_iface_res);
-ControllerInfo jt_jt_info = newInfo(name_str, type_str, jt_res, jt_res);
-ControllerInfo jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res);
-ControllerInfo jp_jv_info = newInfo(name_str, type_str, jp_res, jv_res);
-ControllerInfo jp_cv_info = newInfo(name_str, type_str, jp_res, cv_res);
-ControllerInfo jp_cp_info = newInfo(name_str, type_str, jp_res, cp_res);
-ControllerInfo jv_jv_info = newInfo(name_str, type_str, jv_res, jv_res);
-ControllerInfo jv_cv_info = newInfo(name_str, type_str, jv_res, cv_res);
-ControllerInfo jv_cp_info = newInfo(name_str, type_str, jv_res, cp_res);
-ControllerInfo cv_cv_info = newInfo(name_str, type_str, cv_res, cv_res);
-ControllerInfo cv_cp_info = newInfo(name_str, type_str, cv_res, cp_res);
-ControllerInfo cp_cp_info = newInfo(name_str, type_str, cp_res, cp_res);
-ControllerInfo jp_jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res, jp_res);
-ControllerInfo jp_info = newInfo(name_str, type_str, jp_res);
-ControllerInfo jv_info = newInfo(name_str, type_str, jv_res);
-ControllerInfo jt_info = newInfo(name_str, type_str, jt_res);
-ControllerInfo cv_info = newInfo(name_str, type_str, cv_res);
-ControllerInfo cp_info = newInfo(name_str, type_str, cp_res);
-ControllerInfo jt_jp_info = newInfo(name_str, type_str, jt_res, jp_res);
-ControllerInfo jt_jv_info = newInfo(name_str, type_str, jt_res, jv_res);
-ControllerInfo jt_cv_info = newInfo(name_str, type_str, jt_res, cv_res);
-ControllerInfo jt_cp_info = newInfo(name_str, type_str, jt_res, cp_res);
-
-INSTANTIATE_TEST_CASE_P(nonAdmissibleRequests,
- ControllerConflict,
- ::testing::Values(std::list<ControllerInfo>{no_id_info},
- std::list<ControllerInfo>{unknown_iface_info},
- std::list<ControllerInfo>{jt_jt_info},
- std::list<ControllerInfo>{jp_jp_info},
- std::list<ControllerInfo>{jp_jv_info},
- std::list<ControllerInfo>{jp_cv_info},
- std::list<ControllerInfo>{jp_cp_info},
- std::list<ControllerInfo>{jv_jv_info},
- std::list<ControllerInfo>{jv_cv_info},
- std::list<ControllerInfo>{jv_cp_info},
- std::list<ControllerInfo>{cv_cv_info},
- std::list<ControllerInfo>{cv_cp_info},
- std::list<ControllerInfo>{cp_cp_info},
- std::list<ControllerInfo>{jp_jp_jp_info}));
-
-INSTANTIATE_TEST_CASE_P(admissibleRequests,
- NoControllerConflict,
- ::testing::Values(std::list<ControllerInfo>{jp_info},
- std::list<ControllerInfo>{jv_info},
- std::list<ControllerInfo>{jt_info},
- std::list<ControllerInfo>{cv_info},
- std::list<ControllerInfo>{cp_info},
- std::list<ControllerInfo>{jt_jp_info},
- std::list<ControllerInfo>{jt_jv_info},
- std::list<ControllerInfo>{jt_cv_info},
- std::list<ControllerInfo>{jt_cp_info},
- std::list<ControllerInfo>{cp_info, cp_arm2_info}));
-
-TEST_P(ControllerConflict, ConflictsForIncompatibleControllers) {
- EXPECT_TRUE(callCheckForConflict(GetParam()));
-}
-
-TEST_P(NoControllerConflict, DoesNotConflictForCompatibleControllers) {
- EXPECT_FALSE(callCheckForConflict(GetParam()));
-}
-
-TEST_P(NoControllerConflict, CanPrepareSwitchForCompatibleControllers) {
- EXPECT_TRUE(callPrepareSwitch(GetParam()));
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/test/franka_hw_interfaces_test.cpp b/franka_hw/test/franka_hw_interfaces_test.cpp
deleted file mode 100644
index 7087632..0000000
--- a/franka_hw/test/franka_hw_interfaces_test.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <array>
-#include <random>
-#include <string>
-#include <vector>
-
-#include <gtest/gtest.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_hw.h>
-#include <franka_hw/franka_model_interface.h>
-
-extern std::string arm_id;
-extern std::array<std::string, 7> joint_names;
-
-namespace franka_hw {
-
-TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) {
- 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 =
- robot_ptr->get<hardware_interface::JointStateInterface>();
- hardware_interface::PositionJointInterface* pj_interface =
- robot_ptr->get<hardware_interface::PositionJointInterface>();
- hardware_interface::VelocityJointInterface* vj_interface =
- robot_ptr->get<hardware_interface::VelocityJointInterface>();
- hardware_interface::EffortJointInterface* ej_interface =
- robot_ptr->get<hardware_interface::EffortJointInterface>();
- FrankaPoseCartesianInterface* fpc_interface = robot_ptr->get<FrankaPoseCartesianInterface>();
- FrankaVelocityCartesianInterface* fvc_interface =
- robot_ptr->get<FrankaVelocityCartesianInterface>();
- FrankaStateInterface* fs_interface = robot_ptr->get<FrankaStateInterface>();
-
- ASSERT_NE(nullptr, js_interface);
- ASSERT_NE(nullptr, pj_interface);
- ASSERT_NE(nullptr, vj_interface);
- ASSERT_NE(nullptr, ej_interface);
- ASSERT_NE(nullptr, fpc_interface);
- ASSERT_NE(nullptr, fvc_interface);
- ASSERT_NE(nullptr, fs_interface);
-
- // Model interface not available with this signature
- 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_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");
- 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());
-
- EXPECT_NO_THROW(fs_interface->getHandle(arm_id + "_robot"));
-}
-
-TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
- 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>();
- hardware_interface::VelocityJointInterface* vj_interface =
- robot_ptr->get<hardware_interface::VelocityJointInterface>();
- hardware_interface::EffortJointInterface* ej_interface =
- robot_ptr->get<hardware_interface::EffortJointInterface>();
- ASSERT_NE(nullptr, pj_interface);
- ASSERT_NE(nullptr, vj_interface);
- ASSERT_NE(nullptr, ej_interface);
-
- std::vector<joint_limits_interface::JointLimits> joint_limits(7);
- std::vector<hardware_interface::JointHandle> position_handles(7);
- 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]));
- }
-
- std::uniform_real_distribution<double> uniform_distribution(0.1, 3.0);
- std::default_random_engine random_engine;
-
- for (size_t i = 0; i < joint_names.size(); ++i) {
- position_handles[i].setCommand(joint_limits[i].max_position +
- 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));
- }
- 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);
- }
-
- for (size_t i = 0; i < joint_names.size(); ++i) {
- position_handles[i].setCommand(joint_limits[i].min_position -
- 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));
- }
- 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);
- }
-}
-
-} // namespace franka_hw
diff --git a/franka_hw/test/launch/franka_hw_test.test b/franka_hw/test/launch/franka_hw_test.test
deleted file mode 100644
index 64ffded..0000000
--- a/franka_hw/test/launch/franka_hw_test.test
+++ /dev/null
@@ -1,8 +0,0 @@
-<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" >
- <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_hw/test/main.cpp b/franka_hw/test/main.cpp
deleted file mode 100644
index 11c2f74..0000000
--- a/franka_hw/test/main.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include <gtest/gtest.h>
-#include <ros/ros.h>
-
-int main(int argc, char** argv) {
- ::testing::InitGoogleTest(&argc, argv);
- ros::init(argc, argv, "franka_hw_test_node");
- return RUN_ALL_TESTS();
-}
diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt
deleted file mode 100644
index 1f6f926..0000000
--- a/franka_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,22 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_msgs)
-
-find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_msgs)
-
-add_message_files(FILES Errors.msg FrankaState.msg)
-
-add_service_files(FILES
- SetCartesianImpedance.srv
- SetEEFrame.srv
- SetForceTorqueCollisionBehavior.srv
- SetFullCollisionBehavior.srv
- SetJointImpedance.srv
- SetKFrame.srv
- SetLoad.srv
-)
-
-add_action_files(FILES ErrorRecovery.action)
-
-generate_messages(DEPENDENCIES std_msgs actionlib_msgs)
-
-catkin_package(CATKIN_DEPENDS message_runtime std_msgs actionlib_msgs)
diff --git a/franka_msgs/action/ErrorRecovery.action b/franka_msgs/action/ErrorRecovery.action
deleted file mode 100644
index a845151..0000000
--- a/franka_msgs/action/ErrorRecovery.action
+++ /dev/null
@@ -1,2 +0,0 @@
----
----
diff --git a/franka_msgs/mainpage.dox b/franka_msgs/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_msgs/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_msgs/msg/Errors.msg b/franka_msgs/msg/Errors.msg
deleted file mode 100644
index a50780f..0000000
--- a/franka_msgs/msg/Errors.msg
+++ /dev/null
@@ -1,36 +0,0 @@
-bool joint_position_limits_violation
-bool cartesian_position_limits_violation
-bool self_collision_avoidance_violation
-bool joint_velocity_violation
-bool cartesian_velocity_violation
-bool force_control_safety_violation
-bool joint_reflex
-bool cartesian_reflex
-bool max_goal_pose_deviation_violation
-bool max_path_pose_deviation_violation
-bool cartesian_velocity_profile_safety_violation
-bool joint_position_motion_generator_start_pose_invalid
-bool joint_motion_generator_position_limits_violation
-bool joint_motion_generator_velocity_limits_violation
-bool joint_motion_generator_velocity_discontinuity
-bool joint_motion_generator_acceleration_discontinuity
-bool cartesian_position_motion_generator_start_pose_invalid
-bool cartesian_motion_generator_elbow_limit_violation
-bool cartesian_motion_generator_velocity_limits_violation
-bool cartesian_motion_generator_velocity_discontinuity
-bool cartesian_motion_generator_acceleration_discontinuity
-bool cartesian_motion_generator_elbow_sign_inconsistent
-bool cartesian_motion_generator_start_elbow_invalid
-bool cartesian_motion_generator_joint_position_limits_violation
-bool cartesian_motion_generator_joint_velocity_limits_violation
-bool cartesian_motion_generator_joint_velocity_discontinuity
-bool cartesian_motion_generator_joint_acceleration_discontinuity
-bool cartesian_position_motion_generator_invalid_frame
-bool force_controller_desired_force_tolerance_violation
-bool controller_torque_discontinuity
-bool start_elbow_sign_inconsistent
-bool communication_constraints_violation
-bool power_limit_violation
-bool joint_p2p_insufficient_torque_for_planning
-bool tau_j_range_violation
-bool instability_detected
diff --git a/franka_msgs/msg/FrankaState.msg b/franka_msgs/msg/FrankaState.msg
deleted file mode 100644
index b7eea2c..0000000
--- a/franka_msgs/msg/FrankaState.msg
+++ /dev/null
@@ -1,52 +0,0 @@
-std_msgs/Header header
-float64[6] cartesian_collision
-float64[6] cartesian_contact
-float64[7] q
-float64[7] q_d
-float64[7] dq
-float64[7] dq_d
-float64[7] ddq_d
-float64[7] theta
-float64[7] dtheta
-float64[7] tau_J
-float64[7] dtau_J
-float64[7] tau_J_d
-float64[6] K_F_ext_hat_K
-float64[2] elbow
-float64[2] elbow_d
-float64[2] elbow_c
-float64[2] delbow_c
-float64[2] ddelbow_c
-float64[7] joint_collision
-float64[7] joint_contact
-float64[6] O_F_ext_hat_K
-float64[6] O_dP_EE_d
-float64[6] O_dP_EE_c
-float64[6] O_ddP_EE_c
-float64[7] tau_ext_hat_filtered
-float64 m_ee
-float64[3] F_x_Cee
-float64[9] I_ee
-float64 m_load
-float64[3] F_x_Cload
-float64[9] I_load
-float64 m_total
-float64[3] F_x_Ctotal
-float64[9] I_total
-float64[16] O_T_EE
-float64[16] O_T_EE_d
-float64[16] O_T_EE_c
-float64[16] F_T_EE
-float64[16] EE_T_K
-float64 time
-float64 control_command_success_rate
-uint8 ROBOT_MODE_OTHER=0
-uint8 ROBOT_MODE_IDLE=1
-uint8 ROBOT_MODE_MOVE=2
-uint8 ROBOT_MODE_GUIDING=3
-uint8 ROBOT_MODE_REFLEX=4
-uint8 ROBOT_MODE_USER_STOPPED=5
-uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6
-uint8 robot_mode
-franka_msgs/Errors current_errors
-franka_msgs/Errors last_motion_errors
diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml
deleted file mode 100644
index c8bcef0..0000000
--- a/franka_msgs/package.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_msgs</name>
- <version>0.7.0</version>
- <description>franka_msgs provides messages specific to Franka Emika research robots</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_msgs</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <build_depend>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>
- <architecture_independent/>
- </export>
-</package>
diff --git a/franka_msgs/rosdoc.yaml b/franka_msgs/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_msgs/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_msgs/srv/SetCartesianImpedance.srv b/franka_msgs/srv/SetCartesianImpedance.srv
deleted file mode 100644
index 3a763ba..0000000
--- a/franka_msgs/srv/SetCartesianImpedance.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[6] cartesian_stiffness
----
-bool success
-string error
-
diff --git a/franka_msgs/srv/SetEEFrame.srv b/franka_msgs/srv/SetEEFrame.srv
deleted file mode 100644
index bb7d0fd..0000000
--- a/franka_msgs/srv/SetEEFrame.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[16] F_T_EE
----
-bool success
-string error
-
diff --git a/franka_msgs/srv/SetForceTorqueCollisionBehavior.srv b/franka_msgs/srv/SetForceTorqueCollisionBehavior.srv
deleted file mode 100644
index f6aeaba..0000000
--- a/franka_msgs/srv/SetForceTorqueCollisionBehavior.srv
+++ /dev/null
@@ -1,8 +0,0 @@
-float64[7] lower_torque_thresholds_nominal
-float64[7] upper_torque_thresholds_nominal
-float64[6] lower_force_thresholds_nominal
-float64[6] upper_force_thresholds_nominal
----
-bool success
-string error
-
diff --git a/franka_msgs/srv/SetFullCollisionBehavior.srv b/franka_msgs/srv/SetFullCollisionBehavior.srv
deleted file mode 100644
index c4d2b2d..0000000
--- a/franka_msgs/srv/SetFullCollisionBehavior.srv
+++ /dev/null
@@ -1,12 +0,0 @@
-float64[7] lower_torque_thresholds_acceleration
-float64[7] upper_torque_thresholds_acceleration
-float64[7] lower_torque_thresholds_nominal
-float64[7] upper_torque_thresholds_nominal
-float64[6] lower_force_thresholds_acceleration
-float64[6] upper_force_thresholds_acceleration
-float64[6] lower_force_thresholds_nominal
-float64[6] upper_force_thresholds_nominal
----
-bool success
-string error
-
diff --git a/franka_msgs/srv/SetJointImpedance.srv b/franka_msgs/srv/SetJointImpedance.srv
deleted file mode 100644
index c966398..0000000
--- a/franka_msgs/srv/SetJointImpedance.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[7] joint_stiffness
----
-bool success
-string error
-
diff --git a/franka_msgs/srv/SetKFrame.srv b/franka_msgs/srv/SetKFrame.srv
deleted file mode 100644
index 774c373..0000000
--- a/franka_msgs/srv/SetKFrame.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64[16] EE_T_K
----
-bool success
-string error
-
diff --git a/franka_msgs/srv/SetLoad.srv b/franka_msgs/srv/SetLoad.srv
deleted file mode 100644
index b24211a..0000000
--- a/franka_msgs/srv/SetLoad.srv
+++ /dev/null
@@ -1,7 +0,0 @@
-float64 mass
-float64[3] F_x_center_load
-float64[9] load_inertia
----
-bool success
-string error
-
diff --git a/franka_ros/CMakeLists.txt b/franka_ros/CMakeLists.txt
deleted file mode 100644
index 11908b9..0000000
--- a/franka_ros/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(franka_ros)
-find_package(catkin REQUIRED)
-catkin_metapackage()
diff --git a/franka_ros/mainpage.dox b/franka_ros/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_ros/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_ros/package.xml b/franka_ros/package.xml
deleted file mode 100644
index fa817f1..0000000
--- a/franka_ros/package.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_ros</name>
- <version>0.7.0</version>
- <description>franka_ros is a metapackage for all Franka Emika ROS packages</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_ros</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <exec_depend>franka_control</exec_depend>
- <exec_depend>franka_description</exec_depend>
- <exec_depend>franka_example_controllers</exec_depend>
- <exec_depend>franka_gripper</exec_depend>
- <exec_depend>franka_hw</exec_depend>
- <exec_depend>franka_msgs</exec_depend>
- <exec_depend>franka_visualization</exec_depend>
- <exec_depend>panda_moveit_config</exec_depend>
-
- <export>
- <metapackage />
- </export>
-</package>
diff --git a/franka_ros/rosdoc.yaml b/franka_ros/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_ros/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_visualization/CMakeLists.txt b/franka_visualization/CMakeLists.txt
deleted file mode 100644
index 3354d32..0000000
--- a/franka_visualization/CMakeLists.txt
+++ /dev/null
@@ -1,64 +0,0 @@
-cmake_minimum_required(VERSION 3.4)
-project(franka_visualization)
-
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-find_package(catkin REQUIRED COMPONENTS
- sensor_msgs
- roscpp
-)
-
-find_package(Franka 0.7.0 REQUIRED)
-
-catkin_package(CATKIN_DEPENDS sensor_msgs roscpp)
-
-set(EXECUTABLES robot_joint_state_publisher gripper_joint_state_publisher)
-
-foreach(executable ${EXECUTABLES})
- add_executable(${executable}
- src/${executable}.cpp
- )
- add_dependencies(${executable}
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
- )
- target_include_directories(${executable} SYSTEM PUBLIC
- ${Franka_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
- )
- target_link_libraries(${executable} PUBLIC
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
- )
-endforeach()
-
-## Installation
-install(TARGETS ${EXECUTABLES}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(DIRECTORY launch
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY config
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-## Tools
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
- RESULT_VARIABLE CLANG_TOOLS
-)
-if(CLANG_TOOLS)
- file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
- file(GLOB_RECURSE HEADERS
- ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
- ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
- )
- add_format_target(franka_visualization FILES ${SOURCES} ${HEADERS})
- add_tidy_target(franka_visualization
- FILES ${SOURCES}
- DEPENDS ${EXECUTABLES}
- )
-endif()
diff --git a/franka_visualization/config/gripper_settings.yaml b/franka_visualization/config/gripper_settings.yaml
deleted file mode 100644
index 0a286b2..0000000
--- a/franka_visualization/config/gripper_settings.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-joint_names:
- - panda_finger_joint1
- - panda_finger_joint2
diff --git a/franka_visualization/config/robot_settings.yaml b/franka_visualization/config/robot_settings.yaml
deleted file mode 100644
index 7bd6f1a..0000000
--- a/franka_visualization/config/robot_settings.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-joint_names:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
diff --git a/franka_visualization/launch/franka_visualization.launch b/franka_visualization/launch/franka_visualization.launch
deleted file mode 100644
index 61ce3e7..0000000
--- a/franka_visualization/launch/franka_visualization.launch
+++ /dev/null
@@ -1,30 +0,0 @@
-<?xml version="1.0" ?>
-
-<launch>
- <arg name="load_gripper" default="true" />
- <arg name="robot_ip" />
- <arg name="publish_rate" default="30" />
-
- <param unless="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" />
- <param if="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
-
- <node name="robot_joint_state_publisher" pkg="franka_visualization" type="robot_joint_state_publisher" output="screen">
- <rosparam command="load" file="$(find franka_visualization)/config/robot_settings.yaml" />
- <param name="robot_ip" value="$(arg robot_ip)" />
- <param name="publish_rate" value="$(arg publish_rate)" />
- </node>
-
- <node name="gripper_joint_state_publisher" pkg="franka_visualization" type="gripper_joint_state_publisher" output="screen" if="$(arg load_gripper)">
- <rosparam command="load" file="$(find franka_visualization)/config/gripper_settings.yaml" />
- <param name="robot_ip" value="$(arg robot_ip)" />
- <param name="publish_rate" value="$(arg publish_rate)" />
- </node>
-
- <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
- <param name="rate" value="$(arg publish_rate)" />
- <rosparam param="source_list">[robot_joint_state_publisher/joint_states, gripper_joint_state_publisher/joint_states]</rosparam>
- </node>
-
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
- <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_visualization)/launch/franka_visualization.rviz"/>
-</launch>
diff --git a/franka_visualization/launch/franka_visualization.rviz b/franka_visualization/launch/franka_visualization.rviz
deleted file mode 100644
index 07c4f7f..0000000
--- a/franka_visualization/launch/franka_visualization.rviz
+++ /dev/null
@@ -1,187 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 89
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- Splitter Ratio: 0.5
- Tree Height: 746
- - 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: ""
-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
- - 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
- panda_hand:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_leftfinger:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link0:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link1:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link2:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link3:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link4:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link5:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link6:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link7:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- panda_link8:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- panda_rightfinger:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- Name: RobotModel
- Robot Description: robot_description
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: panda_link0
- 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: 3.39362764
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: -0.139062241
- Y: 0.151414081
- Z: 0.0437288694
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.0500000007
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.00999999978
- Pitch: 0.485397696
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 0.500398219
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 1059
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016a00000384fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000384000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000384fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000384000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000004f90000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1918
- X: 1920
- Y: 19
diff --git a/franka_visualization/mainpage.dox b/franka_visualization/mainpage.dox
deleted file mode 100644
index 941d0bf..0000000
--- a/franka_visualization/mainpage.dox
+++ /dev/null
@@ -1,6 +0,0 @@
-/**
- * @mainpage
- * @htmlinclude "manifest.html"
- *
- * Overview page for Franka Emika research robots: https://frankaemika.github.io
- */
diff --git a/franka_visualization/package.xml b/franka_visualization/package.xml
deleted file mode 100644
index 8679db2..0000000
--- a/franka_visualization/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>franka_visualization</name>
- <version>0.7.0</version>
- <description>This package contains visualization tools for Franka Emika.</description>
- <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
- <license>Apache 2.0</license>
-
- <url type="website">http://wiki.ros.org/franka_visualization</url>
- <url type="repository">https://github.com/frankaemika/franka_ros</url>
- <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
- <author>Franka Emika GmbH</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <depend>sensor_msgs</depend>
- <depend>roscpp</depend>
- <depend>libfranka</depend>
-
- <exec_depend>franka_description</exec_depend>
- <exec_depend>xacro</exec_depend>
-</package>
diff --git a/franka_visualization/rosdoc.yaml b/franka_visualization/rosdoc.yaml
deleted file mode 100644
index 96ee597..0000000
--- a/franka_visualization/rosdoc.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-- builder: doxygen
- javadoc_autobrief: YES
diff --git a/franka_visualization/src/gripper_joint_state_publisher.cpp b/franka_visualization/src/gripper_joint_state_publisher.cpp
deleted file mode 100644
index c689a22..0000000
--- a/franka_visualization/src/gripper_joint_state_publisher.cpp
+++ /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
-#include <franka/exception.h>
-#include <franka/gripper.h>
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-
-int main(int argc, char** argv) {
- ros::init(argc, argv, "gripper_joint_state_publisher");
- ros::NodeHandle node_handle("~");
-
- std::vector<std::string> joint_names;
- node_handle.getParam("joint_names", joint_names);
-
- std::string robot_ip;
- node_handle.getParam("robot_ip", robot_ip);
-
- double publish_rate;
- node_handle.getParam("publish_rate", publish_rate);
-
- ros::Rate rate(publish_rate);
-
- sensor_msgs::JointState states;
- states.effort.resize(joint_names.size());
- states.name.resize(joint_names.size());
- states.position.resize(joint_names.size());
- states.velocity.resize(joint_names.size());
-
- for (size_t i = 0; i < joint_names.size(); i++) {
- states.name[i] = joint_names[i];
- }
-
- ros::Publisher publisher = node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
-
- try {
- franka::Gripper gripper(robot_ip);
-
- while (ros::ok()) {
- franka::GripperState gripper_state = gripper.readOnce();
- states.header.stamp = ros::Time::now();
- for (size_t i = 0; i < joint_names.size(); i++) {
- states.position[i] = gripper_state.width * 0.5;
- states.velocity[i] = 0.0;
- states.effort[i] = 0.0;
- }
- publisher.publish(states);
- ros::spinOnce();
- rate.sleep();
- }
-
- } catch (const franka::Exception& e) {
- ROS_ERROR_STREAM("Exception: " << e.what());
- return -1;
- }
-
- return 0;
-}
diff --git a/franka_visualization/src/robot_joint_state_publisher.cpp b/franka_visualization/src/robot_joint_state_publisher.cpp
deleted file mode 100644
index 61fed6c..0000000
--- a/franka_visualization/src/robot_joint_state_publisher.cpp
+++ /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
-#include <franka/exception.h>
-#include <franka/robot.h>
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-
-int main(int argc, char** argv) {
- ros::init(argc, argv, "robot_joint_state_publisher");
- ros::NodeHandle node_handle("~");
-
- std::vector<std::string> joint_names;
- node_handle.getParam("joint_names", joint_names);
-
- std::string robot_ip;
- node_handle.getParam("robot_ip", robot_ip);
-
- double publish_rate;
- node_handle.getParam("publish_rate", publish_rate);
-
- ros::Rate rate(publish_rate);
-
- sensor_msgs::JointState states;
- states.effort.resize(joint_names.size());
- states.name.resize(joint_names.size());
- states.position.resize(joint_names.size());
- states.velocity.resize(joint_names.size());
-
- for (size_t i = 0; i < joint_names.size(); i++) {
- states.name[i] = joint_names[i];
- }
-
- ros::Publisher publisher = node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
-
- try {
- franka::Robot robot(robot_ip);
-
- robot.read([&](const franka::RobotState& robot_state) {
- states.header.stamp = ros::Time::now();
- for (size_t i = 0; i < joint_names.size(); i++) {
- states.position[i] = robot_state.q[i];
- states.velocity[i] = robot_state.dq[i];
- states.effort[i] = robot_state.tau_J[i];
- }
- publisher.publish(states);
- ros::spinOnce();
- rate.sleep();
- return ros::ok();
- });
-
- } catch (const franka::Exception& e) {
- ROS_ERROR_STREAM("Exception: " << e.what());
- return -1;
- }
-
- return 0;
-}
diff --git a/franka_control/mainpage.dox b/mainpage.dox
similarity index 100%
rename from franka_control/mainpage.dox
rename to mainpage.dox
diff --git a/franka_description/meshes/collision/finger.stl b/meshes/collision/finger.stl
similarity index 100%
rename from franka_description/meshes/collision/finger.stl
rename to meshes/collision/finger.stl
diff --git a/franka_description/meshes/collision/hand.stl b/meshes/collision/hand.stl
similarity index 100%
rename from franka_description/meshes/collision/hand.stl
rename to meshes/collision/hand.stl
diff --git a/franka_description/meshes/collision/link0.stl b/meshes/collision/link0.stl
similarity index 100%
rename from franka_description/meshes/collision/link0.stl
rename to meshes/collision/link0.stl
diff --git a/franka_description/meshes/collision/link1.stl b/meshes/collision/link1.stl
similarity index 100%
rename from franka_description/meshes/collision/link1.stl
rename to meshes/collision/link1.stl
diff --git a/franka_description/meshes/collision/link2.stl b/meshes/collision/link2.stl
similarity index 100%
rename from franka_description/meshes/collision/link2.stl
rename to meshes/collision/link2.stl
diff --git a/franka_description/meshes/collision/link3.stl b/meshes/collision/link3.stl
similarity index 100%
rename from franka_description/meshes/collision/link3.stl
rename to meshes/collision/link3.stl
diff --git a/franka_description/meshes/collision/link4.stl b/meshes/collision/link4.stl
similarity index 100%
rename from franka_description/meshes/collision/link4.stl
rename to meshes/collision/link4.stl
diff --git a/franka_description/meshes/collision/link5.stl b/meshes/collision/link5.stl
similarity index 100%
rename from franka_description/meshes/collision/link5.stl
rename to meshes/collision/link5.stl
diff --git a/franka_description/meshes/collision/link6.stl b/meshes/collision/link6.stl
similarity index 100%
rename from franka_description/meshes/collision/link6.stl
rename to meshes/collision/link6.stl
diff --git a/franka_description/meshes/collision/link7.stl b/meshes/collision/link7.stl
similarity index 100%
rename from franka_description/meshes/collision/link7.stl
rename to meshes/collision/link7.stl
diff --git a/franka_description/meshes/visual/finger.dae b/meshes/visual/finger.dae
similarity index 100%
rename from franka_description/meshes/visual/finger.dae
rename to meshes/visual/finger.dae
diff --git a/franka_description/meshes/visual/hand.dae b/meshes/visual/hand.dae
similarity index 100%
rename from franka_description/meshes/visual/hand.dae
rename to meshes/visual/hand.dae
diff --git a/franka_description/meshes/visual/link0.dae b/meshes/visual/link0.dae
similarity index 100%
rename from franka_description/meshes/visual/link0.dae
rename to meshes/visual/link0.dae
diff --git a/franka_description/meshes/visual/link1.dae b/meshes/visual/link1.dae
similarity index 100%
rename from franka_description/meshes/visual/link1.dae
rename to meshes/visual/link1.dae
diff --git a/franka_description/meshes/visual/link2.dae b/meshes/visual/link2.dae
similarity index 100%
rename from franka_description/meshes/visual/link2.dae
rename to meshes/visual/link2.dae
diff --git a/franka_description/meshes/visual/link3.dae b/meshes/visual/link3.dae
similarity index 100%
rename from franka_description/meshes/visual/link3.dae
rename to meshes/visual/link3.dae
diff --git a/franka_description/meshes/visual/link4.dae b/meshes/visual/link4.dae
similarity index 100%
rename from franka_description/meshes/visual/link4.dae
rename to meshes/visual/link4.dae
diff --git a/franka_description/meshes/visual/link5.dae b/meshes/visual/link5.dae
similarity index 100%
rename from franka_description/meshes/visual/link5.dae
rename to meshes/visual/link5.dae
diff --git a/franka_description/meshes/visual/link6.dae b/meshes/visual/link6.dae
similarity index 100%
rename from franka_description/meshes/visual/link6.dae
rename to meshes/visual/link6.dae
diff --git a/franka_description/meshes/visual/link7.dae b/meshes/visual/link7.dae
similarity index 100%
rename from franka_description/meshes/visual/link7.dae
rename to meshes/visual/link7.dae
diff --git a/franka_description/package.xml b/package.xml
similarity index 100%
rename from franka_description/package.xml
rename to package.xml
diff --git a/franka_description/robots/dual_panda_example.urdf.xacro b/robots/dual_panda_example.urdf.xacro
similarity index 100%
rename from franka_description/robots/dual_panda_example.urdf.xacro
rename to robots/dual_panda_example.urdf.xacro
diff --git a/franka_description/robots/hand.urdf.xacro b/robots/hand.urdf.xacro
similarity index 100%
rename from franka_description/robots/hand.urdf.xacro
rename to robots/hand.urdf.xacro
diff --git a/franka_description/robots/hand.xacro b/robots/hand.xacro
similarity index 100%
rename from franka_description/robots/hand.xacro
rename to robots/hand.xacro
diff --git a/franka_description/robots/panda.gazebo.xacro b/robots/panda.gazebo.xacro
similarity index 100%
rename from franka_description/robots/panda.gazebo.xacro
rename to robots/panda.gazebo.xacro
diff --git a/franka_description/robots/panda.transmission.xacro b/robots/panda.transmission.xacro
similarity index 100%
rename from franka_description/robots/panda.transmission.xacro
rename to robots/panda.transmission.xacro
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/robots/panda_arm.urdf.xacro
similarity index 100%
rename from franka_description/robots/panda_arm.urdf.xacro
rename to robots/panda_arm.urdf.xacro
diff --git a/franka_description/robots/panda_arm.xacro b/robots/panda_arm.xacro
similarity index 100%
rename from franka_description/robots/panda_arm.xacro
rename to robots/panda_arm.xacro
diff --git a/franka_description/robots/panda_arm_hand.urdf.xacro b/robots/panda_arm_hand.urdf.xacro
similarity index 100%
rename from franka_description/robots/panda_arm_hand.urdf.xacro
rename to robots/panda_arm_hand.urdf.xacro
diff --git a/franka_control/rosdoc.yaml b/rosdoc.yaml
similarity index 100%
rename from franka_control/rosdoc.yaml
rename to rosdoc.yaml
--
GitLab