From d87e42cf31a0ebc47246291a6260243d94fe781a Mon Sep 17 00:00:00 2001 From: Hydran00 Date: Thu, 12 Dec 2024 16:25:37 +0100 Subject: [PATCH] Major refactor: - all the works under vf_control is moved under haptic_control - haptic_control.cpp is renamed into sample_teleoperation - improved readme - added github CI for open3d --- .github/workflows/main.yml | 55 +- README.md | 102 +- haptic_control.sh | 5 - src/haptic_control/CMakeLists.txt | 105 +- src/haptic_control/config/parameters.yaml | 23 +- .../include/circular_buffer.hpp | 0 .../include/haptic_control_base.hpp | 10 +- .../closest_on_triangle.hpp | 2 +- .../include/mesh_virtual_fixtures}/json.hpp | 0 .../mesh_virtual_fixtures}/location.hpp | 0 .../include/mesh_virtual_fixtures}/mesh.hpp | 0 .../mesh_virtual_fixtures}/qp_wrapper.hpp | 0 .../mesh_virtual_fixtures}/vf_computation.hpp | 0 .../mesh_virtual_fixtures}/vf_enforcer.hpp | 0 .../mesh_virtual_fixtures}/visualization.hpp | 0 ...c_control.hpp => sample_teleoperation.hpp} | 16 +- .../include/system_interface.hpp | 62 +- .../include/utils.hpp | 0 .../launch/auto_calibration.launch.py | 8 +- .../launch/haptic_control.launch.py | 162 +-- .../launch/sample_teleoperation.launch.py | 54 + .../launch/sim_haptic_control.launch.py | 97 -- .../launch/test_vf.launch.py | 0 src/haptic_control/package.xml | 1 + .../rviz/vf.rviz | 26 +- .../src/haptic_control_base.cpp | 6 + .../src/mesh_test.cpp | 4 +- ...c_control.cpp => sample_teleoperation.cpp} | 1053 +++++++++-------- .../test/vf_test.cpp | 4 +- src/vf_control/CMakeLists.txt | 72 -- src/vf_control/config/parameters.yaml | 49 - src/vf_control/launch/vf.launch.py | 109 -- src/vf_control/package.xml | 49 - start_RaptorAPIWrapper.sh | 4 - start_TestAdmittance.sh | 4 - start_TestCalibration.sh | 4 - start_TestImpedance.sh | 4 - 37 files changed, 924 insertions(+), 1166 deletions(-) delete mode 100755 haptic_control.sh rename src/{vf_control => haptic_control}/include/circular_buffer.hpp (100%) rename src/{vf_control => haptic_control}/include/haptic_control_base.hpp (95%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/closest_on_triangle.hpp (99%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/json.hpp (100%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/location.hpp (100%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/mesh.hpp (100%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/qp_wrapper.hpp (100%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/vf_computation.hpp (100%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/vf_enforcer.hpp (100%) rename src/{vf_control/include/vf => haptic_control/include/mesh_virtual_fixtures}/visualization.hpp (100%) rename src/haptic_control/include/{haptic_control.hpp => sample_teleoperation.hpp} (93%) rename src/{vf_control => haptic_control}/include/system_interface.hpp (75%) rename src/{vf_control => haptic_control}/include/utils.hpp (100%) mode change 100755 => 100644 src/haptic_control/launch/haptic_control.launch.py create mode 100644 src/haptic_control/launch/sample_teleoperation.launch.py delete mode 100755 src/haptic_control/launch/sim_haptic_control.launch.py rename src/{vf_control => haptic_control}/launch/test_vf.launch.py (100%) rename src/{vf_control => haptic_control}/rviz/vf.rviz (87%) rename src/{vf_control => haptic_control}/src/haptic_control_base.cpp (98%) rename src/{vf_control => haptic_control}/src/mesh_test.cpp (99%) rename src/haptic_control/src/{haptic_control.cpp => sample_teleoperation.cpp} (89%) rename src/{vf_control => haptic_control}/test/vf_test.cpp (99%) delete mode 100755 src/vf_control/CMakeLists.txt delete mode 100755 src/vf_control/config/parameters.yaml delete mode 100644 src/vf_control/launch/vf.launch.py delete mode 100755 src/vf_control/package.xml delete mode 100755 start_RaptorAPIWrapper.sh delete mode 100755 start_TestAdmittance.sh delete mode 100755 start_TestCalibration.sh delete mode 100755 start_TestImpedance.sh diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 0767700..00445d2 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -8,28 +8,45 @@ on: jobs: humble_source: runs-on: ubuntu-22.04 + strategy: + matrix: + package-name: + - test_calibration + - test_admittance + - test_impedance + - haptic_control steps: - - uses: ros-tooling/setup-ros@v0.7 + # Step 1: Set up ROS Humble + - name: Setup ROS Humble + uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: humble - - run: sudo apt-get install -y libpciaccess-dev libomp-dev - - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: test_calibration - target-ros2-distro: humble - skip-tests: true - - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: test_admittance - target-ros2-distro: humble - skip-tests: true - - uses: ros-tooling/action-ros-ci@v0.3 + + # Step 2: Install additional dependencies + - name: Install Dependencies + run: sudo apt-get update && sudo apt-get install -y libpciaccess-dev libomp-dev + + # Step 3: Clone and build Open3D with caching + - name: Cache Open3D Build + uses: actions/cache@v3 with: - package-name: test_impedance - target-ros2-distro: humble - skip-tests: true - - uses: ros-tooling/action-ros-ci@v0.3 + path: Open3D/build + key: ${{ runner.os }}-open3d-${{ hashFiles('Open3D/**') }} + + - name: Clone and Build Open3D + run: | + git clone https://github.com/isl-org/Open3D || true + cd Open3D + source util/install_deps_ubuntu.sh || exit 1 + mkdir -p build && cd build + cmake .. || exit 1 + make -j$(nproc) || exit 1 + sudo make install || exit 1 + + # Step 4: Build and process each ROS package + - name: Build and Process ROS Package + uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: haptic_control + package-name: ${{ matrix.package-name }} target-ros2-distro: humble - skip-tests: true + skip-tests: true \ No newline at end of file diff --git a/README.md b/README.md index 99e159e..1cafae2 100755 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ [![Humble CI](https://github.com/idra-lab/haptic_interface_ROS2/actions/workflows/main.yml/badge.svg)](https://github.com/idra-lab/haptic_interface_ROS2/actions/workflows/main.yml) -*The following instructions are the one that were distribuited with the vendor's code with some clarifications. You must follow the instruction in the haptic documentation first (having the right files under /etc/Haption/). Documentation and libraries are [here](https://drive.google.com/drive/folders/1g4NHb75PtUcHunHAImuzkCfoDhdFXWoR?usp=drive_link). The binaries and the headers are already in the correct folder so ***you do not need to follow point #2***. Also you can avoid points #0 and #4 by running the 'entrypoint.sh'* +*The following instructions are the one that were distribuited with the vendor's code with some clarifications. You must follow the instruction in the haptic documentation first (having the right files under /etc/Haption/). Documentation and libraries are [here](https://drive.google.com/drive/folders/1g4NHb75PtUcHunHAImuzkCfoDhdFXWoR?usp=drive_link). You can avoid points #0 and #4 by running the 'entrypoint.sh'* This project includes a basic implementation of the test programs (TestCalibration, TestImpedance and TestAdmittance) distribuited with the haptic interface and the IDRA team's implementation for the teleoperation (under `haptic_control`) using ROS2 and RaptorAPI. This guide is written for Linux, Windows users need to adapt it in the appropriate way. @@ -13,7 +13,7 @@ This guide is written for Linux, Windows users need to adapt it in the appropria ```bash sudo apt-get install libpciaccess-dev ``` -The next requirements are needed only if the virtual fixtures are used, otherwise you can delete the `vf_control` package +The next requirements is needed only if the virtual fixtures are used. - OpenMP ``` sudo apt install libomp-dev @@ -25,71 +25,75 @@ The next requirements are needed only if the virtual fixtures are used, otherwis ```bash git clone https://github.com/idra-lab/haptic_interface_ROS2 haption_ws ``` -1. Source ROS environment and copy shell scripts to the workspace root: +1. Source ROS environment: ```bash source /opt/ros/$ROS-DISTRO/local_setup.bash ``` - ```bash - cp /src/haptic_interface_ROS2/*.sh - ``` -2. Copy the RaptorAPI shared libraries to `src/haption_raptor_api/Dependencies/RaptorAPI/bin/Linux/glibc-` -3. Copy the `desktop_6D_n65.param` in `/etc/Haption/Connector`. Create the missing folders if needed. The parameters file is stored in the IDRA drive folder [here](https://drive.google.com/drive/folders/1g4NHb75PtUcHunHAImuzkCfoDhdFXWoR?usp=sharing), request access to the drive if you need to use the haptic interface. -4. Compile the raptor_api_interfaces: - ```bash - colcon build --packages-select raptor_api_interfaces - ``` - and build the workspace +3. Copy the `desktop_6D_n65.param` in `/etc/Haption/Connector`. Create the missing folders if needed. The parameters file is stored in the IDRA drive folder [here](https://drive.google.com/drive/folders/1g4NHb75PtUcHunHAImuzkCfoDhdFXWoR?usp=sharing), request access to the drive if you need to use the haptic interface. +4. Build the workspace ```bash colcon build --symlink-install ``` -5. Update LD_LIBRARY_PATH so that it includes the path to `src/haption_raptor_api/Dependencies/RaptorAPI/bin/Linux/glibc-` - ``` - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/src/haption_raptor_api/Dependencies/RaptorAPI/bin/Linux/glibc- - ``` -6. Source the workspace: - ```bash - source /install/local_setup.bash - ``` -7. Enter **sudo mode** and run the entrypoint.sh script: +5. Enter **sudo mode** and run the entrypoint.sh script: ```bash sudo su source entrypoint.sh ``` -8. Start the RaptorAPIWrapper node by calling +6. Start the RaptorAPIWrapper node by calling ```bash - ./start_RaptorAPIWrapper.sh + ros2 run haption_raptor_api raptor_api_wrapper ``` -9. Calibrate the robot if it was not already calibrated (in another sudo shell): - - Edit the `test_calibration/parameters.yaml` according to your network setup - - Check that the green `calibration/force feedback` button is not pressed otherwise press it again. - - Run the calibration node by calling - ```bash - ./start_TestCalibration.sh - ``` - - When you read `C_WAITINGFORPOWER: P_NOPOWER` in the RaptorAPIWrapper terminal, press the green `calibration/force feedback` button on the haptic device to power it on. - You will see the haptic device moving to the calibration position. -10. Run the impedance node (in another sudo shell): - - Edit the `test_impedance/parameters.yaml` according to your network setup - - Run - ```bash - ./start_TestImpedance.sh - ``` - Or run the admittance node (in another sudo shell): - - Edit the `test_admittance/parameters.yaml` according to your network setup - - Run - ```bash - ./start_TestAdmittance.sh - ``` ## Ethernet configuration The computer must be connected via ethernet to the haptic device black box. The network interface used must have the `192.168.100.50` IP address. In Ubuntu you can set this IP from `Settings -> Network -> Select the right network interface -> Click the gear -> IPv4` and set - Address: `192.168.100.50` - Netmask: `255.255.255.0` -## Robot teleoperation + +## Run the examples +#### Calibration +Calibrate the robot if it was not already calibrated (in another sudo shell): + - Edit the `test_calibration/parameters.yaml` according to your network setup + - Check that the green `calibration/force feedback` button is not pressed otherwise press it again. + - Run the calibration node by calling + ```bash + ros2 run test_calibration test_calibration --ros-args --params-file ./src/test_calibration/config/parameters.yaml + ``` + - When you read `C_WAITINGFORPOWER: P_NOPOWER` in the RaptorAPIWrapper terminal, press the green `calibration/force feedback` button on the haptic device to power it on. + You will see the haptic device moving to the calibration position. +#### Impedance control +. Run the impedance node (in another sudo shell): +- Edit the `test_impedance/parameters.yaml` according to your network setup +- Run + ```bash + ros2 run test_impedance test_impedance --ros-args --params-file ./src/test_impedance/config/parameters.yaml + ``` +#### Admittance control +Or run the admittance node (in another sudo shell): +- Edit the `test_admittance/parameters.yaml` according to your network setup +- Run + ```bash + ros2 run test_impedance test_impedance --ros-args --params-file ./src/test_impedance/config/parameters.yaml + ``` + + + +## Robot teleoperation sample The haptic interface can be used to command a target pose and the resulting force measured by the robot can be -exerted by the haptic interface through the `haptic_control` node. Also a safe XYZ cartesian position zone limit is implemented. Limits can be changes modifyng the parameters in `haptic_control/parameters.yaml`. -Finally run +exerted by the haptic interface through the `haptic_control` package that I implemented. Check `haptic_control/parameters.yaml` for the list of parameters. ```bash -./haptic_control.sh +ros2 launch haptic_control sample_teleoperation.launch.py +``` +## Mesh Virtual Fixtures control +An implementation of the algorithm presented [here](https://ieeexplore.ieee.org/document/9341590/) is implemented. +### Requirements +- Open3D C++ +### Run the teleoperation with mesh virtual fixtures +``` +ros2 launch haptic_control haptic_control.launch.py use_fixtures:=true +``` +### Delay simulation +A delay simulation is implemented and can be tested with +``` +ros2 launch haptic_control haptic_control.launch.py use_fixtures:=true delay:=0.4 ``` # Trouble shooting - If you get this error `HAPTION::RaptorAPI::StartLogging() failed with error 13` you probably did not sourced `entrypoint.sh` diff --git a/haptic_control.sh b/haptic_control.sh deleted file mode 100755 index 8b4cb53..0000000 --- a/haptic_control.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash -clear -colcon build --packages-select haptic_control -. install/local_setup.bash -ros2 launch haptic_control haptic_control.launch.py haption_ws_path:=$(pwd)/ diff --git a/src/haptic_control/CMakeLists.txt b/src/haptic_control/CMakeLists.txt index 393b659..76f4d73 100755 --- a/src/haptic_control/CMakeLists.txt +++ b/src/haptic_control/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) -set(This haptic_control) project(haptic_control) + # Default to C99 if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) @@ -11,71 +11,88 @@ endif() if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() +set(CMAKE_BUILD_TYPE Release) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -fopenmp) endif() - - -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(raptor_api_interfaces REQUIRED) find_package(haption_raptor_api REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) - -# find eigen +find_package(visualization_msgs REQUIRED) find_package(Eigen3 REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -add_executable(${This} src/haptic_control.cpp) -ament_target_dependencies(${This} rclcpp raptor_api_interfaces haption_raptor_api tf2_ros tf2_geometry_msgs) -target_include_directories(${This} PUBLIC include) +find_package(Open3D REQUIRED) -target_include_directories(${This} PUBLIC +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(vf_test test/vf_test.cpp) + target_include_directories(vf_test PUBLIC + $ + $ + ) + target_link_libraries(vf_test Eigen3::Eigen Open3D::Open3D qpoases::qpoases gomp) +endif() -$ -$ -${CMAKE_CURRENT_SOURCE_DIR}/../../install/raptor_api_interfaces/include/raptor_api_interfaces -${CMAKE_CURRENT_SOURCE_DIR}/../../install/raptor_api_interfaces/include/ +### QP Oasis +set(QPOASES_INSTALL_DIR ${CMAKE_BINARY_DIR}/qpoases_install) +include(ExternalProject) +ExternalProject_Add( + external_qpoases + URL https://github.com/coin-or/qpOASES/archive/refs/tags/releases/3.2.1.tar.gz + CMAKE_ARGS + -DCMAKE_INSTALL_PREFIX=${QPOASES_INSTALL_DIR} + -DCMAKE_BUILD_TYPE=Release + -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} + -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} + -DBUILD_SHARED_LIBS=OFF + -DQPOASES_BUILD_EXAMPLES=OFF ) +add_library(qpoases_helper INTERFACE) +add_dependencies(qpoases_helper external_qpoases) +target_include_directories(qpoases_helper INTERFACE ${QPOASES_INSTALL_DIR}/include) +target_link_libraries(qpoases_helper INTERFACE ${QPOASES_INSTALL_DIR}/lib/libqpOASES.a) +add_library(qpoases::qpoases ALIAS qpoases_helper) +########## -if(MSVC) +### Open3D 0.18.0 +# if Open3d is not found, then do not build the haptic_control node +if(Open3D_FOUND) + message(STATUS "Open3D found. Building haptic_control node.") else() - execute_process(COMMAND /usr/bin/ldd --version - COMMAND head -n 1 - COMMAND /usr/bin/awk "{print $NF}" - OUTPUT_STRIP_TRAILING_WHITESPACE - OUTPUT_VARIABLE GLIBC) - message("GLIBC=${GLIBC}") - target_compile_definitions(${This} PUBLIC PROTOCOL_FB LINUX _OS_UNIX) - set(CMAKE_C_FLAGS_RELEASE "-Wall") - set(CMAKE_CXX_FLAGS_RELEASE "-Wall") - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") + message(STATUS "Open3D not found. Downloading...") + include(FetchContent) + FetchContent_Declare( + open3d + URL https://github.com/isl-org/Open3D/releases/download/v0.18.0/open3d-devel-linux-x86_64-cxx11-abi-0.18.0.tar.xz + ) + FetchContent_GetProperties(open3d) + if(NOT open3d_POPULATED) + FetchContent_Populate(open3d) + endif() + find_package(Open3D REQUIRED HINTS ${open3d_SOURCE_DIR}) + message(STATUS "Source dir: ${open3d_SOURCE_DIR}") endif() +########## +add_executable(sample_teleoperation src/sample_teleoperation.cpp) +ament_target_dependencies(sample_teleoperation rclcpp raptor_api_interfaces haption_raptor_api tf2_ros tf2_geometry_msgs) +include_directories(include) +target_link_libraries(sample_teleoperation Eigen3::Eigen) + +add_executable(haptic_control src/haptic_control_base.cpp) +ament_target_dependencies(haptic_control rclcpp raptor_api_interfaces haption_raptor_api tf2_ros tf2_geometry_msgs visualization_msgs) +include_directories(include) +target_link_libraries(haptic_control Eigen3::Eigen Open3D::Open3D qpoases::qpoases gomp) -install(TARGETS ${This} +install(TARGETS ${This} haptic_control sample_teleoperation DESTINATION lib/${PROJECT_NAME}) install( - DIRECTORY launch config + DIRECTORY launch config rviz DESTINATION share/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -target_link_libraries(haptic_control Eigen3::Eigen) ament_package() diff --git a/src/haptic_control/config/parameters.yaml b/src/haptic_control/config/parameters.yaml index 659e6ef..c2a3f28 100755 --- a/src/haptic_control/config/parameters.yaml +++ b/src/haptic_control/config/parameters.yaml @@ -4,11 +4,12 @@ haptic_control: ff_device_ip_address: "192.168.100.53" ff_device_param_file: "/etc/Haption/Connector/desktop_6D_n65.param" local_ip_address: "192.168.100.50" - max_force_: 6.0 - # robot position safety limits - enable_safety_sphere: true + haptic_control_rate: 1000.0 + ft_sensor_rate: 500.0 + max_force: 6.0 # Newton + + enable_safety_sphere: false safety_sphere_radius: 0.48 - use_limits: false min_x: -100.0 max_x: 100.0 min_y: -100.0 @@ -24,10 +25,20 @@ haptic_control: tool_link_name: "probe" base_link_name: "base_link" - ft_link_name: "ft_sensor0_wrench" + ft_link_name: "tool0" + delay: 0.0 # round trip simulated delay in seconds (0.0 for no delay) force_scale : 0.3 - # safety box that can be activated (centered in the current ee position) + # safety box that can be activated (centered in the current ee position) -> Not implemented yet enable_safety_box: false safety_box_length: 0.2 safety_box_width: 0.2 safety_box_height: 0.2 + + # vf parameters: + mesh_type: "sphere" # knot, sphere, file, bunny + output_mesh_path : "/home/zotac01/virtual_fixture_vis.obj" # path to save the output mesh for Rviz visualization + input_mesh_path: "/home/zotac01/Ultrasound-Demo/body_modelling/ros2_ws/final_vf.obj" # path to the input mesh if mesh_type is file + skin_mesh_path: "/home/zotac01/Ultrasound-Demo/body_modelling/ros2_ws/skin_mesh.obj" # path to the skin mesh if mesh_type is file + radius: 0.005 # radius of the end effector sphere + lookup_area: 0.006 # should be at least 2*radius + plane_size: 0.001 # plane size used to visualize active constraints in rviz \ No newline at end of file diff --git a/src/vf_control/include/circular_buffer.hpp b/src/haptic_control/include/circular_buffer.hpp similarity index 100% rename from src/vf_control/include/circular_buffer.hpp rename to src/haptic_control/include/circular_buffer.hpp diff --git a/src/vf_control/include/haptic_control_base.hpp b/src/haptic_control/include/haptic_control_base.hpp similarity index 95% rename from src/vf_control/include/haptic_control_base.hpp rename to src/haptic_control/include/haptic_control_base.hpp index e7d04da..b1177c6 100644 --- a/src/vf_control/include/haptic_control_base.hpp +++ b/src/haptic_control/include/haptic_control_base.hpp @@ -1,6 +1,11 @@ +/* +Teleoperation node using haptic device and force feedback which implements the +mesh virtual fixtures explained in the paper +https://ieeexplore.ieee.org/document/9341590/ +@Author: Davide Nardi +*/ #ifndef __HAPTIC_CONTROL_BASE__ #define __HAPTIC_CONTROL_BASE__ - #include #include @@ -23,7 +28,7 @@ #include "circular_buffer.hpp" #include "system_interface.hpp" #include "utils.hpp" -#include "vf/vf_enforcer.hpp" +#include "mesh_virtual_fixtures/vf_enforcer.hpp" class HapticControlBase : public rclcpp::Node { public: @@ -98,7 +103,6 @@ class HapticControlBase : public rclcpp::Node { std::string ft_link_name_; bool received_haptic_pose_; bool received_ee_pose_; - bool use_limits_; bool enable_safety_sphere_, enable_safety_box_; // delay simulation double delay_; diff --git a/src/vf_control/include/vf/closest_on_triangle.hpp b/src/haptic_control/include/mesh_virtual_fixtures/closest_on_triangle.hpp similarity index 99% rename from src/vf_control/include/vf/closest_on_triangle.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/closest_on_triangle.hpp index 45416e8..e9c7a0e 100644 --- a/src/vf_control/include/vf/closest_on_triangle.hpp +++ b/src/haptic_control/include/mesh_virtual_fixtures/closest_on_triangle.hpp @@ -3,7 +3,7 @@ #include #include -#include "vf/location.hpp" +#include "mesh_virtual_fixtures/location.hpp" double round_up(const double value, const int decimal_places) { const double multiplier = std::pow(10.0, decimal_places); diff --git a/src/vf_control/include/vf/json.hpp b/src/haptic_control/include/mesh_virtual_fixtures/json.hpp similarity index 100% rename from src/vf_control/include/vf/json.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/json.hpp diff --git a/src/vf_control/include/vf/location.hpp b/src/haptic_control/include/mesh_virtual_fixtures/location.hpp similarity index 100% rename from src/vf_control/include/vf/location.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/location.hpp diff --git a/src/vf_control/include/vf/mesh.hpp b/src/haptic_control/include/mesh_virtual_fixtures/mesh.hpp similarity index 100% rename from src/vf_control/include/vf/mesh.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/mesh.hpp diff --git a/src/vf_control/include/vf/qp_wrapper.hpp b/src/haptic_control/include/mesh_virtual_fixtures/qp_wrapper.hpp similarity index 100% rename from src/vf_control/include/vf/qp_wrapper.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/qp_wrapper.hpp diff --git a/src/vf_control/include/vf/vf_computation.hpp b/src/haptic_control/include/mesh_virtual_fixtures/vf_computation.hpp similarity index 100% rename from src/vf_control/include/vf/vf_computation.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/vf_computation.hpp diff --git a/src/vf_control/include/vf/vf_enforcer.hpp b/src/haptic_control/include/mesh_virtual_fixtures/vf_enforcer.hpp similarity index 100% rename from src/vf_control/include/vf/vf_enforcer.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/vf_enforcer.hpp diff --git a/src/vf_control/include/vf/visualization.hpp b/src/haptic_control/include/mesh_virtual_fixtures/visualization.hpp similarity index 100% rename from src/vf_control/include/vf/visualization.hpp rename to src/haptic_control/include/mesh_virtual_fixtures/visualization.hpp diff --git a/src/haptic_control/include/haptic_control.hpp b/src/haptic_control/include/sample_teleoperation.hpp similarity index 93% rename from src/haptic_control/include/haptic_control.hpp rename to src/haptic_control/include/sample_teleoperation.hpp index f13e3d3..fc3640d 100644 --- a/src/haptic_control/include/haptic_control.hpp +++ b/src/haptic_control/include/sample_teleoperation.hpp @@ -1,5 +1,9 @@ -#ifndef HAPTIC_CONTROL_HPP -#define HAPTIC_CONTROL_HPP +/* +Sample example of teleoperation using haptic device and force feedback +@Author: Davide Nardi +*/ +#ifndef SAMPLE_TELEOPERATION_HPP +#define SAMPLE_TELEOPERATION_HPP #include #include @@ -25,6 +29,7 @@ #include "raptor_api_interfaces/msg/out_virtuose_pose.hpp" #include "raptor_api_interfaces/msg/out_virtuose_speed.hpp" #include "raptor_api_interfaces/msg/out_virtuose_status.hpp" +#include "raptor_api_interfaces/srv/virtuose_calibrate.hpp" #include "raptor_api_interfaces/srv/virtuose_impedance.hpp" #include "raptor_api_interfaces/srv/virtuose_reset.hpp" @@ -75,7 +80,9 @@ class HapticControl : public rclcpp::Node { _in_virtuose_force; // ROS2 service client_s rclcpp::Client::SharedPtr - client_; + impedance_client_; + rclcpp::Client::SharedPtr + calibration_client_; // ROS2 timers rclcpp::TimerBase::SharedPtr pose_update_timer_; rclcpp::TimerBase::SharedPtr impedanceThread_; @@ -110,7 +117,6 @@ class HapticControl : public rclcpp::Node { std::string ft_link_name_; bool received_haptic_pose_; bool received_ee_pose_; - bool use_limits_; bool enable_safety_sphere_, enable_safety_box_; // Storage for virtuose_node status int64_t status_date_sec_; @@ -131,4 +137,4 @@ class HapticControl : public rclcpp::Node { Eigen::Vector3d x_tilde_old_, x_tilde_new_; }; -#endif // HAPTIC_CONTROL_HPP +#endif // SAMPLE_TELEOPERATION_HPP \ No newline at end of file diff --git a/src/vf_control/include/system_interface.hpp b/src/haptic_control/include/system_interface.hpp similarity index 75% rename from src/vf_control/include/system_interface.hpp rename to src/haptic_control/include/system_interface.hpp index 87acf6e..73b5b3d 100644 --- a/src/vf_control/include/system_interface.hpp +++ b/src/haptic_control/include/system_interface.hpp @@ -13,6 +13,7 @@ #include "raptor_api_interfaces/msg/out_virtuose_pose.hpp" #include "raptor_api_interfaces/msg/out_virtuose_speed.hpp" #include "raptor_api_interfaces/msg/out_virtuose_status.hpp" +#include "raptor_api_interfaces/srv/virtuose_calibrate.hpp" #include "raptor_api_interfaces/srv/virtuose_impedance.hpp" #include "raptor_api_interfaces/srv/virtuose_reset.hpp" @@ -40,17 +41,23 @@ class SystemInterface : public rclcpp::Node { wrench_publisher_ = this->create_publisher( "in_virtuose_force", 1); + calibration_client_ = + this->create_client( + "virtuose_calibrate"); impedance_client_ = this->create_client( "virtuose_impedance"); } void create_connection() { + auto cal = std::make_shared< + raptor_api_interfaces::srv::VirtuoseCalibrate::Request>(); auto imp = std::make_shared< raptor_api_interfaces::srv::VirtuoseImpedance::Request>(); - imp->channel = "SimpleChannelUDP"; - imp->ff_device_ip_address = "192.168.100.53"; - imp->ff_device_param_file = "/etc/Haption/Connector/desktop_6D_n65.param"; - imp->local_ip_address = "192.168.100.50"; + imp->channel = cal->channel = "SimpleChannelUDP"; + imp->ff_device_ip_address = cal->ff_device_ip_address = "192.168.100.53"; + imp->ff_device_param_file = cal->ff_device_param_file = + "/etc/Haption/Connector/desktop_6D_n65.param"; + imp->local_ip_address = cal->local_ip_address = "192.168.100.50"; imp->base_frame.translation.x = 0.0; imp->base_frame.translation.y = 0.0; @@ -61,24 +68,57 @@ class SystemInterface : public rclcpp::Node { imp->base_frame.rotation.z = q_haptic_base_to_robot_base_.z(); imp->base_frame.rotation.w = q_haptic_base_to_robot_base_.w(); + while (!calibration_client_->wait_for_service( + std::literals::chrono_literals::operator""s(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Interrupted while waiting for the calibration service. Exiting."); + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Calibration service not available, waiting again..."); + } + auto calibration_result = calibration_client_->async_send_request(cal); + bool success = false; + // Wait for the result. + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), + calibration_result) == + rclcpp::FutureReturnCode::SUCCESS) { + success = calibration_result.get()->success; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calibration result: %d", + success); + // return; + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Failed to call service calibration"); + rclcpp::shutdown(); + } + + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to calibrate!"); + rclcpp::shutdown(); + } + while (!impedance_client_->wait_for_service( std::literals::chrono_literals::operator""s(1))) { if (!rclcpp::ok()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Interrupted while waiting for the service. Exiting."); + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Interrupted while waiting for the impedance service. Exiting."); return; } RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "service not available, waiting again..."); + "Impedance service not available, waiting again..."); } - auto result = impedance_client_->async_send_request(imp); + auto impedance_result = impedance_client_->async_send_request(imp); // Wait for the result. if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), - result) == + impedance_result) == rclcpp::FutureReturnCode::SUCCESS) { // Store client_ ID given by virtuose_node - client__id_ = result.get()->client_id; + client__id_ = impedance_result.get()->client_id; RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client_ ID is: %d", client__id_); } else { @@ -181,6 +221,8 @@ class SystemInterface : public rclcpp::Node { pose_subscriber_; rclcpp::Client::SharedPtr impedance_client_; + rclcpp::Client::SharedPtr + calibration_client_; rclcpp::TimerBase::SharedPtr set_target_wrench_timer_; }; diff --git a/src/vf_control/include/utils.hpp b/src/haptic_control/include/utils.hpp similarity index 100% rename from src/vf_control/include/utils.hpp rename to src/haptic_control/include/utils.hpp diff --git a/src/haptic_control/launch/auto_calibration.launch.py b/src/haptic_control/launch/auto_calibration.launch.py index 95c1d6d..559fa04 100644 --- a/src/haptic_control/launch/auto_calibration.launch.py +++ b/src/haptic_control/launch/auto_calibration.launch.py @@ -1,15 +1,9 @@ from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription, OpaqueFunction -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_context import LaunchContext from launch_ros.parameter_descriptions import ParameterFile -from launch.actions import TimerAction, DeclareLaunchArgument +from launch.actions import TimerAction # get package share directory from ament_index_python.packages import get_package_share_directory -import time -import sys diff --git a/src/haptic_control/launch/haptic_control.launch.py b/src/haptic_control/launch/haptic_control.launch.py old mode 100755 new mode 100644 index 2998f35..ff9afb5 --- a/src/haptic_control/launch/haptic_control.launch.py +++ b/src/haptic_control/launch/haptic_control.launch.py @@ -1,130 +1,84 @@ from launch import LaunchDescription from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration from launch_ros.parameter_descriptions import ParameterFile -from launch.actions import TimerAction +from launch.actions import TimerAction, DeclareLaunchArgument from ament_index_python.packages import get_package_share_directory +from launch.actions import DeclareLaunchArgument -import time -import sys +# PythonExpression +from launch.conditions import LaunchConfigurationEquals - - -#################### NODE FOR CALLING CALIBRATION HAPTIC DEVICE ################# -import rclpy -from rclpy.node import Node as RCLNode -from rokubimini_msgs.srv import ResetWrench - -class FTReset(RCLNode): - def __init__(self): - super().__init__('reset_wrench') - self.cli = self.create_client(ResetWrench, '/bus0/ft_sensor0/reset_wrench') - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - self.req = ResetWrench.Request() - - def send_request(self): - self.req.desired_wrench.force.x = 0.0 - self.req.desired_wrench.force.y = 0.0 - self.req.desired_wrench.force.z = 0.0 - self.req.desired_wrench.torque.x = 0.0 - self.req.desired_wrench.torque.y = 0.0 - self.req.desired_wrench.torque.z = 0.0 - self.future = self.cli.call_async(self.req) - rclpy.spin_until_future_complete(self, self.future) - return self.future.result() - -################################################################################ - - - -################################################################################ -######################## LAUNCH DESCRIPTION #################################### -################################################################################ - def generate_launch_description(): - - ld = LaunchDescription() - print("\033[92m"+ - " _____ _ _ \n"+ - " | __ \ | | | | \n"+ - " | |__) |___ __ _ __| |_ _ | |_ ___ \n"+ - " | _ // _ \/ _` |/ _` | | | | | __/ _ \ \n"+ - " | | \ \ __/ (_| | (_| | |_| | | || (_) | \n"+ - " |_| \_\___|\__,_|\__,_|\__, | \__\___/ _ \n"+ - " | | | | __/ | | | \n"+ - " | |_ ___| | ___ ___ _ |___/___ _ __ __ _| |_ ___ \n"+ - " | __/ _ \ |/ _ \/ _ \| '_ \ / _ \ '__/ _` | __/ _ \ \n"+ - " | || __/ | __/ (_) | |_) | __/ | | (_| | || __/\n"+ - " \__\___|_|\___|\___/| .__/ \___|_| \__,_|\__\___|\n" - " | | \n"+ - " |_| \n\033[00m") - - - ############################## HAPTIC DEVICE CALIBRATION ################### - print("\n\n\n\033[93m WARNING: ROBOT MUST NOT BE IN CONTACT WITH ANYTHING DURING CALIBRATION\033[00m\n\n") - # time.sleep(3) - print("\n\n\033[92mRESETTING FORCE SENSOR...\033[00m\n") - rclpy.init() - reset_wrench_node = FTReset() - response = reset_wrench_node.send_request() - reset_wrench_node.get_logger().info('\n\n'+str(response)) - print("Calibration result: "+str(response)) - while 'success=True' not in str(response): - print("\nERROR: Force sensor not reset, trying again...\n") - response = reset_wrench_node.send_request() - reset_wrench_node.get_logger().info('\n\n'+str(response)) - reset_wrench_node.destroy_node() - rclpy.shutdown() + ld = LaunchDescription() - print("\n\n\033[91mREMEMBER TO ACTIVATE FORCE FEEDBACK BUTTON ON HAPTIC DEVICE\033[00m\n\n") + # LOGINFO + ld.add_action( + DeclareLaunchArgument( + "use_fixtures", + default_value="false", + description="Use this argument to activate the fixtures (true or false)", + ) + ) + ld.add_action( + DeclareLaunchArgument( + "delay", + default_value="0.0", + description="Use this argument to simulate a delay in the system (in seconds), use 0.0 for no delay", + ) + ) - ############################## HAPTIC DEVICE CONTROL NODE ################## # haptic_wrapper haptic_wrapper = TimerAction( - period = 2.0, - actions = [Node( - package="haption_raptor_api", - executable="raptor_api_wrapper" - )] - - ) - - haptic_parameters_calibration = get_package_share_directory('test_calibration') + "/config/parameters.yaml" - print("CALIBRATION FILE: ",haptic_parameters_calibration) - - # CALIBRATION NODE - haptic_calibration_node = TimerAction( - period = 2.0, - actions = [Node( - package="test_calibration", - executable="test_calibration", - parameters=[ParameterFile(haptic_parameters_calibration)] - )] + period=0.0, + actions=[Node(package="haption_raptor_api", + executable="raptor_api_wrapper", + # prefix=['xterm -fa "Monospace" -fs 14 -e gdb -ex run --args'] + ) + ], ) # SET RIGHT PATH TO YAML - haptic_parameters_control = get_package_share_directory('haptic_control') + "/config/parameters.yaml" - - haptic_control_node = TimerAction( - period=4.0, + params = get_package_share_directory("haptic_control") + "/config/parameters.yaml" + vf_node = TimerAction( + period=2.5, actions=[ Node( package="haptic_control", executable="haptic_control", # remappings=[('/target_frame', '/target_frame_haptic')], parameters=[ - ParameterFile(haptic_parameters_control) - ] + ParameterFile(params), + {"use_fixtures": LaunchConfiguration("use_fixtures")}, + {"delay": LaunchConfiguration("delay")}, + ], + remappings=[ + ( + + # remember to change ft_sensor_rate in the yaml file + "bus0/ft_sensor0/ft_sensor_readings/wrench", + "/force_torque_sensor_broadcaster/wrench", + ) + ], + # prefix=["xterm -hold -fa 'Monospace' -fs 14 -e "], + # output='screen', + # emulate_tty=True, + # arguments=[('__log_level:=debug')] ) - ] + ], + ) + # load rviz if use_fixtures is true using ifcondition + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", get_package_share_directory("haptic_control") + "/rviz/vf.rviz"], + condition=LaunchConfigurationEquals("use_fixtures", "true"), ) - - ld.add_action(haptic_wrapper) - ld.add_action(haptic_calibration_node) - ld.add_action(haptic_control_node) + ld.add_action(vf_node) + ld.add_action(rviz) return ld - - diff --git a/src/haptic_control/launch/sample_teleoperation.launch.py b/src/haptic_control/launch/sample_teleoperation.launch.py new file mode 100644 index 0000000..c5f8a5d --- /dev/null +++ b/src/haptic_control/launch/sample_teleoperation.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile +from launch.actions import TimerAction + +# get package share directory +from ament_index_python.packages import get_package_share_directory +from launch.actions import DeclareLaunchArgument + +# PythonExpression +from launch.conditions import LaunchConfigurationEquals +from launch.actions import LogInfo + + +def generate_launch_description(): + + ld = LaunchDescription() + + # haptic_wrapper + haptic_wrapper = TimerAction( + period=0.0, + actions=[Node(package="haption_raptor_api", + executable="raptor_api_wrapper", + # prefix=['xterm -fa "Monospace" -fs 14 -e gdb -ex run --args'] + ) + ], + ) + + # SET RIGHT PATH TO YAML + params = get_package_share_directory("haptic_control") + "/config/parameters.yaml" + + sample_teleoperation = TimerAction( + period=1.0, + actions=[ + Node( + package="haptic_control", + executable="sample_teleoperation", + # remappings=[('/target_frame', '/target_frame_haptic')], + parameters=[ + ParameterFile(params), + ], + remappings=[ + ( + # remember to change ft_sensor_rate in the yaml file + "bus0/ft_sensor0/ft_sensor_readings/wrench", # 1 kHz + "/force_torque_sensor_broadcaster/wrench", # 0.5 kHz + ) + ], + ) + ], + ) + ld.add_action(haptic_wrapper) + ld.add_action(sample_teleoperation) + return ld diff --git a/src/haptic_control/launch/sim_haptic_control.launch.py b/src/haptic_control/launch/sim_haptic_control.launch.py deleted file mode 100755 index 4f3c2d3..0000000 --- a/src/haptic_control/launch/sim_haptic_control.launch.py +++ /dev/null @@ -1,97 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription, OpaqueFunction -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_context import LaunchContext -from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterFile -from launch.actions import TimerAction, DeclareLaunchArgument -import time -import sys - - -# THIS IS USED FOR THE COPPELIASIM SIMULATION - - -################################################################################ -######################## LAUNCH DESCRIPTION #################################### -################################################################################ - -def generate_launch_description(): - - ld = LaunchDescription() - - print("\033[92m"+ - " _____ _ _ \n"+ - " | __ \ | | | | \n"+ - " | |__) |___ __ _ __| |_ _ | |_ ___ \n"+ - " | _ // _ \/ _` |/ _` | | | | | __/ _ \ \n"+ - " | | \ \ __/ (_| | (_| | |_| | | || (_) | \n"+ - " |_| \_\___|\__,_|\__,_|\__, | \__\___/ _ \n"+ - " | | | | __/ | | | \n"+ - " | |_ ___| | ___ ___ _ |___/___ _ __ __ _| |_ ___ \n"+ - " | __/ _ \ |/ _ \/ _ \| '_ \ / _ \ '__/ _` | __/ _ \ \n"+ - " | || __/ | __/ (_) | |_) | __/ | | (_| | || __/\n"+ - " \__\___|_|\___|\___/| .__/ \___|_| \__,_|\__\___|\n" - " | | \n"+ - " |_| \n\033[00m") - - # time.sleep(1) - # Get path to haption_ws from command line - for arg in sys.argv: - if arg.startswith("haption_ws_path:="): - haption_ws_path = str(arg.split(":=")[1]) - - - - - print("\n\n\033[91mREMEMBER TO ACTIVATE FORCE FEEDBACK BUTTON ON HAPTIC DEVICE\033[00m\n\n") - time.sleep(1) - - ############################## HAPTIC DEVICE CONTROL NODE ################## - # haptic_wrapper - haptic_wrapper = TimerAction( - period = 2.0, - actions = [Node( - package="haption_raptor_api", - executable="raptor_api_wrapper" - )] - - ) - - haptic_parameters_calibration = haption_ws_path + "src/test_calibration/parameters.yaml" - - - # CALIBRATION NODE - haptic_calibration_node = Node( - package="test_calibration", - executable="test_calibration", - parameters=[ - ParameterFile(haptic_parameters_calibration) - ] - ) - - # SET RIGHT PATH TO YAML - haptic_parameters_control = haption_ws_path + "src/haptic_control/parameters.yaml" - - haptic_control_node = TimerAction( - period=6.0, - actions=[ - Node( - package="haptic_control", - executable="haptic_control", - parameters=[ - ParameterFile(haptic_parameters_control) - ] - ) - ] - ) - - - ld.add_action(haptic_wrapper) - ld.add_action(haptic_calibration_node) - ld.add_action(haptic_control_node) - return ld - - diff --git a/src/vf_control/launch/test_vf.launch.py b/src/haptic_control/launch/test_vf.launch.py similarity index 100% rename from src/vf_control/launch/test_vf.launch.py rename to src/haptic_control/launch/test_vf.launch.py diff --git a/src/haptic_control/package.xml b/src/haptic_control/package.xml index f6e72eb..f9e23a5 100755 --- a/src/haptic_control/package.xml +++ b/src/haptic_control/package.xml @@ -34,6 +34,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest haption_raptor_api raptor_api_interfaces diff --git a/src/vf_control/rviz/vf.rviz b/src/haptic_control/rviz/vf.rviz similarity index 87% rename from src/vf_control/rviz/vf.rviz rename to src/haptic_control/rviz/vf.rviz index 9200ddd..f359cdc 100644 --- a/src/vf_control/rviz/vf.rviz +++ b/src/haptic_control/rviz/vf.rviz @@ -1,6 +1,6 @@ Panels: - Class: rviz_common/Displays - Help Height: 85 + Help Height: 172 Name: Displays Property Tree Widget: Expanded: @@ -9,7 +9,7 @@ Panels: - /MarkerArray1 - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 1250 + Tree Height: 220 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -53,7 +53,7 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: - patient: true + mesh: true target_pose: true virtual_fixture: true Topic: @@ -135,33 +135,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 0.1727123260498047 + Distance: 1.0169681310653687 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.03981896489858627 - Y: -0.03444981575012207 - Z: 0.5195462107658386 + X: 0.047765351831912994 + Y: 0.21256861090660095 + Z: 0.001814984600059688 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.44979748129844666 + Pitch: 0.40479737520217896 Target Frame: Value: Orbit (rviz) - Yaw: 1.535398244857788 + Yaw: 1.905397891998291 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1561 + Height: 2691 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000575fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e00000575000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000579fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000579000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000034d00000042fc0100000002fb0000000800540069006d006501000000000000034d000002a600fffffffb0000000800540069006d006501000000000000045000000000000000000000034d0000057500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000575fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000da00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e00000575000001de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000579fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000005790000017800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000de00000006afc0100000002fb0000000800540069006d0065010000000000000de0000006ee00fffffffb0000000800540069006d0065010000000000000450000000000000000000000de00000094e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -170,6 +170,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 845 + Width: 3552 X: 222 - Y: 5 + Y: 68 diff --git a/src/vf_control/src/haptic_control_base.cpp b/src/haptic_control/src/haptic_control_base.cpp similarity index 98% rename from src/vf_control/src/haptic_control_base.cpp rename to src/haptic_control/src/haptic_control_base.cpp index 9e33409..8d39275 100644 --- a/src/vf_control/src/haptic_control_base.cpp +++ b/src/haptic_control/src/haptic_control_base.cpp @@ -1,3 +1,9 @@ +/* +Teleoperation node using haptic device and force feedback which implements the +mesh virtual fixtures explained in the paper +https://ieeexplore.ieee.org/document/9341590/ +@Author: Davide Nardi +*/ #include "haptic_control_base.hpp" using namespace Eigen; diff --git a/src/vf_control/src/mesh_test.cpp b/src/haptic_control/src/mesh_test.cpp similarity index 99% rename from src/vf_control/src/mesh_test.cpp rename to src/haptic_control/src/mesh_test.cpp index 57b8f63..1ed8873 100644 --- a/src/vf_control/src/mesh_test.cpp +++ b/src/haptic_control/src/mesh_test.cpp @@ -4,8 +4,8 @@ #include #include "open3d/Open3D.h" -#include "vf/location.hpp" -#include "vf/mesh.hpp" +#include "mesh_virtual_fixtures/location.hpp" +#include "mesh_virtual_fixtures/mesh.hpp" void testFindNearest() { auto o3d_mesh = open3d::geometry::TriangleMesh::CreateSphere(0.2, 40); diff --git a/src/haptic_control/src/haptic_control.cpp b/src/haptic_control/src/sample_teleoperation.cpp similarity index 89% rename from src/haptic_control/src/haptic_control.cpp rename to src/haptic_control/src/sample_teleoperation.cpp index 324bd67..0ec07c1 100644 --- a/src/haptic_control/src/haptic_control.cpp +++ b/src/haptic_control/src/sample_teleoperation.cpp @@ -1,505 +1,550 @@ -#include "haptic_control.hpp" - -using namespace Eigen; - -using std::placeholders::_1; -using std::placeholders::_2; -using namespace std::chrono_literals; - -HapticControl::HapticControl(const std::string &name, - const std::string &namespace_, - const rclcpp::NodeOptions &options) - : Node(name, namespace_, options) { - // safety sphere around robot base link to prevent singularity - this->enable_safety_sphere_ = - this->get_parameter("enable_safety_sphere").as_bool(); - if (this->enable_safety_sphere_) { - this->safety_sphere_radius_ = - this->get_parameter("safety_sphere_radius").as_double(); - this->safety_sphere_radius_ = - std::clamp(this->safety_sphere_radius_, 0.0, 2.0); - } else { - this->safety_sphere_radius_ = std::numeric_limits::infinity(); - } - // safety box dimension - this->enable_safety_box_ = this->get_parameter("enable_safety_box").as_bool(); - if (this->enable_safety_box_) { - this->safety_box_width_ = - this->get_parameter("safety_box_width").as_double(); // x - this->safety_box_length_ = - this->get_parameter("safety_box_length").as_double(); // y - this->safety_box_height_ = - this->get_parameter("safety_box_height").as_double(); // z - } else { - this->safety_box_width_ = std::numeric_limits::infinity(); - this->safety_box_length_ = std::numeric_limits::infinity(); - this->safety_box_height_ = std::numeric_limits::infinity(); - } - - this->get_parameter("max_force_", max_force_); - // safety XYZ position zone -> read from config file - this->force_scale_ = this->get_parameter("force_scale").as_double(); - this->tool_link_name_ = this->get_parameter("tool_link_name").as_string(); - this->base_link_name_ = this->get_parameter("base_link_name").as_string(); - this->ft_link_name_ = this->get_parameter("ft_link_name").as_string(); - - // Create a parameter subscriber that can be used to monitor parameter changes - param_subscriber_ = std::make_shared(this); - this->get_parameter("max_force_", max_force_); - - // init wrench msg - current_wrench_.header.frame_id = base_link_name_; - current_wrench_.wrench.force.x = 0.0; - current_wrench_.wrench.force.y = 0.0; - current_wrench_.wrench.force.z = 0.0; - current_wrench_.wrench.torque.x = 0.0; - current_wrench_.wrench.torque.y = 0.0; - current_wrench_.wrench.torque.z = 0.0; - - // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Preparing publishers"); - out_virtuose_status_ = - this->create_subscription( - "out_virtuose_status", 1, - std::bind(&HapticControl::out_virtuose_statusCB, this, _1)); - _out_virtuose_pose_ = - this->create_subscription( - "out_virtuose_pose", 1, - std::bind(&HapticControl::out_virtuose_pose_CB, this, _1)); - - target_pos_publisher_ = - this->create_publisher("/target_frame", - 1); - current_target_pos_publisher_ = - this->create_publisher( - "/target_frame_vf", 1); - - _in_virtuose_force = - this->create_publisher( - "in_virtuose_force", 1); - // create force/wrench subscriber - ft_subscriber_ = this->create_subscription( - "bus0/ft_sensor0/ft_sensor_readings/wrench", 1, - std::bind(&HapticControl::SetWrenchCB, this, _1)); - - client_ = this->create_client( - "virtuose_impedance"); - - // Set a callback for parameters updates - // Safety sphere - cb_enable_safety_sphere_ = param_subscriber_->add_parameter_callback( - "enable_safety_sphere", std::bind(&HapticControl::enable_safety_sphere_CB, - this, std::placeholders::_1)); - cb_safety_sphere_radius_ = param_subscriber_->add_parameter_callback( - "safety_sphere_radius", - std::bind(&HapticControl::set_safety_sphere_radius_CB, this, - std::placeholders::_1)); - // Safety box - cb_enable_safety_box_ = param_subscriber_->add_parameter_callback( - "enable_safety_box", std::bind(&HapticControl::enable_safety_box_CB, this, - std::placeholders::_1)); - cb_safety_box_width_ = param_subscriber_->add_parameter_callback( - "safety_box_width", std::bind(&HapticControl::set_safety_box_width_CB, - this, std::placeholders::_1)); - cb_safety_box_length_ = param_subscriber_->add_parameter_callback( - "safety_box_length", std::bind(&HapticControl::set_safety_box_length_CB, - this, std::placeholders::_1)); - cb_safety_box_height_ = param_subscriber_->add_parameter_callback( - "safety_box_height", std::bind(&HapticControl::set_safety_box_height_CB, - this, std::placeholders::_1)); - - received_haptic_pose_ = false; - - // Initializes the TF2 transform listener and buffer - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_shared(this); - - // defines the rotation from the robot base frame to the haptic base frame - Eigen::Quaterniond tmp(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); - q_haptic_base_to_robot_base_.x() = tmp.x(); - q_haptic_base_to_robot_base_.y() = tmp.y(); - q_haptic_base_to_robot_base_.z() = tmp.z(); - q_haptic_base_to_robot_base_.w() = tmp.w(); - - x_new_ << 0.0, 0.0, 0.0; - x_old_ << 0.0, 0.0, 0.0; - x_tilde_new_ << 0.0, 0.0, 0.0; - x_tilde_old_ << 0.0, 0.0, 0.0; -} - -void HapticControl::enable_safety_sphere_CB(const rclcpp::Parameter &p) { - RCLCPP_INFO(this->get_logger(), "Toggled Safety sphere"); - enable_safety_sphere_ = (bool)p.as_int(); -} -void HapticControl::set_safety_sphere_radius_CB(const rclcpp::Parameter &p) { - RCLCPP_INFO(this->get_logger(), - "Received an update to the safety sphere radius"); - safety_sphere_radius_ = std::clamp(p.as_double(), 0.0, 2.0); -} -void HapticControl::enable_safety_box_CB(const rclcpp::Parameter &p) { - RCLCPP_INFO(this->get_logger(), "Toggled Safety box"); - enable_safety_box_ = (bool)p.as_int(); -} -void HapticControl::set_safety_box_width_CB(const rclcpp::Parameter &p) { - RCLCPP_INFO(this->get_logger(), "Received an update to the safety box width"); - safety_box_width_ = std::clamp(p.as_double(), 0.0, 2.0); -} -void HapticControl::set_safety_box_length_CB(const rclcpp::Parameter &p) { - RCLCPP_INFO(this->get_logger(), - "Received an update to the safety box length"); - safety_box_length_ = std::clamp(p.as_double(), 0.0, 2.0); -} -void HapticControl::set_safety_box_height_CB(const rclcpp::Parameter &p) { - RCLCPP_INFO(this->get_logger(), - "Received an update to the safety box height"); - safety_box_height_ = std::clamp(p.as_double(), 0.0, 2.0); -} - -void HapticControl::SetWrenchCB( - const geometry_msgs::msg::WrenchStamped target_wrench) { - current_wrench_.header.stamp = target_wrench.header.stamp; - geometry_msgs::msg::WrenchStamped force; - force.header.stamp = target_wrench.header.stamp; - force.wrench.force.x = target_wrench.wrench.force.x; - force.wrench.force.y = target_wrench.wrench.force.y; - force.wrench.force.z = target_wrench.wrench.force.z; - force.wrench.torque.x = target_wrench.wrench.torque.x; - force.wrench.torque.y = target_wrench.wrench.torque.y; - force.wrench.torque.z = target_wrench.wrench.torque.z; - - // Map forces from probe frame to haptic base frame (since haptic base frame - // coincides with robot base frame) - try { - auto trans = tf_buffer_->lookupTransform(base_link_name_, ft_link_name_, - tf2::TimePointZero); - // apply rotation to the force - tf2::doTransform(force, force, trans); - - current_wrench_.wrench.force.x = force.wrench.force.x; - current_wrench_.wrench.force.y = force.wrench.force.y; - current_wrench_.wrench.force.z = force.wrench.force.z; - current_wrench_.wrench.torque.x = force.wrench.torque.x; - current_wrench_.wrench.torque.y = force.wrench.torque.y; - current_wrench_.wrench.torque.z = force.wrench.torque.z; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 200, - "F/T sensor pose transform not available: %s", - ex.what()); - current_wrench_.wrench.force.x = current_wrench_.wrench.force.y = - current_wrench_.wrench.force.z = 0.0; - current_wrench_.wrench.torque.x = current_wrench_.wrench.torque.y = - current_wrench_.wrench.torque.z = 0.0; - return; - } -} -// Callback for topic out_virtuose_pose_ -void HapticControl::out_virtuose_pose_CB( - const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg) { - if (!received_haptic_pose_) { - // Store last pose date - received_haptic_pose_ = true; - haptic_starting_position_.pose.position.x = - msg->virtuose_pose.translation.x; - haptic_starting_position_.pose.position.y = - msg->virtuose_pose.translation.y; - haptic_starting_position_.pose.position.z = - msg->virtuose_pose.translation.z; - haptic_starting_position_.pose.orientation.x = - msg->virtuose_pose.rotation.x; - haptic_starting_position_.pose.orientation.y = - msg->virtuose_pose.rotation.y; - haptic_starting_position_.pose.orientation.z = - msg->virtuose_pose.rotation.z; - haptic_starting_position_.pose.orientation.w = - msg->virtuose_pose.rotation.w; - x_new_ << haptic_starting_position_.pose.position.x, - haptic_starting_position_.pose.position.y, - haptic_starting_position_.pose.position.z; - x_old_ = x_new_; - } - pose_date_nanosec_ = msg->header.stamp.nanosec; - pose_date_sec_ = msg->header.stamp.sec; - cur_pose_[0] = msg->virtuose_pose.translation.x; - cur_pose_[1] = msg->virtuose_pose.translation.y; - cur_pose_[2] = msg->virtuose_pose.translation.z; - cur_pose_[3] = msg->virtuose_pose.rotation.x; - cur_pose_[4] = msg->virtuose_pose.rotation.y; - cur_pose_[5] = msg->virtuose_pose.rotation.z; - cur_pose_[6] = msg->virtuose_pose.rotation.w; - - x_old_ = x_new_; - x_new_ << cur_pose_[0], cur_pose_[1], cur_pose_[2]; - - // ENFORCE SPHERE SAFETY - if (enable_safety_sphere_) { - x_tilde_new_ = x_tilde_old_ + (x_new_ - x_old_); - } else { - x_tilde_new_ = x_new_; - } - - // if (ctr_ % 1000 == 0) - // { - // printf("Virtuose pose: %f %f %f %f %f %f %f\n", cur_pose_[0], cur_pose_[1], - // cur_pose_[2], cur_pose_[3], cur_pose_[4], cur_pose_[5], cur_pose_[6]); - // } -} - -// Callback for topic out_virtuose_status -void HapticControl::out_virtuose_statusCB( - const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg) { - // Store last status date - status_date_nanosec_ = msg->header.stamp.nanosec; - status_date_sec_ = msg->header.stamp.sec; - status_state_ = msg->state; - status_button_ = msg->buttons; -} - -void HapticControl::callImpedanceService() { - received_ee_pose_ = false; - while (!received_ee_pose_) { - try { - auto trans = tf_buffer_->lookupTransform(base_link_name_, tool_link_name_, - tf2::TimePointZero); - ee_starting_position.pose.position.x = trans.transform.translation.x; - ee_starting_position.pose.position.y = trans.transform.translation.y; - ee_starting_position.pose.position.z = trans.transform.translation.z; - ee_starting_position.pose.orientation.x = trans.transform.rotation.x; - ee_starting_position.pose.orientation.y = trans.transform.rotation.y; - ee_starting_position.pose.orientation.z = trans.transform.rotation.z; - ee_starting_position.pose.orientation.w = trans.transform.rotation.w; - received_ee_pose_ = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "End Effector pose not available: %s", ex.what()); - } - } - if (enable_safety_sphere_) { - Eigen::Vector3d ee_start(ee_starting_position.pose.position.x, - ee_starting_position.pose.position.y, - ee_starting_position.pose.position.z); - if (ee_start.norm() > safety_sphere_radius_) { - RCLCPP_ERROR(this->get_logger(), - "ERROR: End effector is outside the safety sphere, please " - "move it inside the safety sphere"); - exit(1); - } - } - RCLCPP_INFO(this->get_logger(), - "\n\nEnd Effector Pose received \nEE starting position is : x: " - "%f | y: %f | z: %f | \nEE starting rotation is : x: %f | y: " - "%f | z: %f | w: %f\n", - ee_starting_position.pose.position.x, - ee_starting_position.pose.position.y, - ee_starting_position.pose.position.z, - ee_starting_position.pose.orientation.x, - ee_starting_position.pose.orientation.y, - ee_starting_position.pose.orientation.z, - ee_starting_position.pose.orientation.w); - - // Request impedance mode - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending impedance request"); - - auto imp = std::make_shared< - raptor_api_interfaces::srv::VirtuoseImpedance::Request>(); - this->get_parameter("channel", imp->channel); - this->get_parameter("ff_device_ip_address", imp->ff_device_ip_address); - this->get_parameter("ff_device_param_file", imp->ff_device_param_file); - this->get_parameter("local_ip_address", imp->local_ip_address); - - imp->base_frame.translation.x = 0.0; - imp->base_frame.translation.y = 0.0; - imp->base_frame.translation.z = 0.0; - - imp->base_frame.rotation.x = q_haptic_base_to_robot_base_.x(); - imp->base_frame.rotation.y = q_haptic_base_to_robot_base_.y(); - imp->base_frame.rotation.z = q_haptic_base_to_robot_base_.z(); - imp->base_frame.rotation.w = q_haptic_base_to_robot_base_.w(); - - while (!client_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Interrupted while waiting for the service. Exiting."); - return; - } - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "service not available, waiting again..."); - } - - auto result = client_->async_send_request(imp); - // Wait for the result. - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), - result) == - rclcpp::FutureReturnCode::SUCCESS) { - // Store client_ ID given by virtuose_node - client__id_ = result.get()->client_id; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client_ ID is: %d", - client__id_); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Failed to call service impedance"); - return; - } - - if (client__id_ == 0) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Failed to call service impedance, client__id_ is zero!"); - return; - } - - ctr_ = 0; - - // Perform impedance loop at 1000 Hz - impedanceThread_ = this->create_wall_timer( - 1ms, std::bind(&HapticControl::impedanceThread, this)); - RCLCPP_INFO(this->get_logger(), "\033[0;32mImpedance thread started\033[0m"); -} - -// This function is called at 1000 Hz -void HapticControl::impedanceThread() { - if (!received_haptic_pose_) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Haptic pose not available"); - return; - } - // Apply the force - raptor_api_interfaces::msg::InVirtuoseForce force; - force.header.stamp.nanosec = get_clock()->now().nanoseconds(); - force.header.stamp.sec = get_clock()->now().seconds(); - force.client_id = client__id_; - // filter noise - float alpha = 0; - force.virtuose_force.force.x = - 0.3 * (alpha * old_force_.virtuose_force.force.x + - (1 - alpha) * current_wrench_.wrench.force.x); - force.virtuose_force.force.y = - 0.3 * (alpha * old_force_.virtuose_force.force.y + - (1 - alpha) * current_wrench_.wrench.force.y); - force.virtuose_force.force.z = - 0.3 * (alpha * old_force_.virtuose_force.force.z + - (1 - alpha) * current_wrench_.wrench.force.z); - // torque omitted for control simplicity - force.virtuose_force.torque.x = - 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.x + (1 - alpha) - // * current_wrench_.wrench.torque.x); - force.virtuose_force.torque.y = - 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.y + (1 - alpha) - // * current_wrench_.wrench.torque.y); - force.virtuose_force.torque.z = - 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.z + (1 - alpha) - // * current_wrench_.wrench.torque.z); - - // SAFE ZONE FORCE - - force.virtuose_force.force.x = - std::clamp(force.virtuose_force.force.x, -6.0, 6.0); - force.virtuose_force.force.y = - std::clamp(force.virtuose_force.force.y, -6.0, 6.0); - force.virtuose_force.force.z = - std::clamp(force.virtuose_force.force.z, -6.0, 6.0); - - // // SAFE ZONE TORQUE - // force.virtuose_force.torque.x = - // std::clamp(force.virtuose_force.torque.x,-0.2,0.2); - // force.virtuose_force.torque.y = - // std::clamp(force.virtuose_force.torque.y,-0.2,0.2); - // force.virtuose_force.torque.z = - // std::clamp(force.virtuose_force.torque.z,-0.2,0.2); - - // updating old force - old_force_.virtuose_force.force.x = force.virtuose_force.force.x; - old_force_.virtuose_force.force.y = force.virtuose_force.force.y; - old_force_.virtuose_force.force.z = force.virtuose_force.force.z; - old_force_.virtuose_force.torque.x = force.virtuose_force.torque.x; - old_force_.virtuose_force.torque.y = force.virtuose_force.torque.y; - old_force_.virtuose_force.torque.z = force.virtuose_force.torque.z; - - _in_virtuose_force->publish(force); - ctr_++; - - // Publish target position - target_pose_.header.stamp = target_pose_tf_.header.stamp = get_clock()->now(); - target_pose_.header.frame_id = target_pose_tf_.header.frame_id = - base_link_name_; - target_pose_tf_.child_frame_id = "haptic_interface_target"; - - // computing error - Eigen::Vector3d error; - error[0] = x_tilde_new_[0] - haptic_starting_position_.pose.position.x; - error[1] = x_tilde_new_[1] - haptic_starting_position_.pose.position.y; - error[2] = x_tilde_new_[2] - haptic_starting_position_.pose.position.z; - - target_pose_.pose.position.x = target_pose_tf_.transform.translation.x = - ee_starting_position.pose.position.x + error(0); - target_pose_.pose.position.y = target_pose_tf_.transform.translation.y = - ee_starting_position.pose.position.y + error(1); - target_pose_.pose.position.z = target_pose_tf_.transform.translation.z = - ee_starting_position.pose.position.z + error(2); - - if (enable_safety_sphere_) { - Eigen::Vector3d target_position_vec(target_pose_.pose.position.x, - target_pose_.pose.position.y, - target_pose_.pose.position.z); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Current distance: %f, safety sphere radius: %f", - target_position_vec.norm(), safety_sphere_radius_); - if (target_position_vec.norm() > safety_sphere_radius_) { - // project the target on the safety sphere to prevent singularities - projectTargetOnSphere(target_position_vec, safety_sphere_radius_); - } else { - // update the old pose only if the target is within the safety sphere - x_tilde_old_ = x_tilde_new_; - } - } - - // eigen quat order is w x y z - Eigen::Quaterniond qStart(haptic_starting_position_.pose.orientation.w, - haptic_starting_position_.pose.orientation.x, - haptic_starting_position_.pose.orientation.y, - haptic_starting_position_.pose.orientation.z); - Eigen::Quaterniond qCur(cur_pose_[6], cur_pose_[3], cur_pose_[4], - cur_pose_[5]); - Eigen::Quaterniond qEEStart(ee_starting_position.pose.orientation.w, - ee_starting_position.pose.orientation.x, - ee_starting_position.pose.orientation.y, - ee_starting_position.pose.orientation.z); - - // computing the difference between the current orientation and the starting - // orientation of the haptic device - Eigen::Quaterniond qDiff = qCur * qStart.conjugate(); - qDiff.normalize(); - - // applying delta rotation to the end effector starting orientation - Eigen::Quaterniond qTarget = qDiff * qEEStart; - qTarget.normalize(); - - target_pose_.pose.orientation.x = target_pose_tf_.transform.rotation.x = - qTarget.x(); - target_pose_.pose.orientation.y = target_pose_tf_.transform.rotation.y = - qTarget.y(); - target_pose_.pose.orientation.z = target_pose_tf_.transform.rotation.z = - qTarget.z(); - target_pose_.pose.orientation.w = target_pose_tf_.transform.rotation.w = - qTarget.w(); - - // send trasnform and publish target pose - tf_broadcaster_->sendTransform(target_pose_tf_); - target_pos_publisher_->publish(target_pose_); -} -void HapticControl::projectTargetOnSphere(Eigen::Vector3d &target_position_vec, - double safety_sphere_radius_) { - target_position_vec = - target_position_vec.normalized() * safety_sphere_radius_; -} -int main(int argc, char **argv) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting Haptic Control node"); - - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:"); - - node->callImpedanceService(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; +#include "sample_teleoperation.hpp" + +using namespace Eigen; + +using std::placeholders::_1; +using std::placeholders::_2; +using namespace std::chrono_literals; +/* +Sample example of teleoperation using haptic device and force feedback +@Author: Davide Nardi +*/ +HapticControl::HapticControl(const std::string &name, + const std::string &namespace_, + const rclcpp::NodeOptions &options) + : Node(name, namespace_, options) { + // safety sphere around robot base link to prevent singularity + this->enable_safety_sphere_ = + this->get_parameter("enable_safety_sphere").as_bool(); + if (this->enable_safety_sphere_) { + this->safety_sphere_radius_ = + this->get_parameter("safety_sphere_radius").as_double(); + this->safety_sphere_radius_ = + std::clamp(this->safety_sphere_radius_, 0.0, 2.0); + } else { + this->safety_sphere_radius_ = std::numeric_limits::infinity(); + } + // safety box dimension (Todo) + this->enable_safety_box_ = this->get_parameter("enable_safety_box").as_bool(); + if (this->enable_safety_box_) { + this->safety_box_width_ = + this->get_parameter("safety_box_width").as_double(); // x + this->safety_box_length_ = + this->get_parameter("safety_box_length").as_double(); // y + this->safety_box_height_ = + this->get_parameter("safety_box_height").as_double(); // z + } else { + this->safety_box_width_ = std::numeric_limits::infinity(); + this->safety_box_length_ = std::numeric_limits::infinity(); + this->safety_box_height_ = std::numeric_limits::infinity(); + } + + this->get_parameter("max_force", max_force_); + // safety XYZ position zone -> read from config file + this->force_scale_ = this->get_parameter("force_scale").as_double(); + this->tool_link_name_ = this->get_parameter("tool_link_name").as_string(); + this->base_link_name_ = this->get_parameter("base_link_name").as_string(); + this->ft_link_name_ = this->get_parameter("ft_link_name").as_string(); + + // Create a parameter subscriber that can be used to monitor parameter changes + param_subscriber_ = std::make_shared(this); + + // init wrench msg + current_wrench_.header.frame_id = base_link_name_; + current_wrench_.wrench.force.x = 0.0; + current_wrench_.wrench.force.y = 0.0; + current_wrench_.wrench.force.z = 0.0; + current_wrench_.wrench.torque.x = 0.0; + current_wrench_.wrench.torque.y = 0.0; + current_wrench_.wrench.torque.z = 0.0; + + // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Preparing publishers"); + out_virtuose_status_ = + this->create_subscription( + "out_virtuose_status", 1, + std::bind(&HapticControl::out_virtuose_statusCB, this, _1)); + _out_virtuose_pose_ = + this->create_subscription( + "out_virtuose_pose", 1, + std::bind(&HapticControl::out_virtuose_pose_CB, this, _1)); + + target_pos_publisher_ = + this->create_publisher("/target_frame", + 1); + current_target_pos_publisher_ = + this->create_publisher( + "/target_frame_vf", 1); + + _in_virtuose_force = + this->create_publisher( + "in_virtuose_force", 1); + // create force/wrench subscriber + ft_subscriber_ = this->create_subscription( + "bus0/ft_sensor0/ft_sensor_readings/wrench", 1, + std::bind(&HapticControl::SetWrenchCB, this, _1)); + + impedance_client_ = + this->create_client( + "virtuose_impedance"); + calibration_client_ = + this->create_client( + "virtuose_calibrate"); + + // Set a callback for parameters updates + // Safety sphere + cb_enable_safety_sphere_ = param_subscriber_->add_parameter_callback( + "enable_safety_sphere", std::bind(&HapticControl::enable_safety_sphere_CB, + this, std::placeholders::_1)); + cb_safety_sphere_radius_ = param_subscriber_->add_parameter_callback( + "safety_sphere_radius", + std::bind(&HapticControl::set_safety_sphere_radius_CB, this, + std::placeholders::_1)); + // Safety box + cb_enable_safety_box_ = param_subscriber_->add_parameter_callback( + "enable_safety_box", std::bind(&HapticControl::enable_safety_box_CB, this, + std::placeholders::_1)); + cb_safety_box_width_ = param_subscriber_->add_parameter_callback( + "safety_box_width", std::bind(&HapticControl::set_safety_box_width_CB, + this, std::placeholders::_1)); + cb_safety_box_length_ = param_subscriber_->add_parameter_callback( + "safety_box_length", std::bind(&HapticControl::set_safety_box_length_CB, + this, std::placeholders::_1)); + cb_safety_box_height_ = param_subscriber_->add_parameter_callback( + "safety_box_height", std::bind(&HapticControl::set_safety_box_height_CB, + this, std::placeholders::_1)); + + received_haptic_pose_ = false; + + // Initializes the TF2 transform listener and buffer + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_broadcaster_ = std::make_shared(this); + + // defines the rotation from the robot base frame to the haptic base frame + Eigen::Quaterniond tmp(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + q_haptic_base_to_robot_base_.x() = tmp.x(); + q_haptic_base_to_robot_base_.y() = tmp.y(); + q_haptic_base_to_robot_base_.z() = tmp.z(); + q_haptic_base_to_robot_base_.w() = tmp.w(); + + x_new_ << 0.0, 0.0, 0.0; + x_old_ << 0.0, 0.0, 0.0; + x_tilde_new_ << 0.0, 0.0, 0.0; + x_tilde_old_ << 0.0, 0.0, 0.0; +} + +void HapticControl::enable_safety_sphere_CB(const rclcpp::Parameter &p) { + RCLCPP_INFO(this->get_logger(), "Toggled Safety sphere"); + enable_safety_sphere_ = (bool)p.as_int(); +} +void HapticControl::set_safety_sphere_radius_CB(const rclcpp::Parameter &p) { + RCLCPP_INFO(this->get_logger(), + "Received an update to the safety sphere radius"); + safety_sphere_radius_ = std::clamp(p.as_double(), 0.0, 2.0); +} +void HapticControl::enable_safety_box_CB(const rclcpp::Parameter &p) { + RCLCPP_INFO(this->get_logger(), "Toggled Safety box"); + enable_safety_box_ = (bool)p.as_int(); +} +void HapticControl::set_safety_box_width_CB(const rclcpp::Parameter &p) { + RCLCPP_INFO(this->get_logger(), "Received an update to the safety box width"); + safety_box_width_ = std::clamp(p.as_double(), 0.0, 2.0); +} +void HapticControl::set_safety_box_length_CB(const rclcpp::Parameter &p) { + RCLCPP_INFO(this->get_logger(), + "Received an update to the safety box length"); + safety_box_length_ = std::clamp(p.as_double(), 0.0, 2.0); +} +void HapticControl::set_safety_box_height_CB(const rclcpp::Parameter &p) { + RCLCPP_INFO(this->get_logger(), + "Received an update to the safety box height"); + safety_box_height_ = std::clamp(p.as_double(), 0.0, 2.0); +} + +void HapticControl::SetWrenchCB( + const geometry_msgs::msg::WrenchStamped target_wrench) { + current_wrench_.header.stamp = target_wrench.header.stamp; + geometry_msgs::msg::WrenchStamped force; + force.header.stamp = target_wrench.header.stamp; + force.wrench.force.x = target_wrench.wrench.force.x; + force.wrench.force.y = target_wrench.wrench.force.y; + force.wrench.force.z = target_wrench.wrench.force.z; + force.wrench.torque.x = target_wrench.wrench.torque.x; + force.wrench.torque.y = target_wrench.wrench.torque.y; + force.wrench.torque.z = target_wrench.wrench.torque.z; + + // Map forces from probe frame to haptic base frame (since haptic base frame + // coincides with robot base frame) + try { + auto trans = tf_buffer_->lookupTransform(base_link_name_, ft_link_name_, + tf2::TimePointZero); + // apply rotation to the force + tf2::doTransform(force, force, trans); + + current_wrench_.wrench.force.x = force.wrench.force.x; + current_wrench_.wrench.force.y = force.wrench.force.y; + current_wrench_.wrench.force.z = force.wrench.force.z; + current_wrench_.wrench.torque.x = force.wrench.torque.x; + current_wrench_.wrench.torque.y = force.wrench.torque.y; + current_wrench_.wrench.torque.z = force.wrench.torque.z; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 200, + "F/T sensor pose transform not available: %s", + ex.what()); + current_wrench_.wrench.force.x = current_wrench_.wrench.force.y = + current_wrench_.wrench.force.z = 0.0; + current_wrench_.wrench.torque.x = current_wrench_.wrench.torque.y = + current_wrench_.wrench.torque.z = 0.0; + return; + } +} +// Callback for topic out_virtuose_pose_ +void HapticControl::out_virtuose_pose_CB( + const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg) { + if (!received_haptic_pose_) { + // Store last pose date + received_haptic_pose_ = true; + haptic_starting_position_.pose.position.x = + msg->virtuose_pose.translation.x; + haptic_starting_position_.pose.position.y = + msg->virtuose_pose.translation.y; + haptic_starting_position_.pose.position.z = + msg->virtuose_pose.translation.z; + haptic_starting_position_.pose.orientation.x = + msg->virtuose_pose.rotation.x; + haptic_starting_position_.pose.orientation.y = + msg->virtuose_pose.rotation.y; + haptic_starting_position_.pose.orientation.z = + msg->virtuose_pose.rotation.z; + haptic_starting_position_.pose.orientation.w = + msg->virtuose_pose.rotation.w; + x_new_ << haptic_starting_position_.pose.position.x, + haptic_starting_position_.pose.position.y, + haptic_starting_position_.pose.position.z; + x_old_ = x_new_; + } + pose_date_nanosec_ = msg->header.stamp.nanosec; + pose_date_sec_ = msg->header.stamp.sec; + cur_pose_[0] = msg->virtuose_pose.translation.x; + cur_pose_[1] = msg->virtuose_pose.translation.y; + cur_pose_[2] = msg->virtuose_pose.translation.z; + cur_pose_[3] = msg->virtuose_pose.rotation.x; + cur_pose_[4] = msg->virtuose_pose.rotation.y; + cur_pose_[5] = msg->virtuose_pose.rotation.z; + cur_pose_[6] = msg->virtuose_pose.rotation.w; + + x_old_ = x_new_; + x_new_ << cur_pose_[0], cur_pose_[1], cur_pose_[2]; + + // ENFORCE SPHERE SAFETY + if (enable_safety_sphere_) { + x_tilde_new_ = x_tilde_old_ + (x_new_ - x_old_); + } else { + x_tilde_new_ = x_new_; + } + + // if (ctr_ % 1000 == 0) + // { + // printf("Virtuose pose: %f %f %f %f %f %f %f\n", cur_pose_[0], cur_pose_[1], + // cur_pose_[2], cur_pose_[3], cur_pose_[4], cur_pose_[5], cur_pose_[6]); + // } +} + +// Callback for topic out_virtuose_status +void HapticControl::out_virtuose_statusCB( + const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg) { + // Store last status date + status_date_nanosec_ = msg->header.stamp.nanosec; + status_date_sec_ = msg->header.stamp.sec; + status_state_ = msg->state; + status_button_ = msg->buttons; +} + +void HapticControl::callImpedanceService() { + received_ee_pose_ = false; + while (!received_ee_pose_) { + try { + auto trans = tf_buffer_->lookupTransform(base_link_name_, tool_link_name_, + tf2::TimePointZero); + ee_starting_position.pose.position.x = trans.transform.translation.x; + ee_starting_position.pose.position.y = trans.transform.translation.y; + ee_starting_position.pose.position.z = trans.transform.translation.z; + ee_starting_position.pose.orientation.x = trans.transform.rotation.x; + ee_starting_position.pose.orientation.y = trans.transform.rotation.y; + ee_starting_position.pose.orientation.z = trans.transform.rotation.z; + ee_starting_position.pose.orientation.w = trans.transform.rotation.w; + received_ee_pose_ = true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "End Effector pose not available: %s", ex.what()); + } + } + if (enable_safety_sphere_) { + Eigen::Vector3d ee_start(ee_starting_position.pose.position.x, + ee_starting_position.pose.position.y, + ee_starting_position.pose.position.z); + if (ee_start.norm() > safety_sphere_radius_) { + RCLCPP_ERROR(this->get_logger(), + "ERROR: End effector is outside the safety sphere, please " + "move it inside the safety sphere"); + exit(1); + } + } + RCLCPP_INFO(this->get_logger(), + "\n\nEnd Effector Pose received \nEE starting position is : x: " + "%f | y: %f | z: %f | \nEE starting rotation is : x: %f | y: " + "%f | z: %f | w: %f\n", + ee_starting_position.pose.position.x, + ee_starting_position.pose.position.y, + ee_starting_position.pose.position.z, + ee_starting_position.pose.orientation.x, + ee_starting_position.pose.orientation.y, + ee_starting_position.pose.orientation.z, + ee_starting_position.pose.orientation.w); + + // Request impedance mode + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending impedance request"); + + auto imp = std::make_shared< + raptor_api_interfaces::srv::VirtuoseImpedance::Request>(); + this->get_parameter("channel", imp->channel); + this->get_parameter("ff_device_ip_address", imp->ff_device_ip_address); + this->get_parameter("ff_device_param_file", imp->ff_device_param_file); + this->get_parameter("local_ip_address", imp->local_ip_address); + + auto cal = std::make_shared< + raptor_api_interfaces::srv::VirtuoseCalibrate::Request>(); + cal->channel = imp->channel; + cal->ff_device_ip_address = imp->ff_device_ip_address; + cal->ff_device_param_file = imp->ff_device_param_file; + cal->local_ip_address = imp->local_ip_address; + + imp->base_frame.translation.x = 0.0; + imp->base_frame.translation.y = 0.0; + imp->base_frame.translation.z = 0.0; + + imp->base_frame.rotation.x = q_haptic_base_to_robot_base_.x(); + imp->base_frame.rotation.y = q_haptic_base_to_robot_base_.y(); + imp->base_frame.rotation.z = q_haptic_base_to_robot_base_.z(); + imp->base_frame.rotation.w = q_haptic_base_to_robot_base_.w(); + + while (!calibration_client_->wait_for_service( + std::literals::chrono_literals::operator""s(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Interrupted while waiting for the calibration service. Exiting."); + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Calibration service not available, waiting again..."); + } + auto calibration_result = calibration_client_->async_send_request(cal); + bool success = false; + // Wait for the result. + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), + calibration_result) == + rclcpp::FutureReturnCode::SUCCESS) { + success = calibration_result.get()->success; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calibration result: %d", + success); + // return; + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Failed to call service calibration"); + rclcpp::shutdown(); + } + + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to calibrate!"); + rclcpp::shutdown(); + } + + while (!impedance_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "service not available, waiting again..."); + } + + auto result = impedance_client_->async_send_request(imp); + // Wait for the result. + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), + result) == + rclcpp::FutureReturnCode::SUCCESS) { + // Store client_ ID given by virtuose_node + client__id_ = result.get()->client_id; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client_ ID is: %d", + client__id_); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Failed to call service impedance"); + return; + } + + if (client__id_ == 0) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Failed to call service impedance, client__id_ is zero!"); + return; + } + + ctr_ = 0; + + // Perform impedance loop at 1000 Hz + impedanceThread_ = this->create_wall_timer( + 1ms, std::bind(&HapticControl::impedanceThread, this)); + RCLCPP_INFO(this->get_logger(), "\033[0;32mImpedance thread started\033[0m"); +} + +// This function is called at 1000 Hz +void HapticControl::impedanceThread() { + if (!received_haptic_pose_) { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Haptic pose not available"); + return; + } + // Apply the force + raptor_api_interfaces::msg::InVirtuoseForce force; + force.header.stamp.nanosec = get_clock()->now().nanoseconds(); + force.header.stamp.sec = get_clock()->now().seconds(); + force.client_id = client__id_; + // filter noise + float alpha = 0; + force.virtuose_force.force.x = + force_scale_ * (alpha * old_force_.virtuose_force.force.x + + (1 - alpha) * current_wrench_.wrench.force.x); + force.virtuose_force.force.y = + force_scale_ * (alpha * old_force_.virtuose_force.force.y + + (1 - alpha) * current_wrench_.wrench.force.y); + force.virtuose_force.force.z = + force_scale_ * (alpha * old_force_.virtuose_force.force.z + + (1 - alpha) * current_wrench_.wrench.force.z); + // torque omitted for control simplicity + force.virtuose_force.torque.x = + 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.x + (1 - alpha) + // * current_wrench_.wrench.torque.x); + force.virtuose_force.torque.y = + 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.y + (1 - alpha) + // * current_wrench_.wrench.torque.y); + force.virtuose_force.torque.z = + 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.z + (1 - alpha) + // * current_wrench_.wrench.torque.z); + + // SAFE ZONE FORCE + + force.virtuose_force.force.x = + std::clamp(force.virtuose_force.force.x, -max_force_, max_force_); + force.virtuose_force.force.y = + std::clamp(force.virtuose_force.force.y, -max_force_, max_force_); + force.virtuose_force.force.z = + std::clamp(force.virtuose_force.force.z, -max_force_, max_force_); + + // // SAFE ZONE TORQUE + // force.virtuose_force.torque.x = + // std::clamp(force.virtuose_force.torque.x,-0.2,0.2); + // force.virtuose_force.torque.y = + // std::clamp(force.virtuose_force.torque.y,-0.2,0.2); + // force.virtuose_force.torque.z = + // std::clamp(force.virtuose_force.torque.z,-0.2,0.2); + + // updating old force + old_force_.virtuose_force.force.x = force.virtuose_force.force.x; + old_force_.virtuose_force.force.y = force.virtuose_force.force.y; + old_force_.virtuose_force.force.z = force.virtuose_force.force.z; + old_force_.virtuose_force.torque.x = force.virtuose_force.torque.x; + old_force_.virtuose_force.torque.y = force.virtuose_force.torque.y; + old_force_.virtuose_force.torque.z = force.virtuose_force.torque.z; + + _in_virtuose_force->publish(force); + ctr_++; + + // Publish target position + target_pose_.header.stamp = target_pose_tf_.header.stamp = get_clock()->now(); + target_pose_.header.frame_id = target_pose_tf_.header.frame_id = + base_link_name_; + target_pose_tf_.child_frame_id = "haptic_interface_target"; + + // computing error + Eigen::Vector3d error; + error[0] = x_tilde_new_[0] - haptic_starting_position_.pose.position.x; + error[1] = x_tilde_new_[1] - haptic_starting_position_.pose.position.y; + error[2] = x_tilde_new_[2] - haptic_starting_position_.pose.position.z; + + target_pose_.pose.position.x = target_pose_tf_.transform.translation.x = + ee_starting_position.pose.position.x + error(0); + target_pose_.pose.position.y = target_pose_tf_.transform.translation.y = + ee_starting_position.pose.position.y + error(1); + target_pose_.pose.position.z = target_pose_tf_.transform.translation.z = + ee_starting_position.pose.position.z + error(2); + + if (enable_safety_sphere_) { + Eigen::Vector3d target_position_vec(target_pose_.pose.position.x, + target_pose_.pose.position.y, + target_pose_.pose.position.z); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Current distance: %f, safety sphere radius: %f", + target_position_vec.norm(), safety_sphere_radius_); + if (target_position_vec.norm() > safety_sphere_radius_) { + // project the target on the safety sphere to prevent singularities + projectTargetOnSphere(target_position_vec, safety_sphere_radius_); + } else { + // update the old pose only if the target is within the safety sphere + x_tilde_old_ = x_tilde_new_; + } + } + + // eigen quat order is w x y z + Eigen::Quaterniond qStart(haptic_starting_position_.pose.orientation.w, + haptic_starting_position_.pose.orientation.x, + haptic_starting_position_.pose.orientation.y, + haptic_starting_position_.pose.orientation.z); + Eigen::Quaterniond qCur(cur_pose_[6], cur_pose_[3], cur_pose_[4], + cur_pose_[5]); + Eigen::Quaterniond qEEStart(ee_starting_position.pose.orientation.w, + ee_starting_position.pose.orientation.x, + ee_starting_position.pose.orientation.y, + ee_starting_position.pose.orientation.z); + + // computing the difference between the current orientation and the starting + // orientation of the haptic device + Eigen::Quaterniond qDiff = qCur * qStart.conjugate(); + qDiff.normalize(); + + // applying delta rotation to the end effector starting orientation + Eigen::Quaterniond qTarget = qDiff * qEEStart; + qTarget.normalize(); + + target_pose_.pose.orientation.x = target_pose_tf_.transform.rotation.x = + qTarget.x(); + target_pose_.pose.orientation.y = target_pose_tf_.transform.rotation.y = + qTarget.y(); + target_pose_.pose.orientation.z = target_pose_tf_.transform.rotation.z = + qTarget.z(); + target_pose_.pose.orientation.w = target_pose_tf_.transform.rotation.w = + qTarget.w(); + + // send trasnform and publish target pose + tf_broadcaster_->sendTransform(target_pose_tf_); + target_pos_publisher_->publish(target_pose_); +} +void HapticControl::projectTargetOnSphere(Eigen::Vector3d &target_position_vec, + double safety_sphere_radius_) { + target_position_vec = + target_position_vec.normalized() * safety_sphere_radius_; +} +int main(int argc, char **argv) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting Haptic Control node"); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:"); + + node->callImpedanceService(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; } \ No newline at end of file diff --git a/src/vf_control/test/vf_test.cpp b/src/haptic_control/test/vf_test.cpp similarity index 99% rename from src/vf_control/test/vf_test.cpp rename to src/haptic_control/test/vf_test.cpp index 6445759..d6d7a89 100644 --- a/src/vf_control/test/vf_test.cpp +++ b/src/haptic_control/test/vf_test.cpp @@ -5,8 +5,8 @@ #include #include "open3d/Open3D.h" -#include "vf/location.hpp" -#include "vf/mesh.hpp" +#include "mesh_virtual_fixtures/location.hpp" +#include "mesh_virtual_fixtures/mesh.hpp" TEST(vf_control, testFindNearest) { auto o3d_mesh = open3d::geometry::TriangleMesh::CreateSphere(0.2, 40); diff --git a/src/vf_control/CMakeLists.txt b/src/vf_control/CMakeLists.txt deleted file mode 100755 index 77450c7..0000000 --- a/src/vf_control/CMakeLists.txt +++ /dev/null @@ -1,72 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(vf_control) - - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_BUILD_TYPE Release) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -fopenmp) -endif() -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(raptor_api_interfaces REQUIRED) -find_package(haption_raptor_api REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(Open3D REQUIRED) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(vf_test test/vf_test.cpp) - target_include_directories(vf_test PUBLIC - $ - $ - ) - target_link_libraries(vf_test Eigen3::Eigen Open3D::Open3D qpoases::qpoases) -endif() - -set (QPOASES_INSTALL_DIR ${CMAKE_BINARY_DIR}/qpoases_install) -include(ExternalProject) -ExternalProject_Add( - external_qpoases - URL https://github.com/coin-or/qpOASES/archive/refs/tags/releases/3.2.1.tar.gz - CMAKE_ARGS - -DCMAKE_INSTALL_PREFIX=${QPOASES_INSTALL_DIR} - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} - -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} - -DBUILD_SHARED_LIBS=OFF - -DQPOASES_BUILD_EXAMPLES=OFF -) -add_library(qpoases_helper INTERFACE) -add_dependencies(qpoases_helper external_qpoases) -target_include_directories(qpoases_helper INTERFACE ${QPOASES_INSTALL_DIR}/include) -target_link_libraries(qpoases_helper INTERFACE ${QPOASES_INSTALL_DIR}/lib/libqpOASES.a) -add_library(qpoases::qpoases ALIAS qpoases_helper) - - -add_executable(vf_control src/haptic_control_base.cpp) -ament_target_dependencies(vf_control rclcpp raptor_api_interfaces haption_raptor_api tf2_ros tf2_geometry_msgs visualization_msgs) -include_directories(include) -target_link_libraries(vf_control Eigen3::Eigen Open3D::Open3D qpoases::qpoases gomp) - -install(TARGETS ${This} vf_control - DESTINATION lib/${PROJECT_NAME}) - -install( - DIRECTORY launch config rviz - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/src/vf_control/config/parameters.yaml b/src/vf_control/config/parameters.yaml deleted file mode 100755 index 457a5b5..0000000 --- a/src/vf_control/config/parameters.yaml +++ /dev/null @@ -1,49 +0,0 @@ - -haptic_control: - ros__parameters: - # vf params - - channel: "SimpleChannelUDP" - ff_device_ip_address: "192.168.100.53" - ff_device_param_file: "/etc/Haption/Connector/desktop_6D_n65.param" - local_ip_address: "192.168.100.50" - haptic_control_rate: 1000.0 - ft_sensor_rate: 500.0 - # vf params - - enable_safety_sphere: false - safety_sphere_radius: 0.48 - use_limits: false - min_x: -100.0 - max_x: 100.0 - min_y: -100.0 - max_y: 100.0 - min_z: -100.0 - max_z: 100.0 - # min_x_: 0.33 - # max_x_: 0.48 - # min_y_: -0.03 - # max_y_: 0.18 - # min_z_: -0.10 - # max_z_: 0.13 - - tool_link_name: "probe" - base_link_name: "base_link" - ft_link_name: "tool0" - delay: 0.0 # round trip simulated delay in seconds (0.0 for no delay) - force_scale : 0.3 - # safety box that can be activated (centered in the current ee position) - enable_safety_box: false - safety_box_length: 0.2 - safety_box_width: 0.2 - safety_box_height: 0.2 - -# vf parameters: -# ros__parameters: - mesh_type: "sphere" # knot, sphere, file, bunny - input_mesh_path: "/home/zotac01/Ultrasound-Demo/body_modelling/ros2_ws/final_vf.obj" - skin_mesh_path: "/home/zotac01/Ultrasound-Demo/body_modelling/ros2_ws/skin_mesh.obj" - output_mesh_path : "/home/zotac01/virtual_fixture_vis.obj" - radius: 0.005 - lookup_area: 0.006 # should be at least 2*radius - plane_size: 0.001 # plane size used to visualize active constraints in rviz \ No newline at end of file diff --git a/src/vf_control/launch/vf.launch.py b/src/vf_control/launch/vf.launch.py deleted file mode 100644 index 0196389..0000000 --- a/src/vf_control/launch/vf.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription, OpaqueFunction -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_context import LaunchContext -from launch_ros.parameter_descriptions import ParameterFile -from launch.actions import TimerAction, DeclareLaunchArgument - -# get package share directory -from ament_index_python.packages import get_package_share_directory -from launch.actions import DeclareLaunchArgument - -# PythonExpression -from launch.conditions import LaunchConfigurationEquals -from launch.actions import LogInfo -import time -import sys - - -def generate_launch_description(): - - ld = LaunchDescription() - - # LOGINFO - ld.add_action( - DeclareLaunchArgument( - "use_fixtures", - default_value="false", - description="Use this argument to activate the fixtures (true or false)", - ) - ) - ld.add_action( - DeclareLaunchArgument( - "delay", - default_value="0.0", - description="Use this argument to simulate a delay in the system (in seconds), use 0.0 for no delay", - ) - ) - - # haptic_wrapper - haptic_wrapper = TimerAction( - period=0.0, - actions=[Node(package="haption_raptor_api", - executable="raptor_api_wrapper", - # prefix=['xterm -fa "Monospace" -fs 14 -e gdb -ex run --args'] - ) - ], - ) - - haptic_parameters_calibration = ( - get_package_share_directory("test_calibration") + "/config/parameters.yaml" - ) - - # CALIBRATION NODE - haptic_calibration_node = TimerAction( - period=0.0, - actions=[ - Node( - package="test_calibration", - executable="test_calibration", - parameters=[ParameterFile(haptic_parameters_calibration)], - ) - ], - ) - - # SET RIGHT PATH TO YAML - vf_params = get_package_share_directory("vf_control") + "/config/parameters.yaml" - vf_node = TimerAction( - period=2.5, - actions=[ - Node( - package="vf_control", - executable="vf_control", - # remappings=[('/target_frame', '/target_frame_haptic')], - parameters=[ - ParameterFile(vf_params), - {"use_fixtures": LaunchConfiguration("use_fixtures")}, - {"delay": LaunchConfiguration("delay")}, - ], - remappings=[ - ( - - # remember to change ft_sensor_rate in the yaml file - "bus0/ft_sensor0/ft_sensor_readings/wrench", - "/force_torque_sensor_broadcaster/wrench", - ) - ], - # prefix=["xterm -hold -fa 'Monospace' -fs 14 -e "], - # output='screen', - # emulate_tty=True, - # arguments=[('__log_level:=debug')] - ) - ], - ) - # load rviz if use_fixtures is true using ifcondition - rviz = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", get_package_share_directory("vf_control") + "/rviz/vf.rviz"], - condition=LaunchConfigurationEquals("use_fixtures", "true"), - ) - ld.add_action(haptic_wrapper) - ld.add_action(haptic_calibration_node) - ld.add_action(vf_node) - ld.add_action(rviz) - return ld diff --git a/src/vf_control/package.xml b/src/vf_control/package.xml deleted file mode 100755 index 3653945..0000000 --- a/src/vf_control/package.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - vf_control - 0.0.0 - The position_control package - - - - - haption - - - - - - TODO - - - - - - - - - - - - - Davide Nardi - - - ament_cmake - - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - haption_raptor_api - raptor_api_interfaces - - - - - - - ament_cmake - - diff --git a/start_RaptorAPIWrapper.sh b/start_RaptorAPIWrapper.sh deleted file mode 100755 index 9b6764f..0000000 --- a/start_RaptorAPIWrapper.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash -colcon build --packages-select haption_raptor_api -. install/setup.bash -ros2 run haption_raptor_api raptor_api_wrapper diff --git a/start_TestAdmittance.sh b/start_TestAdmittance.sh deleted file mode 100755 index 143195e..0000000 --- a/start_TestAdmittance.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash -colcon build --packages-select test_admittance -. install/local_setup.bash -ros2 run test_admittance test_admittance --ros-args --params-file ./src/test_admittance/config/parameters.yaml diff --git a/start_TestCalibration.sh b/start_TestCalibration.sh deleted file mode 100755 index 1b16a07..0000000 --- a/start_TestCalibration.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash -colcon build --packages-select test_calibration -. install/local_setup.bash -ros2 run test_calibration test_calibration --ros-args --params-file ./src/test_calibration/config/parameters.yaml diff --git a/start_TestImpedance.sh b/start_TestImpedance.sh deleted file mode 100755 index 8f1d37a..0000000 --- a/start_TestImpedance.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash -colcon build --packages-select test_impedance -. install/local_setup.bash -ros2 run test_impedance test_impedance --ros-args --params-file ./src/test_impedance/config/parameters.yaml