├── .gitignore ├── forcedimension_ros2.repos ├── fd_hardware ├── fd_hardware_plugin.xml ├── package.xml ├── CMakeLists.txt ├── include │ └── fd_hardware │ │ ├── visibility_control.h │ │ └── fd_effort_hi.hpp └── src │ └── fd_effort_hi.cpp ├── fd_controllers ├── ee_pose_broadcaster │ ├── ee_pose_plugin.xml │ ├── package.xml │ ├── CMakeLists.txt │ ├── include │ │ └── ee_pose_broadcaster │ │ │ ├── visibility_control.h │ │ │ └── ee_pose_broadcaster.hpp │ └── src │ │ └── ee_pose_broadcaster.cpp ├── fd_inertia_broadcaster │ ├── fd_inertia_broadcaster_plugin.xml │ ├── package.xml │ ├── CMakeLists.txt │ ├── include │ │ └── fd_inertia_broadcaster │ │ │ ├── visibility_control.h │ │ │ └── fd_inertia_broadcaster.hpp │ └── src │ │ └── fd_inertia_broadcaster.cpp └── fd_clutch_broadcaster │ ├── fd_clutch_broadcaster_plugin.xml │ ├── package.xml │ ├── CMakeLists.txt │ ├── include │ └── fd_clutch_broadcaster │ │ ├── visibility_control.h │ │ └── fd_clutch_broadcaster.hpp │ └── src │ └── fd_clutch_broadcaster.cpp ├── .github ├── workflows │ ├── ci.yml │ ├── ci-jazzy.yml │ └── ci-format.yml └── mergify.yml ├── fd_description ├── package.xml ├── CMakeLists.txt ├── config │ ├── fd.config.xacro │ └── fd_controllers.yaml ├── ros2_control │ └── fd.r2c_hardware.xacro └── urdf │ └── fd.urdf.xacro ├── fd_bringup ├── CMakeLists.txt ├── package.xml └── launch │ └── fd.launch.py ├── CONTRIBUTING.md ├── .pre-commit-config.yaml ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | install/ 4 | log/ 5 | fd_hardware/external/ 6 | -------------------------------------------------------------------------------- /forcedimension_ros2.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | fd_sdk_vendor: 3 | type: git 4 | url: https://github.com/ICube-Robotics/fd_sdk_vendor.git 5 | version: main 6 | -------------------------------------------------------------------------------- /fd_hardware/fd_hardware_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ROS2 Control hardware interface for Force Dimension Haptic Interfaces. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /fd_controllers/ee_pose_broadcaster/ee_pose_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The ee pose broadcaster publishes the current end-effector pose from ros2_control fd system. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /fd_controllers/fd_inertia_broadcaster/fd_inertia_broadcaster_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The fd inertia broadcaster publishes the current cartesian inertia from ros2_control fd system. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /fd_controllers/fd_clutch_broadcaster/fd_clutch_broadcaster_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | The fd inertia broadcaster publishes the current cartesian inertia from ros2_control fd system. 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI (rolling) 2 | on: 3 | push: 4 | branches: [ main ] 5 | pull_request: 6 | branches: [ main ] 7 | workflow_dispatch: 8 | schedule: 9 | - cron: '0 0 * * 0' 10 | jobs: 11 | build: 12 | runs-on: ubuntu-latest 13 | container: 14 | image: rostooling/setup-ros-docker:ubuntu-noble-latest 15 | steps: 16 | - uses: actions/checkout@v4 17 | - uses: ros-tooling/action-ros-ci@0.4.3 18 | with: 19 | target-ros2-distro: rolling 20 | vcs-repo-file-url: ./forcedimension_ros2.repos 21 | -------------------------------------------------------------------------------- /.github/workflows/ci-jazzy.yml: -------------------------------------------------------------------------------- 1 | name: CI (jazzy) 2 | on: 3 | push: 4 | branches: [ main ] 5 | pull_request: 6 | branches: [ main ] 7 | workflow_dispatch: 8 | schedule: 9 | - cron: '0 0 * * 0' 10 | jobs: 11 | build: 12 | runs-on: ubuntu-latest 13 | container: 14 | image: rostooling/setup-ros-docker:ubuntu-noble-latest 15 | steps: 16 | - uses: actions/checkout@v4 17 | - uses: ros-tooling/action-ros-ci@0.4.3 18 | with: 19 | target-ros2-distro: jazzy 20 | vcs-repo-file-url: ./forcedimension_ros2.repos 21 | -------------------------------------------------------------------------------- /.github/workflows/ci-format.yml: -------------------------------------------------------------------------------- 1 | # This is a format job. Pre-commit has a first-party GitHub action, so we use 2 | # that: https://github.com/pre-commit/action 3 | 4 | name: Format 5 | 6 | on: 7 | workflow_dispatch: 8 | pull_request: 9 | 10 | jobs: 11 | pre-commit: 12 | name: Format 13 | runs-on: ubuntu-latest 14 | steps: 15 | - uses: actions/checkout@v4 16 | - uses: actions/setup-python@v4.7.0 17 | with: 18 | python-version: '3.10' 19 | - name: Install system hooks 20 | run: sudo apt install -qq clang-format-14 cppcheck 21 | - uses: pre-commit/action@v3.0.0 22 | with: 23 | extra_args: --all-files --hook-stage manual 24 | -------------------------------------------------------------------------------- /fd_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fd_description 5 | 0.0.0 6 | Package with description files for a Force Dimension Haptic Interface 7 | 8 | m.bednarczyk 9 | 10 | Apache License 2.0 11 | 12 | ament_cmake 13 | 14 | ament_lint_auto 15 | ament_lint_common 16 | 17 | xacro 18 | urdf 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /fd_hardware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fd_hardware 5 | 0.0.0 6 | Hardware interface for Force Dimension Haptic Interfaces. 7 | 8 | m.bednarczyk 9 | 10 | Apache License 2.0 11 | 12 | ament_cmake 13 | 14 | hardware_interface 15 | pluginlib 16 | rclcpp 17 | rclcpp_lifecycle 18 | fd_sdk_vendor 19 | 20 | controller_manager 21 | xacro 22 | 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /fd_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fd_description) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | 21 | if(BUILD_TESTING) 22 | find_package(ament_lint_auto REQUIRED) 23 | ament_lint_auto_find_test_dependencies() 24 | endif() 25 | 26 | ################################################################################ 27 | # Install 28 | ################################################################################ 29 | install( 30 | DIRECTORY urdf ros2_control config 31 | DESTINATION share/${PROJECT_NAME} 32 | ) 33 | 34 | ament_package() 35 | -------------------------------------------------------------------------------- /fd_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fd_bringup) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclpy REQUIRED) 22 | find_package(sensor_msgs REQUIRED) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | ament_lint_auto_find_test_dependencies() 27 | endif() 28 | 29 | ################################################################################ 30 | # Install 31 | ################################################################################ 32 | install( 33 | DIRECTORY launch 34 | DESTINATION share/${PROJECT_NAME} 35 | ) 36 | 37 | 38 | ament_package() 39 | -------------------------------------------------------------------------------- /fd_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fd_bringup 5 | 0.0.0 6 | Package containing launch and run-time configurations 7 | 8 | m.bednarczyk 9 | 10 | Apache License 2.0 11 | 12 | ament_cmake 13 | 14 | ament_lint_auto 15 | ament_lint_common 16 | 17 | rclpy 18 | 19 | rviz2 20 | xacro 21 | controller_manager 22 | joint_state_publisher 23 | joint_state_broadcaster 24 | effort_controllers 25 | fd_description 26 | fd_hardware 27 | urdf 28 | 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /fd_controllers/ee_pose_broadcaster/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ee_pose_broadcaster 4 | 0.0.0 5 | Broadcaster to publish ee pose 6 | m.bednarczyk 7 | 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | pluginlib 13 | rcutils 14 | eigen3_cmake_module 15 | eigen 16 | 17 | pluginlib 18 | rcutils 19 | 20 | controller_interface 21 | hardware_interface 22 | rclcpp_lifecycle 23 | realtime_tools 24 | geometry_msgs 25 | std_msgs 26 | 27 | ament_cmake_gmock 28 | controller_manager 29 | rclcpp 30 | ros2_control_test_assets 31 | 32 | eigen3_cmake_module 33 | eigen 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /.github/mergify.yml: -------------------------------------------------------------------------------- 1 | pull_request_rules: 2 | - name: Backport to humble 3 | conditions: 4 | - base=main 5 | - "label=backport-humble" 6 | actions: 7 | backport: 8 | branches: 9 | - humble 10 | 11 | - name: Ask to resolve conflict 12 | conditions: 13 | - conflict 14 | - author!=mergify[bot] 15 | - author!=dependabot[bot] 16 | actions: 17 | comment: 18 | message: This pull request is in conflict. Could you fix it 19 | @{{author}}? 20 | 21 | - name: Ask to resolve conflict for backports 22 | conditions: 23 | - conflict 24 | - author=mergify[bot] 25 | actions: 26 | comment: 27 | message: This pull request is in conflict. Could you fix it 28 | @tpoignonec? 29 | 30 | - name: development targets main branch 31 | conditions: 32 | - base!=main 33 | - author!=tpoignonec 34 | - author!=mergify[bot] 35 | - author!=dependabot[bot] 36 | actions: 37 | comment: 38 | message: | 39 | @{{author}}, all pull requests must be targeted towards the `master` 40 | development branch. Once merged into `master`, it is possible to 41 | backport to `{{base}}`, but it must be in `master` to have these 42 | changes reflected into new distributions. 43 | -------------------------------------------------------------------------------- /fd_controllers/fd_inertia_broadcaster/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fd_inertia_broadcaster 4 | 0.0.0 5 | Broadcaster to publish cartesian inertia 6 | Thibault Poignonec 7 | 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | pluginlib 13 | rcutils 14 | eigen3_cmake_module 15 | eigen 16 | 17 | pluginlib 18 | rcutils 19 | 20 | controller_interface 21 | hardware_interface 22 | rclcpp_lifecycle 23 | realtime_tools 24 | geometry_msgs 25 | std_msgs 26 | 27 | ament_cmake_gmock 28 | controller_manager 29 | rclcpp 30 | ros2_control_test_assets 31 | 32 | eigen3_cmake_module 33 | eigen 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /fd_controllers/fd_clutch_broadcaster/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fd_clutch_broadcaster 4 | 0.0.0 5 | Broadcaster to publish the clutch state (1 = engaged / 0 = disengaged) 6 | Thibault Poignonec 7 | 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | pluginlib 13 | rcutils 14 | eigen3_cmake_module 15 | eigen 16 | 17 | pluginlib 18 | rcutils 19 | 20 | controller_interface 21 | hardware_interface 22 | rclcpp_lifecycle 23 | realtime_tools 24 | geometry_msgs 25 | std_msgs 26 | 27 | ament_cmake_gmock 28 | controller_manager 29 | rclcpp 30 | ros2_control_test_assets 31 | 32 | eigen3_cmake_module 33 | eigen 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /fd_description/config/fd.config.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 17 | 18 | 19 | 31 | 32 | 36 | 37 | -------------------------------------------------------------------------------- /fd_hardware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fd_hardware) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(hardware_interface REQUIRED) 16 | find_package(pluginlib REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(rclcpp_lifecycle REQUIRED) 19 | find_package(fd_sdk_vendor REQUIRED) 20 | 21 | ## Setup target 22 | add_library( 23 | ${PROJECT_NAME} 24 | SHARED 25 | src/fd_effort_hi.cpp 26 | ) 27 | target_include_directories( 28 | ${PROJECT_NAME} 29 | PRIVATE 30 | include 31 | ) 32 | set(DEPENDENCIES 33 | fd_sdk_vendor 34 | hardware_interface 35 | pluginlib 36 | rclcpp 37 | rclcpp_lifecycle 38 | ) 39 | 40 | ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES}) 41 | 42 | pluginlib_export_plugin_description_file(hardware_interface fd_hardware_plugin.xml) 43 | 44 | # INSTALL 45 | install( 46 | TARGETS ${PROJECT_NAME} 47 | DESTINATION lib 48 | ) 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include 52 | ) 53 | 54 | # install(DIRECTORY launch config 55 | # DESTINATION share/${PROJECT_NAME} 56 | # ) 57 | 58 | if(BUILD_TESTING) 59 | find_package(ament_cmake_gtest REQUIRED) 60 | endif() 61 | 62 | ## EXPORTS 63 | ament_export_include_directories( 64 | include 65 | ) 66 | ament_export_libraries( 67 | ${PROJECT_NAME} 68 | ) 69 | ament_export_dependencies(${DEPENDENCIES}) 70 | ament_package() 71 | -------------------------------------------------------------------------------- /fd_description/config/fd_controllers.yaml: -------------------------------------------------------------------------------- 1 | fd/controller_manager: 2 | ros__parameters: 3 | update_rate: 1000 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | fd_controller: 9 | type: effort_controllers/JointGroupEffortController # ForwardCommandController 10 | 11 | fd_ee_broadcaster: 12 | type: ee_pose_broadcaster/EePoseBroadcaster 13 | 14 | fd_inertia_broadcaster: 15 | type: fd_inertia_broadcaster/FdInertiaBroadcaster 16 | 17 | fd_clutch_broadcaster: 18 | type: fd_clutch_broadcaster/FdClutchBroadcaster 19 | 20 | 21 | fd/fd_controller: 22 | ros__parameters: 23 | command_interfaces: 24 | - effort 25 | state_interfaces: 26 | - position 27 | - velocity 28 | - effort 29 | joints: 30 | - fd_x 31 | - fd_y 32 | - fd_z 33 | ## Note: uncomment the following if using an omega 7 device (e.g., 7 controlled DoF) 34 | # - fd_roll 35 | # - fd_pitch 36 | # - fd_yaw 37 | ## Note: uncomment the following if a clutch is present (e.g., omega 6/7 device) 38 | # - fd_clutch 39 | 40 | fd/fd_ee_broadcaster: 41 | ros__parameters: 42 | transform_translation: 43 | - 0.0 44 | - 0.0 45 | - 0.0 46 | transform_rotation: 47 | - 0.0 48 | - 0.0 49 | - 0.0 50 | joints: 51 | - fd_x 52 | - fd_y 53 | - fd_z 54 | ## Note: uncomment the following if orientation is enabled (e.g., omega 6/7 device) 55 | # - fd_roll 56 | # - fd_pitch 57 | # - fd_yaw 58 | buttons: [button] # Note: only one button supported for now 59 | 60 | fd/fd_inertia_broadcaster: 61 | ros__parameters: 62 | transform_rotation: 63 | - 0.0 64 | - 0.0 65 | - 0.0 66 | inertia_interface_name: fd_inertia 67 | 68 | fd/fd_clutch_broadcaster: 69 | ros__parameters: 70 | clutch_interface_name: "button/position" 71 | is_interface_a_button: true 72 | -------------------------------------------------------------------------------- /fd_controllers/fd_clutch_broadcaster/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fd_clutch_broadcaster) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(controller_interface REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | find_package(rcutils REQUIRED) 18 | find_package(realtime_tools REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | find_package(eigen3_cmake_module REQUIRED) 21 | find_package(Eigen3) 22 | 23 | add_library(fd_clutch_broadcaster 24 | SHARED 25 | src/fd_clutch_broadcaster.cpp 26 | ) 27 | target_include_directories(fd_clutch_broadcaster PRIVATE include) 28 | ament_target_dependencies(fd_clutch_broadcaster 29 | builtin_interfaces 30 | controller_interface 31 | pluginlib 32 | rclcpp_lifecycle 33 | rcutils 34 | realtime_tools 35 | std_msgs 36 | eigen3_cmake_module 37 | Eigen3 38 | ) 39 | # Causes the visibility macros to use dllexport rather than dllimport, 40 | # which is appropriate when building the dll but not consuming it. 41 | target_compile_definitions(fd_clutch_broadcaster PRIVATE "FD_CLUTCH_BROADCASTER_BUILDING_DLL") 42 | # prevent pluginlib from using boost 43 | target_compile_definitions(fd_clutch_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 44 | pluginlib_export_plugin_description_file(controller_interface fd_clutch_broadcaster_plugin.xml) 45 | 46 | install( 47 | DIRECTORY include/ 48 | DESTINATION include 49 | ) 50 | 51 | install( 52 | TARGETS 53 | fd_clutch_broadcaster 54 | RUNTIME DESTINATION bin 55 | ARCHIVE DESTINATION lib 56 | LIBRARY DESTINATION lib 57 | ) 58 | 59 | ament_export_dependencies( 60 | controller_interface 61 | rclcpp_lifecycle 62 | geometry_msgs 63 | eigen3_cmake_module 64 | Eigen3 65 | example_interfaces 66 | ) 67 | ament_export_include_directories( 68 | include 69 | ) 70 | ament_export_libraries( 71 | fd_clutch_broadcaster 72 | ) 73 | ament_package() 74 | -------------------------------------------------------------------------------- /fd_controllers/fd_inertia_broadcaster/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fd_inertia_broadcaster) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(controller_interface REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | find_package(rcutils REQUIRED) 18 | find_package(realtime_tools REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | find_package(eigen3_cmake_module REQUIRED) 21 | find_package(Eigen3) 22 | 23 | add_library(fd_inertia_broadcaster 24 | SHARED 25 | src/fd_inertia_broadcaster.cpp 26 | ) 27 | target_include_directories(fd_inertia_broadcaster PRIVATE include) 28 | ament_target_dependencies(fd_inertia_broadcaster 29 | builtin_interfaces 30 | controller_interface 31 | pluginlib 32 | rclcpp_lifecycle 33 | rcutils 34 | realtime_tools 35 | std_msgs 36 | eigen3_cmake_module 37 | Eigen3 38 | ) 39 | # Causes the visibility macros to use dllexport rather than dllimport, 40 | # which is appropriate when building the dll but not consuming it. 41 | target_compile_definitions(fd_inertia_broadcaster PRIVATE "FD_INERTIA_BROADCASTER_BUILDING_DLL") 42 | # prevent pluginlib from using boost 43 | target_compile_definitions(fd_inertia_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 44 | pluginlib_export_plugin_description_file(controller_interface fd_inertia_broadcaster_plugin.xml) 45 | 46 | install( 47 | DIRECTORY include/ 48 | DESTINATION include 49 | ) 50 | 51 | install( 52 | TARGETS 53 | fd_inertia_broadcaster 54 | RUNTIME DESTINATION bin 55 | ARCHIVE DESTINATION lib 56 | LIBRARY DESTINATION lib 57 | ) 58 | 59 | ament_export_dependencies( 60 | controller_interface 61 | rclcpp_lifecycle 62 | geometry_msgs 63 | eigen3_cmake_module 64 | Eigen3 65 | example_interfaces 66 | ) 67 | ament_export_include_directories( 68 | include 69 | ) 70 | ament_export_libraries( 71 | fd_inertia_broadcaster 72 | ) 73 | ament_package() 74 | -------------------------------------------------------------------------------- /fd_controllers/ee_pose_broadcaster/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ee_pose_broadcaster) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(controller_interface REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | find_package(rcutils REQUIRED) 18 | find_package(realtime_tools REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | find_package(eigen3_cmake_module REQUIRED) 22 | find_package(Eigen3) 23 | 24 | add_library(ee_pose_broadcaster 25 | SHARED 26 | src/ee_pose_broadcaster.cpp 27 | ) 28 | target_include_directories(ee_pose_broadcaster PRIVATE include) 29 | ament_target_dependencies(ee_pose_broadcaster 30 | builtin_interfaces 31 | controller_interface 32 | pluginlib 33 | rclcpp_lifecycle 34 | rcutils 35 | realtime_tools 36 | geometry_msgs 37 | std_msgs 38 | eigen3_cmake_module 39 | Eigen3 40 | ) 41 | # Causes the visibility macros to use dllexport rather than dllimport, 42 | # which is appropriate when building the dll but not consuming it. 43 | target_compile_definitions(ee_pose_broadcaster PRIVATE "EE_POSE_BROADCASTER_BUILDING_DLL") 44 | # prevent pluginlib from using boost 45 | target_compile_definitions(ee_pose_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 46 | pluginlib_export_plugin_description_file(controller_interface ee_pose_plugin.xml) 47 | 48 | install( 49 | DIRECTORY include/ 50 | DESTINATION include 51 | ) 52 | 53 | install( 54 | TARGETS 55 | ee_pose_broadcaster 56 | RUNTIME DESTINATION bin 57 | ARCHIVE DESTINATION lib 58 | LIBRARY DESTINATION lib 59 | ) 60 | 61 | ament_export_dependencies( 62 | controller_interface 63 | rclcpp_lifecycle 64 | geometry_msgs 65 | eigen3_cmake_module 66 | Eigen3 67 | example_interfaces 68 | ) 69 | ament_export_include_directories( 70 | include 71 | ) 72 | ament_export_libraries( 73 | ee_pose_broadcaster 74 | ) 75 | ament_package() 76 | -------------------------------------------------------------------------------- /fd_hardware/include/fd_hardware/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #ifndef FD_HARDWARE__VISIBILITY_CONTROL_H_ 23 | #define FD_HARDWARE__VISIBILITY_CONTROL_H_ 24 | 25 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 26 | // https://gcc.gnu.org/wiki/Visibility 27 | 28 | #if defined _WIN32 || defined __CYGWIN__ 29 | #ifdef __GNUC__ 30 | #define FD_HARDWARE_EXPORT __attribute__((dllexport)) 31 | #define FD_HARDWARE_IMPORT __attribute__((dllimport)) 32 | #else 33 | #define FD_HARDWARE_EXPORT __declspec(dllexport) 34 | #define FD_HARDWARE_IMPORT __declspec(dllimport) 35 | #endif 36 | #ifdef FD_HARDWARE_BUILDING_DLL 37 | #define FD_HARDWARE_PUBLIC FD_HARDWARE_EXPORT 38 | #else 39 | #define FD_HARDWARE_PUBLIC FD_HARDWARE_IMPORT 40 | #endif 41 | #define FD_HARDWARE_PUBLIC_TYPE FD_HARDWARE_PUBLIC 42 | #define FD_HARDWARE_LOCAL 43 | #else 44 | #define FD_HARDWARE_EXPORT __attribute__((visibility("default"))) 45 | #define FD_HARDWARE_IMPORT 46 | #if __GNUC__ >= 4 47 | #define FD_HARDWARE_PUBLIC __attribute__((visibility("default"))) 48 | #define FD_HARDWARE_LOCAL __attribute__((visibility("hidden"))) 49 | #else 50 | #define FD_HARDWARE_PUBLIC 51 | #define FD_HARDWARE_LOCAL 52 | #endif 53 | #define FD_HARDWARE_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // FD_HARDWARE__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #ifndef EE_POSE_BROADCASTER__VISIBILITY_CONTROL_H_ 23 | #define EE_POSE_BROADCASTER__VISIBILITY_CONTROL_H_ 24 | 25 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 26 | // https://gcc.gnu.org/wiki/Visibility 27 | 28 | #if defined _WIN32 || defined __CYGWIN__ 29 | #ifdef __GNUC__ 30 | #define EE_POSE_BROADCASTER_EXPORT __attribute__((dllexport)) 31 | #define EE_POSE_BROADCASTER_IMPORT __attribute__((dllimport)) 32 | #else 33 | #define EE_POSE_BROADCASTER_EXPORT __declspec(dllexport) 34 | #define EE_POSE_BROADCASTER_IMPORT __declspec(dllimport) 35 | #endif 36 | #ifdef EE_POSE_BROADCASTER_BUILDING_DLL 37 | #define EE_POSE_BROADCASTER_PUBLIC EE_POSE_BROADCASTER_EXPORT 38 | #else 39 | #define EE_POSE_BROADCASTER_PUBLIC EE_POSE_BROADCASTER_IMPORT 40 | #endif 41 | #define EE_POSE_BROADCASTER_PUBLIC_TYPE EE_POSE_BROADCASTER_PUBLIC 42 | #define EE_POSE_BROADCASTER_LOCAL 43 | #else 44 | #define EE_POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) 45 | #define EE_POSE_BROADCASTER_IMPORT 46 | #if __GNUC__ >= 4 47 | #define EE_POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) 48 | #define EE_POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) 49 | #else 50 | #define EE_POSE_BROADCASTER_PUBLIC 51 | #define EE_POSE_BROADCASTER_LOCAL 52 | #endif 53 | #define EE_POSE_BROADCASTER_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // EE_POSE_BROADCASTER__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #ifndef FD_CLUTCH_BROADCASTER__VISIBILITY_CONTROL_H_ 23 | #define FD_CLUTCH_BROADCASTER__VISIBILITY_CONTROL_H_ 24 | 25 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 26 | // https://gcc.gnu.org/wiki/Visibility 27 | 28 | #if defined _WIN32 || defined __CYGWIN__ 29 | #ifdef __GNUC__ 30 | #define FD_CLUTCH_BROADCASTER_EXPORT __attribute__((dllexport)) 31 | #define FD_CLUTCH_BROADCASTER_IMPORT __attribute__((dllimport)) 32 | #else 33 | #define FD_CLUTCH_BROADCASTER_EXPORT __declspec(dllexport) 34 | #define FD_CLUTCH_BROADCASTER_IMPORT __declspec(dllimport) 35 | #endif 36 | #ifdef FD_CLUTCH_BROADCASTER_BUILDING_DLL 37 | #define FD_CLUTCH_BROADCASTER_PUBLIC FD_CLUTCH_BROADCASTER_EXPORT 38 | #else 39 | #define FD_CLUTCH_BROADCASTER_PUBLIC FD_CLUTCH_BROADCASTER_IMPORT 40 | #endif 41 | #define FD_CLUTCH_BROADCASTER_PUBLIC_TYPE FD_CLUTCH_BROADCASTER_PUBLIC 42 | #define FD_CLUTCH_BROADCASTER_LOCAL 43 | #else 44 | #define FD_CLUTCH_BROADCASTER_EXPORT __attribute__((visibility("default"))) 45 | #define FD_CLUTCH_BROADCASTER_IMPORT 46 | #if __GNUC__ >= 4 47 | #define FD_CLUTCH_BROADCASTER_PUBLIC __attribute__((visibility("default"))) 48 | #define FD_CLUTCH_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) 49 | #else 50 | #define FD_CLUTCH_BROADCASTER_PUBLIC 51 | #define FD_CLUTCH_BROADCASTER_LOCAL 52 | #endif 53 | #define FD_CLUTCH_BROADCASTER_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // FD_CLUTCH_BROADCASTER__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #ifndef FD_INERTIA_BROADCASTER__VISIBILITY_CONTROL_H_ 23 | #define FD_INERTIA_BROADCASTER__VISIBILITY_CONTROL_H_ 24 | 25 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 26 | // https://gcc.gnu.org/wiki/Visibility 27 | 28 | #if defined _WIN32 || defined __CYGWIN__ 29 | #ifdef __GNUC__ 30 | #define FD_INERTIA_BROADCASTER_EXPORT __attribute__((dllexport)) 31 | #define FD_INERTIA_BROADCASTER_IMPORT __attribute__((dllimport)) 32 | #else 33 | #define FD_INERTIA_BROADCASTER_EXPORT __declspec(dllexport) 34 | #define FD_INERTIA_BROADCASTER_IMPORT __declspec(dllimport) 35 | #endif 36 | #ifdef FD_INERTIA_BROADCASTER_BUILDING_DLL 37 | #define FD_INERTIA_BROADCASTER_PUBLIC FD_INERTIA_BROADCASTER_EXPORT 38 | #else 39 | #define FD_INERTIA_BROADCASTER_PUBLIC FD_INERTIA_BROADCASTER_IMPORT 40 | #endif 41 | #define FD_INERTIA_BROADCASTER_PUBLIC_TYPE FD_INERTIA_BROADCASTER_PUBLIC 42 | #define FD_INERTIA_BROADCASTER_LOCAL 43 | #else 44 | #define FD_INERTIA_BROADCASTER_EXPORT __attribute__((visibility("default"))) 45 | #define FD_INERTIA_BROADCASTER_IMPORT 46 | #if __GNUC__ >= 4 47 | #define FD_INERTIA_BROADCASTER_PUBLIC __attribute__((visibility("default"))) 48 | #define FD_INERTIA_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) 49 | #else 50 | #define FD_INERTIA_BROADCASTER_PUBLIC 51 | #define FD_INERTIA_BROADCASTER_LOCAL 52 | #endif 53 | #define FD_INERTIA_BROADCASTER_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // FD_INERTIA_BROADCASTER__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing Guidelines 2 | Thank you for your interest in contributing to `forcedimension_ros2`. Whether it's a bug report, new feature, correction, or additional documentation, we greatly value feedback and contributions from the community. 3 | 4 | Please read through this document before submitting any issues or pull requests to ensure we have all the necessary information to effectively respond to your bug report or contribution. 5 | 6 | 7 | ## Reporting Bugs/Feature Requests 8 | We welcome you to use the GitHub issue tracker to report bugs or suggest features. 9 | 10 | When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure somebody else hasn't already reported the issue. 11 | 12 | Please try to include as much information as you can. Details like these are incredibly useful: 13 | 14 | * A reproducible test case or series of steps 15 | * The version of our code being used 16 | * Any modifications you've made relevant to the bug 17 | * Anything unusual about your environment or deployment 18 | 19 | 20 | ## Contributing via Pull Requests 21 | Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: 22 | 23 | 1. You are working against the latest source on the *main* branch. 24 | 2. You check existing open, and recently merged, pull requests to make sure someone else hasn't addressed the problem already. 25 | 3. You open an issue to discuss any significant work - we would hate for your time to be wasted. 26 | 27 | To send us a pull request, please: 28 | 29 | 1. Fork the repository. 30 | 2. Modify the source; please focus on the specific change you are contributing. 31 | If you also reformat all the code, it will be hard for us to focus on your change. 32 | 3. Ensure local tests pass. 33 | 4. Commit to your fork using clear commit messages. 34 | 5. Send a pull request, answering any default questions in the pull request interface. 35 | 6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation. 36 | 37 | GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and [creating a pull request](https://help.github.com/articles/creating-a-pull-request/). 38 | 39 | 40 | ## Licensing 41 | Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html): 42 | 43 | ~~~ 44 | 5. Submission of Contributions. Unless You explicitly state otherwise, 45 | any Contribution intentionally submitted for inclusion in the Work 46 | by You to the Licensor shall be under the terms and conditions of 47 | this License, without any additional terms or conditions. 48 | Notwithstanding the above, nothing herein shall supersede or modify 49 | the terms of any separate license agreement you may have executed 50 | with Licensor regarding such Contributions. 51 | ~~~ 52 | -------------------------------------------------------------------------------- /fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/fd_clutch_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef FD_CLUTCH_BROADCASTER__FD_CLUTCH_BROADCASTER_HPP_ 16 | #define FD_CLUTCH_BROADCASTER__FD_CLUTCH_BROADCASTER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "fd_clutch_broadcaster/visibility_control.h" 27 | 28 | #include "controller_interface/controller_interface.hpp" 29 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp" 30 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 31 | #include "realtime_tools/realtime_publisher.hpp" 32 | 33 | #include "std_msgs/msg/bool.hpp" 34 | 35 | namespace fd_clutch_broadcaster 36 | { 37 | class FdClutchBroadcaster : public controller_interface::ControllerInterface 38 | { 39 | public: 40 | FD_CLUTCH_BROADCASTER_PUBLIC 41 | FdClutchBroadcaster(); 42 | 43 | FD_CLUTCH_BROADCASTER_PUBLIC 44 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; 45 | 46 | FD_CLUTCH_BROADCASTER_PUBLIC 47 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 48 | 49 | FD_CLUTCH_BROADCASTER_PUBLIC 50 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 51 | 52 | FD_CLUTCH_BROADCASTER_PUBLIC 53 | controller_interface::return_type update( 54 | const rclcpp::Time & time, 55 | const rclcpp::Duration & period) override; 56 | 57 | FD_CLUTCH_BROADCASTER_PUBLIC 58 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( 59 | const rclcpp_lifecycle::State & previous_state) override; 60 | 61 | FD_CLUTCH_BROADCASTER_PUBLIC 62 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( 63 | const rclcpp_lifecycle::State & previous_state) override; 64 | 65 | FD_CLUTCH_BROADCASTER_PUBLIC 66 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( 67 | const rclcpp_lifecycle::State & previous_state) override; 68 | 69 | protected: 70 | std::string clutch_interface_name_; 71 | 72 | // Publishers 73 | std::shared_ptr> clutch_publisher_; 74 | std::shared_ptr> 75 | realtime_clutch_publisher_; 76 | }; 77 | 78 | } // namespace fd_clutch_broadcaster 79 | 80 | #endif // FD_CLUTCH_BROADCASTER__FD_CLUTCH_BROADCASTER_HPP_ 81 | -------------------------------------------------------------------------------- /fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/fd_inertia_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_ 16 | #define FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "fd_inertia_broadcaster/visibility_control.h" 27 | 28 | #include "controller_interface/controller_interface.hpp" 29 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp" 30 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 31 | #include "realtime_tools/realtime_publisher.hpp" 32 | 33 | #include "std_msgs/msg/float64_multi_array.hpp" 34 | 35 | namespace fd_inertia_broadcaster 36 | { 37 | class FdInertiaBroadcaster : public controller_interface::ControllerInterface 38 | { 39 | public: 40 | FD_INERTIA_BROADCASTER_PUBLIC 41 | FdInertiaBroadcaster(); 42 | 43 | FD_INERTIA_BROADCASTER_PUBLIC 44 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; 45 | 46 | FD_INERTIA_BROADCASTER_PUBLIC 47 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 48 | 49 | FD_INERTIA_BROADCASTER_PUBLIC 50 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 51 | 52 | FD_INERTIA_BROADCASTER_PUBLIC 53 | controller_interface::return_type update( 54 | const rclcpp::Time & time, 55 | const rclcpp::Duration & period) override; 56 | 57 | FD_INERTIA_BROADCASTER_PUBLIC 58 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( 59 | const rclcpp_lifecycle::State & previous_state) override; 60 | 61 | FD_INERTIA_BROADCASTER_PUBLIC 62 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( 63 | const rclcpp_lifecycle::State & previous_state) override; 64 | 65 | FD_INERTIA_BROADCASTER_PUBLIC 66 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( 67 | const rclcpp_lifecycle::State & previous_state) override; 68 | 69 | protected: 70 | std::string inertia_interface_name_; 71 | 72 | Eigen::Matrix4d transform_; 73 | Eigen::Matrix inertia_in_base_, inertia_; 74 | 75 | // Publishers 76 | std::shared_ptr> inertia_publisher_; 77 | std::shared_ptr> 78 | realtime_inertia_publisher_; 79 | }; 80 | 81 | } // namespace fd_inertia_broadcaster 82 | 83 | #endif // FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_ 84 | -------------------------------------------------------------------------------- /fd_description/ros2_control/fd.r2c_hardware.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | fake_components/GenericSystem 10 | 11 | 12 | fd_hardware/FDEffortHardwareInterface 13 | ${interface_id} 14 | ${interface_sn} 15 | ${emulate_button} 16 | ${robot_id}_inertia 17 | ${effector_mass} 18 | ${ignore_orientation_readings} 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EE_POSE_BROADCASTER__EE_POSE_BROADCASTER_HPP_ 16 | #define EE_POSE_BROADCASTER__EE_POSE_BROADCASTER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "controller_interface/controller_interface.hpp" 27 | #include "ee_pose_broadcaster/visibility_control.h" 28 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp" 29 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 30 | #include "realtime_tools/realtime_publisher.hpp" 31 | #include "geometry_msgs/msg/pose_stamped.hpp" 32 | #include "std_msgs/msg/bool.hpp" 33 | 34 | namespace ee_pose_broadcaster 35 | { 36 | class EePoseBroadcaster : public controller_interface::ControllerInterface 37 | { 38 | public: 39 | EE_POSE_BROADCASTER_PUBLIC 40 | EePoseBroadcaster(); 41 | 42 | EE_POSE_BROADCASTER_PUBLIC 43 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; 44 | 45 | EE_POSE_BROADCASTER_PUBLIC 46 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 47 | 48 | EE_POSE_BROADCASTER_PUBLIC 49 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 50 | 51 | EE_POSE_BROADCASTER_PUBLIC 52 | controller_interface::return_type update( 53 | const rclcpp::Time & time, 54 | const rclcpp::Duration & period) override; 55 | 56 | EE_POSE_BROADCASTER_PUBLIC 57 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( 58 | const rclcpp_lifecycle::State & previous_state) override; 59 | 60 | EE_POSE_BROADCASTER_PUBLIC 61 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( 62 | const rclcpp_lifecycle::State & previous_state) override; 63 | 64 | EE_POSE_BROADCASTER_PUBLIC 65 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( 66 | const rclcpp_lifecycle::State & previous_state) override; 67 | 68 | protected: 69 | std::vector joints_; 70 | std::vector buttons_; 71 | std::unordered_map> name_if_value_mapping_; 72 | 73 | Eigen::Matrix4d transform_, pose_; 74 | 75 | // Publishers 76 | std::shared_ptr> ee_pose_publisher_; 77 | std::shared_ptr> 78 | realtime_ee_pose_publisher_; 79 | 80 | std::shared_ptr> fd_button_publisher_; 81 | std::shared_ptr> 82 | realtime_fd_button_publisher_; 83 | }; 84 | 85 | } // namespace ee_pose_broadcaster 86 | 87 | #endif // EE_POSE_BROADCASTER__EE_POSE_BROADCASTER_HPP_ 88 | -------------------------------------------------------------------------------- /fd_hardware/include/fd_hardware/fd_effort_hi.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef FD_HARDWARE__FD_EFFORT_HI_HPP_ 16 | #define FD_HARDWARE__FD_EFFORT_HI_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "hardware_interface/handle.hpp" 23 | #include "hardware_interface/hardware_info.hpp" 24 | #include "hardware_interface/system_interface.hpp" 25 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 26 | #include "rclcpp_lifecycle/state.hpp" 27 | #include "rclcpp/macros.hpp" 28 | #include "fd_hardware/visibility_control.h" 29 | 30 | 31 | namespace fd_hardware 32 | { 33 | class FDEffortHardwareInterface : public hardware_interface::SystemInterface 34 | { 35 | public: 36 | RCLCPP_SHARED_PTR_DEFINITIONS(FDEffortHardwareInterface); 37 | 38 | virtual ~FDEffortHardwareInterface(); 39 | 40 | FD_HARDWARE_PUBLIC 41 | CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; 42 | 43 | FD_HARDWARE_PUBLIC 44 | std::vector export_state_interfaces() override; 45 | 46 | FD_HARDWARE_PUBLIC 47 | std::vector export_command_interfaces() override; 48 | 49 | FD_HARDWARE_PUBLIC 50 | CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 51 | 52 | FD_HARDWARE_PUBLIC 53 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 54 | 55 | FD_HARDWARE_PUBLIC 56 | hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override; 57 | 58 | FD_HARDWARE_PUBLIC 59 | hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; 60 | 61 | private: 62 | /// ID of the interface (Rq: "-1" = invalid/any that is connected) 63 | char interface_ID_ = -1; 64 | 65 | /// Serial number of the interface (Rq: "-1" = invalid / unspecified) 66 | int interface_SN_ = -1; 67 | 68 | /// Turned to true after the connection 69 | bool isConnected_ = false; 70 | 71 | /// If true, the button will be emulated from clutch joint (for omega 6 / sigma 7, see SDK doc) 72 | bool emulate_button_ = false; 73 | 74 | // If true, the reading will emulate an omega 3 interface 75 | bool ignore_orientation_ = false; 76 | 77 | /// Interface mass in Kg, not used if negative 78 | double effector_mass_ = -1.0; 79 | 80 | std::string inertia_interface_name_; 81 | 82 | // Store the command for the robot 83 | std::vector hw_commands_effort_; 84 | std::vector hw_states_position_; 85 | std::vector hw_states_velocity_; 86 | std::vector hw_states_effort_; 87 | std::vector hw_states_inertia_; // upper-triangular matrix 88 | std::vector hw_button_state_; 89 | 90 | /** 91 | Initiate the USB communication with the device. 92 | Warning: Once this method is called, the forces are enable on the device... 93 | @return flag = true if has succeeded 94 | */ 95 | bool connectToDevice(); 96 | 97 | /** 98 | Terminate the USB communication with the device. 99 | See disconnectFromDevice(std_msgs::msg::String& str) for details. 100 | 101 | @return flag = true if has succeeded 102 | */ 103 | bool disconnectFromDevice(); 104 | }; 105 | 106 | 107 | } // namespace fd_hardware 108 | 109 | #endif // FD_HARDWARE__FD_EFFORT_HI_HPP_ 110 | -------------------------------------------------------------------------------- /fd_description/urdf/fd.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # pip install pre-commit 3 | # pre-commit run -a 4 | # 5 | # (to run on all files) 6 | # pre-commit run --all-files 7 | # 8 | # Or: 9 | # 10 | # pre-commit install # (runs every time you commit in git) 11 | # 12 | # To update this file: 13 | # 14 | # pre-commit autoupdate 15 | # 16 | # See https://github.com/pre-commit/pre-commit 17 | 18 | exclude: "ft_gui/ft_gui/ft_calibration_window.py" 19 | 20 | repos: 21 | # Standard hooks 22 | - repo: https://github.com/pre-commit/pre-commit-hooks 23 | rev: v4.4.0 24 | hooks: 25 | - id: check-added-large-files 26 | - id: check-ast 27 | - id: check-case-conflict 28 | - id: check-docstring-first 29 | - id: check-merge-conflict 30 | - id: check-symlinks 31 | - id: check-xml 32 | - id: check-yaml 33 | - id: debug-statements 34 | - id: end-of-file-fixer 35 | - id: mixed-line-ending 36 | - id: trailing-whitespace 37 | exclude_types: [rst] 38 | - id: fix-byte-order-marker 39 | 40 | # Python hooks 41 | - repo: https://github.com/asottile/pyupgrade 42 | rev: v3.3.1 43 | hooks: 44 | - id: pyupgrade 45 | args: [--py36-plus] 46 | 47 | # PyDocStyle 48 | - repo: https://github.com/PyCQA/pydocstyle 49 | rev: 6.3.0 50 | hooks: 51 | - id: pydocstyle 52 | args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] 53 | 54 | - repo: https://github.com/pycqa/flake8 55 | rev: 6.0.0 56 | hooks: 57 | - id: flake8 58 | args: ["--extend-ignore=E501"] 59 | 60 | # Uncrustify 61 | - repo: local 62 | hooks: 63 | - id: ament_uncrustify 64 | name: ament_uncrustify 65 | description: Uncrustify. 66 | stages: [commit] 67 | entry: ament_uncrustify 68 | language: system 69 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 70 | args: ["--reformat"] 71 | 72 | # CPP hooks 73 | - repo: local 74 | hooks: 75 | - id: clang-format 76 | name: clang-format 77 | description: Format files with ClangFormat. 78 | entry: clang-format-14 79 | language: system 80 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 81 | args: ['-fallback-style=none', '-i'] 82 | 83 | # - repo: local 84 | # hooks: 85 | # - id: ament_cppcheck 86 | # name: ament_cppcheck 87 | # description: Static code analysis of C/C++ files. 88 | # stages: [commit] 89 | # entry: ament_cppcheck 90 | # language: system 91 | # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 92 | 93 | # Maybe use https://github.com/cpplint/cpplint instead 94 | - repo: local 95 | hooks: 96 | - id: ament_cpplint 97 | name: ament_cpplint 98 | description: Static code analysis of C/C++ files. 99 | stages: [commit] 100 | entry: ament_cpplint 101 | language: system 102 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 103 | args: ["--linelength=100", "--filter=-whitespace/newline"] 104 | 105 | # Cmake hooks 106 | - repo: local 107 | hooks: 108 | - id: ament_lint_cmake 109 | name: ament_lint_cmake 110 | description: Check format of CMakeLists.txt files. 111 | stages: [commit] 112 | entry: ament_lint_cmake 113 | language: system 114 | files: CMakeLists\.txt$ 115 | 116 | # Copyright 117 | - repo: local 118 | hooks: 119 | - id: ament_copyright 120 | name: ament_copyright 121 | description: Check if copyright notice is available in all files. 122 | stages: [commit] 123 | entry: ament_copyright 124 | language: system 125 | 126 | # Docs - RestructuredText hooks 127 | - repo: https://github.com/PyCQA/doc8 128 | rev: v1.1.1 129 | hooks: 130 | - id: doc8 131 | args: ['--max-line-length=100', '--ignore=D001'] 132 | exclude: CHANGELOG\.rst$ 133 | 134 | - repo: https://github.com/pre-commit/pygrep-hooks 135 | rev: v1.10.0 136 | hooks: 137 | - id: rst-backticks 138 | exclude: CHANGELOG\.rst$ 139 | - id: rst-directive-colons 140 | - id: rst-inline-touching-normal 141 | 142 | # Spellcheck in comments and docs 143 | # skipping of *.svg files is not working... 144 | - repo: https://github.com/codespell-project/codespell 145 | rev: v2.2.2 146 | hooks: 147 | - id: codespell 148 | args: ['--write-changes'] 149 | exclude: CHANGELOG\.rst|\.(svg|pyc)$ 150 | -------------------------------------------------------------------------------- /fd_bringup/launch/fd.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 ICube Laboratory, University of Strasbourg 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument 17 | from launch.substitutions import ( 18 | Command, 19 | FindExecutable, 20 | LaunchConfiguration, 21 | PathJoinSubstitution, 22 | ) 23 | from launch_ros.actions import Node 24 | from launch_ros.parameter_descriptions import ParameterValue 25 | from launch_ros.substitutions import FindPackageShare 26 | 27 | 28 | def generate_launch_description(): 29 | use_fake_hardware = LaunchConfiguration('use_fake_hardware') 30 | use_orientation = LaunchConfiguration('use_orientation') 31 | use_clutch = LaunchConfiguration('use_clutch') 32 | 33 | # Get URDF via xacro 34 | robot_description_content = Command( 35 | [ 36 | PathJoinSubstitution([FindExecutable(name='xacro')]), 37 | ' ', 38 | PathJoinSubstitution( 39 | [ 40 | FindPackageShare('fd_description'), 41 | 'config', 42 | 'fd.config.xacro', 43 | ] 44 | ), 45 | ' use_fake_hardware:=', use_fake_hardware, 46 | ' use_orientation:=', use_orientation, 47 | ' use_clutch:=', use_clutch, 48 | ] 49 | ) 50 | robot_description = \ 51 | {'robot_description': ParameterValue(robot_description_content, value_type=str)} 52 | 53 | phi_controllers = PathJoinSubstitution( 54 | [ 55 | FindPackageShare('fd_description'), 56 | 'config', 57 | 'fd_controllers.yaml', 58 | ] 59 | ) 60 | 61 | node_robot_state_publisher = Node( 62 | package='robot_state_publisher', 63 | executable='robot_state_publisher', 64 | namespace='fd', 65 | output='screen', 66 | parameters=[robot_description], 67 | ) 68 | 69 | # A node to publish world -> iiwa_base transform 70 | static_tf = Node( 71 | package='tf2_ros', 72 | executable='static_transform_publisher', 73 | name='static_transform_publisher', 74 | output='log', 75 | arguments=[ 76 | '0.0', '0.0', '0.0', '3.1416', '0.0', '0.0', 77 | 'world', 78 | 'fd_base' 79 | ], 80 | ) 81 | 82 | controller_manager_node = Node( 83 | package='controller_manager', 84 | executable='ros2_control_node', 85 | namespace='fd', 86 | parameters=[robot_description, phi_controllers], 87 | output={ 88 | 'stdout': 'screen', 89 | 'stderr': 'screen', 90 | }, 91 | ) 92 | 93 | # Load controllers 94 | load_controllers = [] 95 | for controller in [ 96 | 'fd_controller', 97 | 'joint_state_broadcaster', 98 | 'fd_ee_broadcaster', 99 | 'fd_inertia_broadcaster', 100 | 'fd_clutch_broadcaster', 101 | ]: 102 | load_controllers += [ 103 | Node( 104 | package='controller_manager', 105 | executable='spawner', 106 | namespace='fd', 107 | arguments=[ 108 | controller, 109 | '--controller-manager', 110 | '/fd/controller_manager', 111 | ], 112 | ), 113 | ] 114 | 115 | nodes = [ 116 | controller_manager_node, 117 | node_robot_state_publisher, 118 | static_tf, 119 | ] + load_controllers 120 | return LaunchDescription([ 121 | DeclareLaunchArgument( 122 | 'use_fake_hardware', 123 | default_value='false', 124 | description='Use fake r2c hardware interfaces'), 125 | DeclareLaunchArgument( 126 | 'use_orientation', 127 | default_value='false', 128 | description='Read angular positions.velocities' 129 | + '(WARNING! RPY parameterization)' 130 | ), 131 | DeclareLaunchArgument( 132 | 'use_clutch', 133 | default_value='false', 134 | description='Enable clutch (read pos/vel/force and write force)'), 135 | ] + nodes) 136 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # forcedimension_ros2 2 | This stack includes `ros2_control` drivers for Force Dimension SDK compatible haptic interfaces. 3 | 4 | 5 | ***Tested with Jazzy and Humble ROS distributions only*** 6 | 7 | [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) 8 | [![CI (humble)](../../actions/workflows/ci-humble.yml/badge.svg?branch=humble)](../../actions/workflows/ci-humble.yml) 9 | [![CI (jazzy)](../../actions/workflows/ci-jazzy.yml/badge.svg?branch=main)](../../actions/workflows/ci-jazzy.yml) 10 | [![CI (rolling)](../../actions/workflows/ci.yml/badge.svg?branch=main)](../../actions/workflows/ci.yml) 11 | 12 | > **Warning** 13 | > 14 | > The vendor package [fd_sdk_vendor](https://github.com/ICube-Robotics/fd_sdk_vendor.git) is now required! 15 | > Please follow the installation steps carefully. 16 | 17 | > **Note** 18 | > 19 | > Although still online, the `foxy` and `galactic` branches are not maintained anymore and might be obsolete. 20 | 21 | 22 | ## Compatible devices 23 | The driver was currently tested on the following haptic devices: 24 | - Force Dimension [Omega.3](https://www.forcedimension.com/products/omega), [Omega.6](https://www.forcedimension.com/products/omega) and [Omega.7](https://www.forcedimension.com/products/omega) 25 | - Novint [Falcon](https://hapticshouse.com/pages/novints-falcon-haptic-device) 26 | 27 | ## Usage 28 | ### Getting Started 29 | ***Required setup : Ubuntu 24.04 LTS*** 30 | 31 | 1. Install `ros2` packages. The current development is based of `ros2 jazzy`. Installation steps are described [here](https://docs.ros.org/en/jazzy/Installation.html). 32 | 2. Source your `ros2` environment: 33 | ```shell 34 | source /opt/ros/jazzy/setup.bash 35 | ``` 36 | **NOTE**: The ros2 environment needs to be sources in every used terminal. If only one distribution of ros2 is used, it can be added to the `~/.bashrc` file. 37 | 3. Install `colcon` and its extensions : 38 | ```shell 39 | sudo apt install python3-colcon-common-extensions 40 | ``` 41 | 3. Create a new ros2 workspace: 42 | ```shell 43 | mkdir ~/ros2_ws/src 44 | ``` 45 | 4. Pull relevant packages, install dependencies (including the vendor pkg [fd_sdk_vendor](https://github.com/ICube-Robotics/fd_sdk_vendor.git)): 46 | ```shell 47 | cd ~/ros2_ws 48 | cd src 49 | git clone https://github.com/ICube-Robotics/forcedimension_ros2.git 50 | vcs import . < forcedimension_ros2/forcedimension_ros2.repos 51 | rosdep install --ignore-src --from-paths . -y -r 52 | ``` 53 | 6. Compile and source the workspace by using: 54 | ```shell 55 | colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install 56 | source install/setup.bash 57 | ``` 58 | ### Running the driver 59 | 60 | An example launch file is provided with this stack in the `fd_bringup` package. The driver can be run using 61 | ```shell 62 | ros2 launch fd_bringup fd.launch.py 63 | ``` 64 | The device end-effector pose can then be found in the `/fd/ee_pose` and wrench can be set on the `/fd_controller/commands` topic. 65 | Note that __the default launch config is for the Omega 3 device__ (orientation and clutch OFF). 66 | 67 | You can test the readings using [plotjuggler](https://plotjuggler.io/) and the force control by requesting a (small) force along X axis: 68 | ```bash 69 | ros2 topic pub -r 1000 \ 70 | /fd/fd_controller/commands std_msgs/msg/Float64MultiArray "data: [0.5, 0.0, 0.0]" 71 | ``` 72 | 73 | ## Practical information 74 | 75 | ### Initialize usb device 76 | USB devices require `su` privileges to operate unless allowed in udev rules 77 | 78 | To declare a new device : 79 | 1. run `lsusb -v` which gives 80 | ```shell 81 | idVendor = 0x1451 Force Dimension 82 | idProduct = 0x0301 83 | ``` 84 | 2. Create and edit udev rules file 85 | ```shell 86 | sudo nano /etc/udev/rules.d/10-omega_3_USB.rules 87 | ``` 88 | and in the file write 89 | ```shell 90 | ATTRS{idProduct}=="[PRODUCT_ID]", ATTRS{idVendor}=="[VENDOR ID]", MODE="666", GROUP="plugdev" 91 | ``` 92 | **Note**: `[PRODUCT_ID]` is `idProduct` without `0x`, same for `[VENDOR ID]` 93 | 94 | 3. To apply the new rule run 95 | ```shell 96 | sudo udevadm trigger 97 | ``` 98 | 4. You can try your setup by running the `HapticDesk` executable from the sdk `fd_hardware/external/sdk-3.14.0/bin` folder. If the haptic device is recognized, you are ready to go. 99 | 100 | ## Contacts ## 101 | ![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png) 102 | 103 | [ICube Laboratory](https://plateforme.icube.unistra.fr), [University of Strasbourg](https://www.unistra.fr/), France 104 | 105 | __Maciej Bednarczyk:__ [m.bednarczyk@unistra.fr](mailto:m.bednarczyk@unistra.fr), @github: [mcbed](mailto:macbednarczyk@gmail.com) 106 | -------------------------------------------------------------------------------- /fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "fd_clutch_broadcaster/fd_clutch_broadcaster.hpp" 26 | 27 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 28 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 29 | #include "rclcpp/clock.hpp" 30 | #include "rclcpp/qos.hpp" 31 | #include "rclcpp/time.hpp" 32 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 33 | #include "rcpputils/split.hpp" 34 | #include "rcutils/logging_macros.h" 35 | #include "std_msgs/msg/header.hpp" 36 | 37 | 38 | namespace rclcpp_lifecycle 39 | { 40 | class State; 41 | } // namespace rclcpp_lifecycle 42 | 43 | namespace fd_clutch_broadcaster 44 | { 45 | 46 | FdClutchBroadcaster::FdClutchBroadcaster() {} 47 | 48 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 49 | FdClutchBroadcaster::on_init() 50 | { 51 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 52 | } 53 | 54 | controller_interface::InterfaceConfiguration 55 | FdClutchBroadcaster::command_interface_configuration() const 56 | { 57 | return controller_interface::InterfaceConfiguration{ 58 | controller_interface::interface_configuration_type::NONE}; 59 | } 60 | 61 | controller_interface::InterfaceConfiguration FdClutchBroadcaster::state_interface_configuration() 62 | const 63 | { 64 | controller_interface::InterfaceConfiguration state_interfaces_config; 65 | state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 66 | 67 | if (clutch_interface_name_.empty()) { 68 | RCLCPP_WARN(get_node()->get_logger(), "No clutch interface name provided!"); 69 | 70 | } else { 71 | state_interfaces_config.names.push_back(clutch_interface_name_); 72 | } 73 | return state_interfaces_config; 74 | } 75 | 76 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 77 | FdClutchBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) 78 | { 79 | // Declare parameters 80 | try { 81 | auto_declare("clutch_interface_name", std::string("button/position")); 82 | auto_declare("is_interface_a_button", true); 83 | } catch (const std::exception & e) { 84 | fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); 85 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 86 | } 87 | 88 | // Get interface name from parameters 89 | clutch_interface_name_ = get_node()->get_parameter("clutch_interface_name").as_string(); 90 | if (clutch_interface_name_.empty()) { 91 | RCLCPP_ERROR(get_node()->get_logger(), "Please provide the clutch interface name!"); 92 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 93 | } 94 | 95 | try { 96 | clutch_publisher_ = get_node()->create_publisher( 97 | "fd_clutch", 98 | rclcpp::SystemDefaultsQoS()); 99 | realtime_clutch_publisher_ = 100 | std::make_shared>( 101 | clutch_publisher_); 102 | } catch (const std::exception & e) { 103 | // get_node() may throw, logging raw here 104 | fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); 105 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 106 | } 107 | 108 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 109 | } 110 | 111 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 112 | FdClutchBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) 113 | { 114 | if (state_interfaces_.size() != 1) { 115 | RCLCPP_WARN( 116 | get_node()->get_logger(), 117 | "Expecting exactly one state interface."); 118 | } 119 | 120 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 121 | } 122 | 123 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 124 | FdClutchBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) 125 | { 126 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 127 | } 128 | 129 | controller_interface::return_type FdClutchBroadcaster::update( 130 | const rclcpp::Time & /*time*/, 131 | const rclcpp::Duration & /*period*/) 132 | { 133 | RCLCPP_DEBUG(get_node()->get_logger(), "Entering update()"); 134 | if (realtime_clutch_publisher_ && realtime_clutch_publisher_->trylock()) { 135 | RCLCPP_DEBUG(get_node()->get_logger(), "Lock acquired"); 136 | // Read provided state interface 137 | bool is_interface_a_button = get_node()->get_parameter("is_interface_a_button").as_bool(); 138 | double read_value = state_interfaces_[0].get_value(); 139 | 140 | bool clutch_state = false; 141 | if (is_interface_a_button) { 142 | // Clutched (workspace engaged) if button is pressed 143 | clutch_state = (read_value > 0.5) ? true : false; 144 | } else { 145 | // Clutched (workspace engaged) if handle angle is small (i.e., physically clutched) 146 | // E.g., 7th "joint" of Omega 6 / Sigma 7 147 | clutch_state = (read_value < 0.03) ? true : false; 148 | } 149 | 150 | // Publish clucth 151 | auto & fd_clutch_msg = realtime_clutch_publisher_->msg_; 152 | fd_clutch_msg.data = clutch_state; 153 | RCLCPP_DEBUG(get_node()->get_logger(), "publish and unlock"); 154 | realtime_clutch_publisher_->unlockAndPublish(); 155 | } 156 | 157 | return controller_interface::return_type::OK; 158 | } 159 | 160 | } // namespace fd_clutch_broadcaster 161 | 162 | #include "pluginlib/class_list_macros.hpp" 163 | 164 | PLUGINLIB_EXPORT_CLASS( 165 | fd_clutch_broadcaster::FdClutchBroadcaster, controller_interface::ControllerInterface) 166 | -------------------------------------------------------------------------------- /fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "fd_inertia_broadcaster/fd_inertia_broadcaster.hpp" 26 | 27 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 28 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 29 | #include "rclcpp/clock.hpp" 30 | #include "rclcpp/qos.hpp" 31 | #include "rclcpp/time.hpp" 32 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 33 | #include "rcpputils/split.hpp" 34 | #include "rcutils/logging_macros.h" 35 | #include "std_msgs/msg/header.hpp" 36 | 37 | 38 | namespace rclcpp_lifecycle 39 | { 40 | class State; 41 | } // namespace rclcpp_lifecycle 42 | 43 | namespace fd_inertia_broadcaster 44 | { 45 | const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); 46 | const size_t sizeFlattenedInertia = 21; 47 | 48 | unsigned int flattened_index_from_triangular_index( 49 | unsigned int idx_row, 50 | unsigned int idx_col, 51 | unsigned int dim = 6) 52 | { 53 | unsigned int i = idx_row; 54 | unsigned int j = idx_col; 55 | if (idx_col < idx_row) { 56 | i = idx_col; 57 | j = idx_row; 58 | } 59 | return i * (2 * dim - i - 1) / 2 + j; 60 | } 61 | template 62 | void matrixEigenToMsg(const Eigen::MatrixBase & e, std_msgs::msg::Float64MultiArray & m) 63 | { 64 | if (m.layout.dim.size() != 2) { 65 | m.layout.dim.resize(2); 66 | } 67 | m.layout.dim[0].stride = e.rows() * e.cols(); 68 | m.layout.dim[0].size = e.rows(); 69 | m.layout.dim[1].stride = e.cols(); 70 | m.layout.dim[1].size = e.cols(); 71 | if (static_cast(m.data.size()) != e.size()) { 72 | m.data.resize(e.size()); 73 | } 74 | int ii = 0; 75 | for (int i = 0; i < e.rows(); ++i) { 76 | for (int j = 0; j < e.cols(); ++j) { 77 | m.data[ii++] = e.coeff(i, j); 78 | } 79 | } 80 | } 81 | 82 | FdInertiaBroadcaster::FdInertiaBroadcaster() {} 83 | 84 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 85 | FdInertiaBroadcaster::on_init() 86 | { 87 | try { 88 | auto_declare("inertia_interface_name", std::string("fd_inertia")); 89 | auto_declare>("transform_translation", std::vector()); 90 | auto_declare>("transform_rotation", std::vector()); 91 | } catch (const std::exception & e) { 92 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 93 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 94 | } 95 | 96 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 97 | } 98 | 99 | controller_interface::InterfaceConfiguration 100 | FdInertiaBroadcaster::command_interface_configuration() const 101 | { 102 | return controller_interface::InterfaceConfiguration{ 103 | controller_interface::interface_configuration_type::NONE}; 104 | } 105 | 106 | controller_interface::InterfaceConfiguration FdInertiaBroadcaster::state_interface_configuration() 107 | const 108 | { 109 | controller_interface::InterfaceConfiguration state_interfaces_config; 110 | state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 111 | 112 | if (inertia_interface_name_.empty()) { 113 | RCLCPP_WARN(get_node()->get_logger(), "No inertia interface name provided!"); 114 | 115 | } else { 116 | // Map upper triangular part of inertia to inertia state interface 117 | for (uint row = 0; row < 6; row++) { 118 | for (uint col = row; col < 6; col++) { 119 | state_interfaces_config.names.push_back( 120 | inertia_interface_name_ + 121 | "/" + 122 | std::to_string(row) + "" + std::to_string(col) 123 | ); 124 | } 125 | } 126 | } 127 | return state_interfaces_config; 128 | } 129 | 130 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 131 | FdInertiaBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) 132 | { 133 | inertia_interface_name_ = get_node()->get_parameter("inertia_interface_name").as_string(); 134 | if (inertia_interface_name_.empty()) { 135 | RCLCPP_ERROR(get_node()->get_logger(), "Please provide the inertia interface name!"); 136 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 137 | } 138 | 139 | // Get transform parameters 140 | auto transform_trans_param = get_node()->get_parameter("transform_translation").as_double_array(); 141 | auto transform_rot_param = get_node()->get_parameter("transform_rotation").as_double_array(); 142 | Eigen::Quaternion q; 143 | Eigen::Vector3d trans = Eigen::Vector3d::Zero(); 144 | 145 | if (transform_trans_param.size() == 0) { 146 | RCLCPP_INFO(get_node()->get_logger(), "No (linear) transformation provided. Using t = 0,0,0"); 147 | trans << 0.0, 0.0, 0.0; 148 | } else if (transform_trans_param.size() == 3) { 149 | trans << transform_trans_param[0], transform_trans_param[1], transform_trans_param[2]; 150 | } else { 151 | RCLCPP_ERROR(get_node()->get_logger(), "Wrong translation format"); 152 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 153 | } 154 | 155 | if (transform_rot_param.size() == 0) { 156 | q = Eigen::Quaternion(1, 0, 0, 0); 157 | RCLCPP_INFO(get_node()->get_logger(), 158 | "No (angular) transformation provided. Using q = 1,0,0,0"); 159 | } else if (transform_rot_param.size() == 3) { 160 | Eigen::AngleAxisd rollAngle(transform_rot_param[0], Eigen::Vector3d::UnitZ()); 161 | Eigen::AngleAxisd yawAngle(transform_rot_param[1], Eigen::Vector3d::UnitY()); 162 | Eigen::AngleAxisd pitchAngle(transform_rot_param[2], Eigen::Vector3d::UnitX()); 163 | q = rollAngle * yawAngle * pitchAngle; 164 | } else if (transform_rot_param.size() == 4) { 165 | q = Eigen::Quaternion( 166 | transform_rot_param[3], transform_rot_param[0], 167 | transform_rot_param[1], transform_rot_param[2]); 168 | } else { 169 | RCLCPP_ERROR(get_node()->get_logger(), "Wrong rotation format: supported rpy, quaternion"); 170 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 171 | } 172 | transform_ = Eigen::Matrix4d::Identity(); 173 | transform_.block<3, 3>(0, 0) = q.matrix(); 174 | transform_.block<3, 1>(0, 3) = trans; 175 | 176 | std::cout << transform_ << std::endl; 177 | 178 | try { 179 | inertia_publisher_ = get_node()->create_publisher( 180 | "fd_inertia", 181 | rclcpp::SystemDefaultsQoS()); 182 | realtime_inertia_publisher_ = 183 | std::make_shared>( 184 | inertia_publisher_); 185 | } catch (const std::exception & e) { 186 | // get_node() may throw, logging raw here 187 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 188 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 189 | } 190 | 191 | 192 | inertia_in_base_ = Eigen::Matrix::Zero(); 193 | inertia_ = Eigen::Matrix::Zero(); 194 | 195 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 196 | } 197 | 198 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 199 | FdInertiaBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) 200 | { 201 | if (state_interfaces_.size() != sizeFlattenedInertia) { 202 | RCLCPP_WARN( 203 | get_node()->get_logger(), 204 | "Not all requested interfaces exists."); 205 | } 206 | 207 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 208 | } 209 | 210 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 211 | FdInertiaBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) 212 | { 213 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 214 | } 215 | 216 | controller_interface::return_type FdInertiaBroadcaster::update( 217 | const rclcpp::Time & /*time*/, 218 | const rclcpp::Duration & /*period*/) 219 | { 220 | RCLCPP_DEBUG(get_node()->get_logger(), "Entering update()"); 221 | if (realtime_inertia_publisher_ && realtime_inertia_publisher_->trylock()) { 222 | RCLCPP_DEBUG(get_node()->get_logger(), "Lock acquired"); 223 | inertia_in_base_ = Eigen::Matrix::Zero(); 224 | 225 | // Map upper triangular part of inertia to inertia state interface 226 | for (uint row = 0; row < 6; row++) { 227 | for (uint col = 0; col < 6; col++) { 228 | inertia_in_base_(row, col) = 229 | state_interfaces_[flattened_index_from_triangular_index(row, col)].get_value(); 230 | } 231 | } 232 | 233 | // Registration of inertia values 234 | inertia_.block<3, 3>(0, 0) = 235 | transform_.block<3, 3>(0, 0) * inertia_in_base_.block<3, 3>(0, 0) * 236 | transform_.block<3, 3>(0, 0).transpose(); 237 | inertia_.block<3, 3>(3, 3) = 238 | transform_.block<3, 3>(0, 0) * inertia_in_base_.block<3, 3>(3, 3) * 239 | transform_.block<3, 3>(0, 0).transpose(); 240 | 241 | // Publish inertia 242 | auto & fd_inertia_msg = realtime_inertia_publisher_->msg_; 243 | matrixEigenToMsg(inertia_, fd_inertia_msg); 244 | RCLCPP_DEBUG(get_node()->get_logger(), "publish and unlock"); 245 | realtime_inertia_publisher_->unlockAndPublish(); 246 | } 247 | 248 | return controller_interface::return_type::OK; 249 | } 250 | 251 | } // namespace fd_inertia_broadcaster 252 | 253 | #include "pluginlib/class_list_macros.hpp" 254 | 255 | PLUGINLIB_EXPORT_CLASS( 256 | fd_inertia_broadcaster::FdInertiaBroadcaster, controller_interface::ControllerInterface) 257 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "ee_pose_broadcaster/ee_pose_broadcaster.hpp" 26 | 27 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 28 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 29 | #include "rclcpp/clock.hpp" 30 | #include "rclcpp/qos.hpp" 31 | #include "rclcpp/time.hpp" 32 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 33 | #include "rcpputils/split.hpp" 34 | #include "rcutils/logging_macros.h" 35 | #include "std_msgs/msg/header.hpp" 36 | 37 | 38 | namespace rclcpp_lifecycle 39 | { 40 | class State; 41 | } // namespace rclcpp_lifecycle 42 | 43 | namespace ee_pose_broadcaster 44 | { 45 | const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); 46 | using hardware_interface::HW_IF_EFFORT; 47 | using hardware_interface::HW_IF_POSITION; 48 | using hardware_interface::HW_IF_VELOCITY; 49 | 50 | EePoseBroadcaster::EePoseBroadcaster() {} 51 | 52 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 53 | EePoseBroadcaster::on_init() 54 | { 55 | try { 56 | // definition of the parameters that need to be queried from the 57 | // controller configuration file with default values 58 | auto_declare>("joints", std::vector()); 59 | auto_declare>("buttons", std::vector()); 60 | auto_declare>("transform_translation", std::vector()); 61 | auto_declare>("transform_rotation", std::vector()); 62 | } catch (const std::exception & e) { 63 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 64 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 65 | } 66 | 67 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 68 | } 69 | 70 | controller_interface::InterfaceConfiguration 71 | EePoseBroadcaster::command_interface_configuration() const 72 | { 73 | return controller_interface::InterfaceConfiguration{ 74 | controller_interface::interface_configuration_type::NONE}; 75 | } 76 | 77 | controller_interface::InterfaceConfiguration EePoseBroadcaster::state_interface_configuration() 78 | const 79 | { 80 | controller_interface::InterfaceConfiguration state_interfaces_config; 81 | state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 82 | 83 | if (joints_.empty()) { 84 | RCLCPP_WARN(get_node()->get_logger(), "No joint name provided!"); 85 | 86 | } else { 87 | for (const auto & joint : joints_) { 88 | state_interfaces_config.names.push_back(joint + "/" + HW_IF_POSITION); 89 | } 90 | } 91 | 92 | if (buttons_.empty()) { 93 | RCLCPP_WARN(get_node()->get_logger(), "No button name provided!"); 94 | 95 | } else { 96 | for (const auto & button : buttons_) { 97 | state_interfaces_config.names.push_back(button + "/" + HW_IF_POSITION); 98 | } 99 | } 100 | 101 | return state_interfaces_config; 102 | } 103 | 104 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 105 | EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) 106 | { 107 | buttons_ = get_node()->get_parameter("buttons").as_string_array(); 108 | joints_ = get_node()->get_parameter("joints").as_string_array(); 109 | if (joints_.empty()) { 110 | RCLCPP_ERROR(get_node()->get_logger(), "Please provide list of joints in config!"); 111 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 112 | } 113 | 114 | auto transform_trans_param = get_node()->get_parameter("transform_translation").as_double_array(); 115 | auto transform_rot_param = get_node()->get_parameter("transform_rotation").as_double_array(); 116 | Eigen::Quaternion q; 117 | Eigen::Vector3d trans; 118 | 119 | if (transform_trans_param.size() == 0) { 120 | trans << 0.0, 0.0, 0.0; 121 | } else if (transform_trans_param.size() == 3) { 122 | trans << transform_trans_param[0], transform_trans_param[1], transform_trans_param[2]; 123 | } else { 124 | RCLCPP_ERROR(get_node()->get_logger(), "Wrong translation format"); 125 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 126 | } 127 | 128 | if (transform_rot_param.size() == 0) { 129 | q = Eigen::Quaternion(1, 0, 0, 0); 130 | } else if (transform_rot_param.size() == 3) { 131 | Eigen::AngleAxisd rollAngle(transform_rot_param[0], Eigen::Vector3d::UnitZ()); 132 | Eigen::AngleAxisd yawAngle(transform_rot_param[1], Eigen::Vector3d::UnitY()); 133 | Eigen::AngleAxisd pitchAngle(transform_rot_param[2], Eigen::Vector3d::UnitX()); 134 | q = rollAngle * yawAngle * pitchAngle; 135 | } else if (transform_rot_param.size() == 4) { 136 | q = Eigen::Quaternion( 137 | transform_rot_param[3], transform_rot_param[0], 138 | transform_rot_param[1], transform_rot_param[2]); 139 | } else { 140 | RCLCPP_ERROR(get_node()->get_logger(), "Wrong rotation format: supported rpy, quaternion"); 141 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 142 | } 143 | transform_ = Eigen::Matrix4d::Identity(); 144 | pose_ = Eigen::Matrix4d::Identity(); 145 | transform_.block<3, 3>(0, 0) = q.matrix(); 146 | transform_.block<3, 1>(0, 3) = trans; 147 | 148 | std::cout << transform_ << std::endl; 149 | 150 | try { 151 | ee_pose_publisher_ = get_node()->create_publisher( 152 | "ee_pose", 153 | rclcpp::SystemDefaultsQoS()); 154 | 155 | realtime_ee_pose_publisher_ = 156 | std::make_shared>( 157 | ee_pose_publisher_); 158 | 159 | fd_button_publisher_ = get_node()->create_publisher( 160 | "button_state", rclcpp::SystemDefaultsQoS()); 161 | 162 | realtime_fd_button_publisher_ = 163 | std::make_shared>( 164 | fd_button_publisher_); 165 | } catch (const std::exception & e) { 166 | // get_node() may throw, logging raw here 167 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 168 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 169 | } 170 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 171 | } 172 | 173 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 174 | EePoseBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) 175 | { 176 | if (state_interfaces_.size() != (joints_.size())) { 177 | RCLCPP_WARN( 178 | get_node()->get_logger(), 179 | "Not all requested interfaces exists."); 180 | } 181 | 182 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 183 | } 184 | 185 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 186 | EePoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) 187 | { 188 | joints_.clear(); 189 | 190 | return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 191 | } 192 | 193 | double get_value( 194 | const std::unordered_map> & map, 195 | const std::string & name, const std::string & interface_name) 196 | { 197 | const auto & interfaces_and_values = map.at(name); 198 | const auto interface_and_value = interfaces_and_values.find(interface_name); 199 | if (interface_and_value != interfaces_and_values.cend()) { 200 | return interface_and_value->second; 201 | } else { 202 | return kUninitializedValue; 203 | } 204 | } 205 | 206 | controller_interface::return_type EePoseBroadcaster::update( 207 | const rclcpp::Time & /*time*/, 208 | const rclcpp::Duration & /*period*/) 209 | { 210 | for (const auto & state_interface : state_interfaces_) { 211 | name_if_value_mapping_[ 212 | state_interface.get_prefix_name()][state_interface.get_interface_name()] = 213 | state_interface.get_value(); 214 | RCLCPP_DEBUG( 215 | get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_prefix_name().c_str(), 216 | state_interface.get_interface_name().c_str(), state_interface.get_value()); 217 | } 218 | 219 | if (realtime_ee_pose_publisher_ && realtime_ee_pose_publisher_->trylock()) { 220 | pose_ = Eigen::Matrix4d::Identity(); 221 | 222 | if (joints_.size() >= 3) { 223 | double p_x = get_value(name_if_value_mapping_, joints_[0], HW_IF_POSITION); 224 | double p_y = get_value(name_if_value_mapping_, joints_[1], HW_IF_POSITION); 225 | double p_z = get_value(name_if_value_mapping_, joints_[2], HW_IF_POSITION); 226 | 227 | if (std::isnan(p_x) || std::isnan(p_y) || std::isnan(p_z)) { 228 | RCLCPP_DEBUG( 229 | get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!"); 230 | return controller_interface::return_type::ERROR; 231 | } 232 | pose_(0, 3) = p_x; 233 | pose_(1, 3) = p_y; 234 | pose_(2, 3) = p_z; 235 | } 236 | 237 | if (joints_.size() >= 6) { 238 | double roll = get_value(name_if_value_mapping_, joints_[3], HW_IF_POSITION); 239 | double pitch = get_value(name_if_value_mapping_, joints_[4], HW_IF_POSITION); 240 | double yaw = get_value(name_if_value_mapping_, joints_[5], HW_IF_POSITION); 241 | 242 | if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) { 243 | RCLCPP_DEBUG( 244 | get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!"); 245 | return controller_interface::return_type::ERROR; 246 | } 247 | 248 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 249 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 250 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 251 | 252 | Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle; 253 | pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix(); 254 | } 255 | 256 | pose_ = transform_ * pose_; 257 | Eigen::Quaternion q(pose_.block<3, 3>(0, 0)); 258 | 259 | auto & ee_pose_msg = realtime_ee_pose_publisher_->msg_; 260 | 261 | ee_pose_msg.header.stamp = get_node()->get_clock()->now(); 262 | ee_pose_msg.header.frame_id = "fd_base"; 263 | // update pose message 264 | ee_pose_msg.pose.position.x = pose_(0, 3); 265 | ee_pose_msg.pose.position.y = pose_(1, 3); 266 | ee_pose_msg.pose.position.z = pose_(2, 3); 267 | ee_pose_msg.pose.orientation.w = q.w(); 268 | ee_pose_msg.pose.orientation.x = q.x(); 269 | ee_pose_msg.pose.orientation.y = q.y(); 270 | ee_pose_msg.pose.orientation.z = q.z(); 271 | 272 | realtime_ee_pose_publisher_->unlockAndPublish(); 273 | } 274 | 275 | if (!buttons_.empty()) { 276 | if (realtime_fd_button_publisher_ && realtime_fd_button_publisher_->trylock()) { 277 | auto & ee_button_msg = realtime_fd_button_publisher_->msg_; 278 | double button_status = get_value(name_if_value_mapping_, buttons_[0], HW_IF_POSITION); 279 | ee_button_msg.data = button_status > 0.5; 280 | realtime_fd_button_publisher_->unlockAndPublish(); 281 | } 282 | } 283 | 284 | return controller_interface::return_type::OK; 285 | } 286 | 287 | } // namespace ee_pose_broadcaster 288 | 289 | #include "pluginlib/class_list_macros.hpp" 290 | 291 | PLUGINLIB_EXPORT_CLASS( 292 | ee_pose_broadcaster::EePoseBroadcaster, controller_interface::ControllerInterface) 293 | -------------------------------------------------------------------------------- /fd_hardware/src/fd_effort_hi.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, ICube Laboratory, University of Strasbourg 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "fd_hardware/fd_effort_hi.hpp" 27 | 28 | #include "fd_sdk_vendor/dhd.hpp" 29 | #include "fd_sdk_vendor/drd.hpp" 30 | 31 | #include "hardware_interface/component_parser.hpp" 32 | #include "hardware_interface/lexical_casts.hpp" 33 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 34 | #include "rclcpp/rclcpp.hpp" 35 | 36 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 37 | 38 | namespace fd_hardware 39 | { 40 | 41 | rclcpp::Logger LOGGER = rclcpp::get_logger("FDEffortHardwareInterface"); 42 | 43 | unsigned int flattened_index_from_triangular_index( 44 | unsigned int idx_row, 45 | unsigned int idx_col, 46 | unsigned int dim = 6) 47 | { 48 | unsigned int i = idx_row; 49 | unsigned int j = idx_col; 50 | if (idx_col < idx_row) { 51 | i = idx_col; 52 | j = idx_row; 53 | } 54 | return i * (2 * dim - i - 1) / 2 + j; 55 | } 56 | 57 | 58 | FDEffortHardwareInterface::~FDEffortHardwareInterface() 59 | { 60 | // If controller manager is shutdown via Ctrl + C, the on_deactivate methods won't be called. 61 | // We need to call them here to ensure that the device is stopped and disconnected. 62 | on_deactivate(rclcpp_lifecycle::State()); 63 | } 64 | 65 | // ------------------------------------------------------------------------------------------ 66 | CallbackReturn FDEffortHardwareInterface::on_init( 67 | const hardware_interface::HardwareInfo & info) 68 | { 69 | if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { 70 | return CallbackReturn::ERROR; 71 | } 72 | 73 | 74 | hw_states_position_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 75 | hw_states_velocity_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 76 | hw_states_effort_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 77 | hw_commands_effort_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 78 | hw_states_inertia_.resize(15, std::numeric_limits::quiet_NaN()); 79 | hw_button_state_.resize(info_.gpios.size(), std::numeric_limits::quiet_NaN()); 80 | 81 | 82 | for (const hardware_interface::ComponentInfo & joint : info_.joints) { 83 | // PHI has currently exactly 3 states and 1 command interface on each joint 84 | if (joint.command_interfaces.size() != 1) { 85 | RCLCPP_FATAL( 86 | LOGGER, 87 | "Joint '%s' has %lu command interfaces found. 1 expected.", joint.name.c_str(), 88 | joint.command_interfaces.size()); 89 | return CallbackReturn::ERROR; 90 | } 91 | 92 | if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT) { 93 | RCLCPP_FATAL( 94 | LOGGER, 95 | "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), 96 | joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT); 97 | return CallbackReturn::ERROR; 98 | } 99 | 100 | if (joint.state_interfaces.size() != 3) { 101 | RCLCPP_FATAL( 102 | LOGGER, 103 | "Joint '%s' has %ld state interface. 3 expected.", joint.name.c_str(), 104 | joint.state_interfaces.size()); 105 | return CallbackReturn::ERROR; 106 | } 107 | 108 | if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { 109 | RCLCPP_FATAL( 110 | LOGGER, 111 | "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), 112 | joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); 113 | return CallbackReturn::ERROR; 114 | } 115 | if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { 116 | RCLCPP_FATAL( 117 | LOGGER, 118 | "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), 119 | joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); 120 | return CallbackReturn::ERROR; 121 | } 122 | if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { 123 | RCLCPP_FATAL( 124 | LOGGER, 125 | "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), 126 | joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT); 127 | return CallbackReturn::ERROR; 128 | } 129 | } 130 | for (const hardware_interface::ComponentInfo & button : info_.gpios) { 131 | if (button.state_interfaces.size() != 1) { 132 | RCLCPP_FATAL( 133 | LOGGER, 134 | "Button '%s' has %lu state interface. 1 expected.", button.name.c_str(), 135 | button.state_interfaces.size()); 136 | return CallbackReturn::ERROR; 137 | } 138 | if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { 139 | RCLCPP_FATAL( 140 | LOGGER, 141 | "Button '%s' have %s state interface. '%s' expected.", button.name.c_str(), 142 | button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); 143 | return CallbackReturn::ERROR; 144 | } 145 | } 146 | for (const hardware_interface::ComponentInfo & button : info_.gpios) { 147 | if (button.state_interfaces.size() != 1) { 148 | RCLCPP_FATAL( 149 | LOGGER, 150 | "Button '%s' has %lu state interface. 1 expected.", button.name.c_str(), 151 | button.state_interfaces.size()); 152 | return CallbackReturn::ERROR; 153 | } 154 | if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { 155 | RCLCPP_FATAL( 156 | LOGGER, 157 | "Button '%s' have %s state interface. '%s' expected.", button.name.c_str(), 158 | button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); 159 | return CallbackReturn::ERROR; 160 | } 161 | } 162 | 163 | 164 | // Get parameters 165 | auto it_interface_id = info_.hardware_parameters.find("interface_id"); 166 | if (it_interface_id != info_.hardware_parameters.end()) { 167 | interface_ID_ = stoi(it_interface_id->second); 168 | RCLCPP_INFO(LOGGER, "Using interface ID: %d", interface_ID_); 169 | } else { 170 | interface_ID_ = -1; 171 | } 172 | 173 | 174 | auto it_interface_serial_number = info_.hardware_parameters.find("interface_serial_number"); 175 | if (it_interface_serial_number != info_.hardware_parameters.end()) { 176 | interface_SN_ = stoi(it_interface_serial_number->second); 177 | RCLCPP_INFO(LOGGER, "Using interface serial number: %d", interface_SN_); 178 | } else { 179 | interface_SN_ = -1; 180 | } 181 | 182 | auto it_emulate_button = info_.hardware_parameters.find("emulate_button"); 183 | if (it_emulate_button != info_.hardware_parameters.end()) { 184 | emulate_button_ = hardware_interface::parse_bool(it_emulate_button->second); 185 | } else { 186 | emulate_button_ = false; 187 | } 188 | RCLCPP_INFO(LOGGER, "Emulating button: %s", emulate_button_ ? "true" : "false"); 189 | 190 | auto it_fd_inertia = info_.hardware_parameters.find("inertia_interface_name"); 191 | if (it_fd_inertia != info_.hardware_parameters.end()) { 192 | inertia_interface_name_ = it_fd_inertia->second; 193 | } else { 194 | inertia_interface_name_ = "fd_inertia"; 195 | } 196 | 197 | auto it_interface_mass = info_.hardware_parameters.find("effector_mass"); 198 | if (it_interface_mass != info_.hardware_parameters.end()) { 199 | effector_mass_ = hardware_interface::stod(it_interface_mass->second); 200 | RCLCPP_INFO(LOGGER, "Interface mass parameter found: %lf Kg", effector_mass_); 201 | } else { 202 | effector_mass_ = -1.0; 203 | } 204 | 205 | 206 | auto it_ignore_orientation = info_.hardware_parameters.find("ignore_orientation_readings"); 207 | if (it_ignore_orientation != info_.hardware_parameters.end()) { 208 | ignore_orientation_ = hardware_interface::parse_bool(it_ignore_orientation->second); 209 | } else { 210 | ignore_orientation_ = false; 211 | } 212 | RCLCPP_INFO(LOGGER, "Ignoring orientation readings: %s", ignore_orientation_ ? "true" : "false"); 213 | 214 | // Contingency for emulated button 215 | // (commanded clutch force might be always left to NaN...) 216 | if (emulate_button_ && 217 | (info_.joints.size() == 4 || info_.joints.size() > 6)) 218 | { 219 | // Prevent NaN in clutch cmd 220 | hw_commands_effort_[info_.joints.size() - 1] = 0.0; 221 | } 222 | 223 | return CallbackReturn::SUCCESS; 224 | } 225 | // ------------------------------------------------------------------------------------------ 226 | std::vector 227 | FDEffortHardwareInterface::export_state_interfaces() 228 | { 229 | std::vector state_interfaces; 230 | for (uint i = 0; i < info_.joints.size(); i++) { 231 | state_interfaces.emplace_back( 232 | hardware_interface::StateInterface( 233 | info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i])); 234 | } 235 | for (uint i = 0; i < info_.joints.size(); i++) { 236 | state_interfaces.emplace_back( 237 | hardware_interface::StateInterface( 238 | info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i])); 239 | } 240 | for (uint i = 0; i < info_.joints.size(); i++) { 241 | state_interfaces.emplace_back( 242 | hardware_interface::StateInterface( 243 | info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i])); 244 | } 245 | for (uint i = 0; i < info_.gpios.size(); i++) { 246 | state_interfaces.emplace_back( 247 | hardware_interface::StateInterface( 248 | info_.gpios[i].name, hardware_interface::HW_IF_POSITION, &hw_button_state_[i])); 249 | } 250 | 251 | // Map upper triangular part of inertia to inertia state interface 252 | for (uint row = 0; row < 6; row++) { 253 | for (uint col = row; col < 6; col++) { 254 | state_interfaces.emplace_back( 255 | hardware_interface::StateInterface( 256 | inertia_interface_name_, 257 | std::to_string(row) + "" + std::to_string(col), 258 | &hw_states_inertia_[flattened_index_from_triangular_index(row, col)])); 259 | } 260 | } 261 | 262 | return state_interfaces; 263 | } 264 | // ------------------------------------------------------------------------------------------ 265 | std::vector 266 | FDEffortHardwareInterface::export_command_interfaces() 267 | { 268 | std::vector command_interfaces; 269 | for (uint i = 0; i < info_.joints.size(); i++) { 270 | command_interfaces.emplace_back( 271 | hardware_interface::CommandInterface( 272 | info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_effort_[i])); 273 | } 274 | 275 | return command_interfaces; 276 | } 277 | // ------------------------------------------------------------------------------------------ 278 | CallbackReturn FDEffortHardwareInterface::on_activate( 279 | const rclcpp_lifecycle::State & previous_state) 280 | { 281 | (void)previous_state; // hush "-Wunused-parameter" warning 282 | 283 | RCLCPP_INFO(LOGGER, "Starting ...please wait..."); 284 | 285 | if (connectToDevice()) { 286 | RCLCPP_INFO(LOGGER, "System Successfully started!"); 287 | return CallbackReturn::SUCCESS; 288 | } else { 289 | RCLCPP_ERROR(LOGGER, "System Not started!"); 290 | return CallbackReturn::ERROR; 291 | } 292 | } 293 | // ------------------------------------------------------------------------------------------ 294 | CallbackReturn FDEffortHardwareInterface::on_deactivate( 295 | const rclcpp_lifecycle::State & previous_state) 296 | { 297 | (void)previous_state; // hush "-Wunused-parameter" warning 298 | RCLCPP_INFO(LOGGER, "Stopping ...please wait..."); 299 | 300 | if (disconnectFromDevice()) { 301 | RCLCPP_INFO(LOGGER, "System successfully stopped!"); 302 | return CallbackReturn::SUCCESS; 303 | } else { 304 | RCLCPP_ERROR(LOGGER, "System Not stopped!"); 305 | return CallbackReturn::ERROR; 306 | } 307 | } 308 | // ------------------------------------------------------------------------------------------ 309 | hardware_interface::return_type FDEffortHardwareInterface::read( 310 | const rclcpp::Time & /*time*/, 311 | const rclcpp::Duration & /*period*/) 312 | { 313 | // "Success" flag 314 | int flag = 0; 315 | 316 | // Get position & orientation 317 | flag += dhdGetPosition( 318 | &hw_states_position_[0], &hw_states_position_[1], &hw_states_position_[2], 319 | interface_ID_); 320 | if (ignore_orientation_ && hw_states_position_.size() == 4) { 321 | // No orientation, skip! 322 | } else if (!ignore_orientation_ && hw_states_position_.size() > 3) { 323 | flag += dhdGetOrientationRad( 324 | &hw_states_position_[3], &hw_states_position_[4], 325 | &hw_states_position_[5], interface_ID_); 326 | } else if (ignore_orientation_ && hw_states_position_.size() > 3) { 327 | // Force orientation to zero 328 | hw_states_position_[3] = 0.0; 329 | hw_states_position_[4] = 0.0; 330 | hw_states_position_[5] = 0.0; 331 | } 332 | 333 | // Get gripper angle 334 | if (dhdHasGripper(interface_ID_) && hw_states_position_.size() == 4) { 335 | flag += dhdGetGripperAngleRad(&hw_states_position_[3], interface_ID_); 336 | } else if (dhdHasGripper(interface_ID_) && hw_states_position_.size() > 6) { 337 | flag += dhdGetGripperAngleRad(&hw_states_position_[6], interface_ID_); 338 | } 339 | 340 | // Get velocity 341 | flag += dhdGetLinearVelocity( 342 | &hw_states_velocity_[0], &hw_states_velocity_[1], 343 | &hw_states_velocity_[2], interface_ID_); 344 | if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) { 345 | // No orientation, skip! 346 | } else if (!ignore_orientation_ && hw_states_velocity_.size() > 3) { 347 | flag += dhdGetAngularVelocityRad( 348 | &hw_states_velocity_[3], &hw_states_velocity_[4], 349 | &hw_states_velocity_[5], interface_ID_); 350 | } else if (ignore_orientation_ && hw_states_velocity_.size() > 3) { 351 | // Force angular velocity to zero 352 | hw_states_velocity_[3] = 0.0; 353 | hw_states_velocity_[4] = 0.0; 354 | hw_states_velocity_[5] = 0.0; 355 | } 356 | 357 | // Get gripper angular velocity 358 | if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() == 4) { 359 | flag += dhdGetGripperAngularVelocityRad(&hw_states_velocity_[3], interface_ID_); 360 | } else if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() > 6) { 361 | flag += dhdGetGripperAngularVelocityRad(&hw_states_velocity_[6], interface_ID_); 362 | } 363 | 364 | // Get forces 365 | double torque[3]; 366 | double gripper_force; 367 | flag += dhdGetForceAndTorqueAndGripperForce( 368 | &hw_states_effort_[0], &hw_states_effort_[1], &hw_states_effort_[2], 369 | &torque[0], &torque[1], &torque[2], 370 | &gripper_force, 371 | interface_ID_ 372 | ); 373 | 374 | // Get torques 375 | if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) { 376 | // No orientation, skip! 377 | } else if (!ignore_orientation_ && hw_states_effort_.size() > 3) { 378 | hw_states_effort_[3] = torque[0]; 379 | hw_states_effort_[4] = torque[1]; 380 | hw_states_effort_[5] = torque[2]; 381 | } else if (ignore_orientation_ && hw_states_effort_.size() > 3) { 382 | // Force angular forces to zero 383 | hw_states_effort_[3] = 0.0; 384 | hw_states_effort_[4] = 0.0; 385 | hw_states_effort_[5] = 0.0; 386 | } 387 | 388 | // Get gripper force 389 | if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() == 4) { 390 | hw_states_effort_[3] = gripper_force; 391 | } else if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() > 6) { 392 | hw_states_effort_[6] = gripper_force; 393 | } 394 | // Get inertia 395 | double inertia_array[6][6]; 396 | double joint_position[DHD_MAX_DOF]; 397 | // use "dhdGetJointAngles (double j[DHD_MAX_DOF], char ID=-1)" to get the joint positions 398 | flag += dhdEnableExpertMode(); 399 | flag += dhdGetJointAngles(joint_position, interface_ID_); 400 | // use "dhdJointAnglesToInertiaMatrix (double j[DHD_MAX_DOF], double inertia[6][6], char ID=-1)" 401 | // to get the current inertia matrix 402 | flag += dhdJointAnglesToInertiaMatrix(joint_position, inertia_array, interface_ID_); 403 | flag += dhdDisableExpertMode(); 404 | 405 | // Map upper triangular part of inertia to inertia state interface 406 | for (uint row = 0; row < 6; row++) { 407 | for (uint col = row; col < 6; col++) { 408 | hw_states_inertia_[flattened_index_from_triangular_index(row, col)] = inertia_array[row][col]; 409 | } 410 | } 411 | 412 | // Get button status, TODO multiple buttons from index 413 | int button_status = dhdGetButton(0, interface_ID_); 414 | if (button_status == 1) { 415 | hw_button_state_[0] = 1.0; 416 | } else if (button_status == 0) { 417 | hw_button_state_[0] = 0.0; 418 | } else { 419 | RCLCPP_ERROR(LOGGER, "Invalid button reading!"); 420 | flag += -1; 421 | } 422 | 423 | if (flag >= 0) { 424 | return hardware_interface::return_type::OK; 425 | } else { 426 | RCLCPP_ERROR(LOGGER, "Updating from system failed!"); 427 | return hardware_interface::return_type::ERROR; 428 | } 429 | } 430 | // ------------------------------------------------------------------------------------------ 431 | hardware_interface::return_type FDEffortHardwareInterface::write( 432 | const rclcpp::Time & /*time*/, 433 | const rclcpp::Duration & /*period*/) 434 | { 435 | bool isNan = false; 436 | for (auto & command : hw_commands_effort_) { 437 | if (command != command) { 438 | isNan = true; 439 | } 440 | } 441 | 442 | if (!isNan) { 443 | if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() > 6) { 444 | dhdSetForceAndTorqueAndGripperForce( 445 | hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2], 446 | hw_commands_effort_[3], hw_commands_effort_[4], hw_commands_effort_[5], 447 | hw_commands_effort_[6], interface_ID_); 448 | } else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() == 4) { 449 | dhdSetForceAndTorqueAndGripperForce( 450 | hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2], 451 | 0.0, 0.0, 0.0, hw_commands_effort_[3], interface_ID_); 452 | } else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) { 453 | // No clutch joint 454 | dhdSetForceAndTorqueAndGripperForce( 455 | hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2], 456 | hw_commands_effort_[3], hw_commands_effort_[4], hw_commands_effort_[5], 457 | 0, interface_ID_); 458 | } else { 459 | // Only translation is actuated 460 | dhdSetForceAndTorqueAndGripperForce( 461 | hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2], 462 | 0, 0, 0, 0, interface_ID_); 463 | } 464 | } else { 465 | dhdSetForceAndTorqueAndGripperForce(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, interface_ID_); 466 | } 467 | 468 | 469 | return hardware_interface::return_type::OK; 470 | } 471 | // ------------------------------------------------------------------------------------------ 472 | bool FDEffortHardwareInterface::connectToDevice() 473 | { 474 | int major, minor, release, revision; 475 | dhdGetSDKVersion(&major, &minor, &release, &revision); 476 | RCLCPP_INFO( 477 | LOGGER, 478 | "dhd : Using SDK version %d.%d (release %d / revision %d)", 479 | major, minor, release, revision); 480 | 481 | // Open connection 482 | bool dhd_open_success = false; 483 | if (interface_SN_ >= 0) { 484 | // Open specified device from serial number 485 | RCLCPP_INFO( 486 | LOGGER, "dhd : Connecting to device with serial number %d, please wait...", interface_SN_); 487 | uint16_t serialNumber = static_cast(interface_SN_); 488 | interface_ID_ = dhdOpenSerial(serialNumber); 489 | dhd_open_success = (interface_ID_ >= 0); 490 | // dhd_open_success = (dhdOpenSerial(serialNumber) >= 0); 491 | } else if (interface_ID_ >= 0) { 492 | // Open specified device from device ID 493 | RCLCPP_INFO(LOGGER, "dhd : Connecting to device with ID= %d, please wait...", interface_ID_); 494 | interface_ID_ = dhdOpenID(interface_ID_); 495 | dhd_open_success = (interface_ID_ >= 0); 496 | // dhd_open_success = (dhdOpenID(interface_ID_) >= 0); 497 | } else { 498 | // Open default device 499 | RCLCPP_INFO(LOGGER, "dhd : Connecting to default device., please wait.."); 500 | interface_ID_ = dhdOpen(); 501 | dhd_open_success = (interface_ID_ >= 0); 502 | // dhd_open_success = (dhdOpen() >= 0); 503 | } 504 | 505 | // Check connection and setup dhd device 506 | if (dhd_open_success) { 507 | RCLCPP_INFO(LOGGER, "dhd : %s device detected", dhdGetSystemName(interface_ID_)); 508 | uint16_t serialNumber = 0; 509 | if (dhdGetSerialNumber(&serialNumber, interface_ID_) < 0) { 510 | RCLCPP_WARN(LOGGER, "dhd : Impossible to retrieve device serial number: %s!", 511 | dhdErrorGetLastStr()); 512 | } else { 513 | RCLCPP_INFO(LOGGER, "dhd : device serial number = %d", serialNumber); 514 | } 515 | RCLCPP_INFO(LOGGER, "dhd : device interface ID = %d", interface_ID_); 516 | 517 | // Check if the device has 3 dof or more 518 | if (dhdHasWrist(interface_ID_)) { 519 | RCLCPP_INFO(LOGGER, "dhd : Rotation supported"); 520 | } else { 521 | RCLCPP_INFO(LOGGER, "dhd : Rotation not supported"); 522 | } 523 | 524 | // Retrieve the mass of the device 525 | double current_effector_mass = 0.0; 526 | if (dhdGetEffectorMass(¤t_effector_mass, interface_ID_) == DHD_NO_ERROR) { 527 | RCLCPP_INFO(LOGGER, "dhd : Effector Mass = %f (g)", current_effector_mass * 1000.0); 528 | } else { 529 | RCLCPP_WARN(LOGGER, "dhd : Impossible to retrieve effector mass !"); 530 | } 531 | 532 | // Set force limit & enable force 533 | double forceMax = 12; // N 534 | if (dhdSetMaxForce(forceMax, interface_ID_) < DHD_NO_ERROR) { 535 | RCLCPP_ERROR(LOGGER, "dhd : Could not set max force!"); 536 | disconnectFromDevice(); 537 | } 538 | // apply zero force 539 | dhdSetBrakes(DHD_OFF, interface_ID_); 540 | 541 | if (dhdEnableForce(DHD_ON, interface_ID_) < DHD_NO_ERROR) { 542 | RCLCPP_ERROR(LOGGER, "dhd : Could not enable force control!"); 543 | disconnectFromDevice(); 544 | return false; 545 | } 546 | // Set effector mass 547 | if (effector_mass_ > 0.0) { 548 | RCLCPP_INFO( 549 | LOGGER, 550 | "dhd : Changing effector mass from %fto %f (g)!", 551 | current_effector_mass * 1000.0, 552 | effector_mass_ * 1000.0); 553 | if (dhdSetEffectorMass(effector_mass_, interface_ID_) < DHD_NO_ERROR) { 554 | RCLCPP_ERROR(LOGGER, "dhd : Failed to set effector mass!"); 555 | disconnectFromDevice(); 556 | return false; 557 | } 558 | } 559 | // Gravity compensation 560 | if (dhdSetGravityCompensation(DHD_ON, interface_ID_) < DHD_NO_ERROR) { 561 | RCLCPP_ERROR(LOGGER, "dhd : Could not enable the gravity compensation !"); 562 | disconnectFromDevice(); 563 | return false; 564 | } else { 565 | RCLCPP_INFO(LOGGER, "dhd : Gravity compensation enabled"); 566 | } 567 | RCLCPP_INFO(LOGGER, "dhd : Device connected !"); 568 | 569 | // Emulate button 570 | if (emulate_button_ && !dhdHasGripper(interface_ID_)) { 571 | RCLCPP_ERROR(LOGGER, "dhd : Could not enable button emulation, no gripper found!"); 572 | } else if (emulate_button_ && dhdHasGripper(interface_ID_)) { 573 | RCLCPP_INFO(LOGGER, "dhd : Emulating button from clutch joint..."); 574 | if (dhdEmulateButton(DHD_ON, interface_ID_) < DHD_NO_ERROR) { 575 | RCLCPP_ERROR(LOGGER, "dhd : Could not enable button emulation!"); 576 | disconnectFromDevice(); 577 | return false; 578 | } 579 | RCLCPP_INFO(LOGGER, "dhd : OK, button will be emulated from clutch joint."); 580 | } 581 | // Set force to zero 582 | if (dhdSetForceAndTorqueAndGripperForce( 583 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 584 | interface_ID_) < DHD_NO_ERROR) 585 | { 586 | RCLCPP_ERROR(LOGGER, "dhd : Could not initialize force control!"); 587 | disconnectFromDevice(); 588 | return false; 589 | } 590 | 591 | ignore_orientation_ |= !dhdHasWrist(interface_ID_); 592 | if (ignore_orientation_) { 593 | RCLCPP_INFO(LOGGER, "dhd : Orientation will be ignored !"); 594 | } 595 | 596 | // Sleep 100 ms 597 | dhdSleep(0.1); 598 | isConnected_ = true; 599 | 600 | return isConnected_; 601 | } else { 602 | RCLCPP_ERROR(LOGGER, "dhd : Could not connect to device !"); 603 | isConnected_ = false; 604 | return isConnected_; 605 | } 606 | } 607 | // ------------------------------------------------------------------------------------------ 608 | bool FDEffortHardwareInterface::disconnectFromDevice() 609 | { 610 | // Stop the device: disables the force on the haptic device and puts it into BRAKE mode. 611 | int hasStopped = -1; 612 | while (hasStopped < 0) { 613 | RCLCPP_INFO(LOGGER, "dhd : stopping the device, please wait..."); 614 | hasStopped = dhdStop(interface_ID_); 615 | // Sleep 100 ms 616 | dhdSleep(0.1); 617 | } 618 | 619 | // close device connection 620 | int connectionIsClosed = dhdClose(interface_ID_); 621 | if (connectionIsClosed >= 0) { 622 | RCLCPP_INFO(LOGGER, "dhd : Disconnected ! "); 623 | interface_ID_ = -1; 624 | return true; 625 | } else { 626 | RCLCPP_ERROR(LOGGER, "dhd : Failed to disconnect !"); 627 | return false; 628 | } 629 | } 630 | 631 | } // namespace fd_hardware 632 | // ------------------------------------------------------------------------------------------ 633 | #include "pluginlib/class_list_macros.hpp" 634 | 635 | PLUGINLIB_EXPORT_CLASS( 636 | fd_hardware::FDEffortHardwareInterface, hardware_interface::SystemInterface) 637 | --------------------------------------------------------------------------------