From b1da9a2fa27bc26db93fc5e8442468d4e0ac1a40 Mon Sep 17 00:00:00 2001 From: Omid Date: Wed, 29 May 2024 12:09:04 -0500 Subject: [PATCH 1/7] Changes made to the local branch for FRI3 compatibility --- .../lbr_demos_py/joint_trajectory_client.py | 8 ++++---- lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp | 6 ++++++ lbr_fri_ros2/src/async_client.cpp | 3 +++ lbr_fri_ros2/src/interfaces/position_command.cpp | 10 ++++++++++ lbr_fri_ros2/test/test_command_interfaces.cpp | 6 ++++++ .../med7_moveit_config/.setup_assistant | 6 +++--- .../med7_moveit_config/config/joint_limits.yaml | 16 ++++++++-------- .../med7_moveit_config/config/med7.srdf | 4 ++-- .../config/moveit_controllers.yaml | 4 ++-- .../launch/move_group.launch.py | 1 - lbr_moveit_config/med7_moveit_config/package.xml | 4 ++-- .../config/lbr_system_interface.xacro | 3 +-- .../lbr_ros2_control/system_interface.hpp | 5 ++++- lbr_ros2_control/src/system_interface.cpp | 3 +++ 14 files changed, 54 insertions(+), 25 deletions(-) diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py index 5bffbbde..303acc02 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py @@ -72,13 +72,13 @@ def main(args: list = None) -> None: joint_trajectory_client.get_logger().info("Rotating odd joints.") joint_trajectory_client.execute( [ - 1.0, 0.0, - 1.0, 0.0, - 1.0, 0.0, - 1.0, + 0.0, + 0.0, + 0.0, + 0.5, ] ) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index e44d5dd3..d9276349 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -66,6 +66,12 @@ struct EnumMaps { return "JOINT_POSITION"; case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: return "CARTESIAN_POSE"; +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + case KUKA::FRI::EClientCommandMode::JOINT_POSITION: + return "JOINT_POSITION"; + case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: + return "CARTESIAN_POSE"; #endif case KUKA::FRI::EClientCommandMode::TORQUE: return "TORQUE"; diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index a32e7ae1..b3f1787a 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -23,6 +23,9 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #endif #if FRICLIENT_VERSION_MAJOR == 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { command_interface_ptr_ = std::make_shared( diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 786fd479..73791832 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -28,6 +28,16 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); throw std::runtime_error(err); } +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { + std::string err = + "Expected robot in " + + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + + " command mode."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); + throw std::runtime_error(err); + } #endif if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), [](const double &v) { return std::isnan(v); })) { diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 9f956ca0..12356a63 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -38,6 +38,9 @@ class TestCommandInterfaces : public ::testing::Test { #endif #if FRICLIENT_VERSION_MAJOR == 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { command_interface_ = @@ -148,6 +151,9 @@ TEST_F(TestCommandInterfaces, TestPositionCommandInterface) { #endif #if FRICLIENT_VERSION_MAJOR == 2 set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); #endif test_simple(); } diff --git a/lbr_moveit_config/med7_moveit_config/.setup_assistant b/lbr_moveit_config/med7_moveit_config/.setup_assistant index d788c22c..3763934a 100644 --- a/lbr_moveit_config/med7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med7_moveit_config/.setup_assistant @@ -5,9 +5,9 @@ moveit_setup_assistant_config: srdf: relative_path: config/med7.srdf package_settings: - author_name: mhubii - author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640668 + author_name: Omid + author_email: omid.rezayof@utexas.edu + generated_timestamp: 1716909457 control_xacro: command: - position diff --git a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml index e97eee4e..af74c44c 100644 --- a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml @@ -11,35 +11,35 @@ joint_limits: A1: has_velocity_limits: true max_velocity: 1.7104226669544429 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 10.0 A2: has_velocity_limits: true max_velocity: 1.7104226669544429 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 10.0 A3: has_velocity_limits: true max_velocity: 1.7453292519943295 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 10.0 A4: has_velocity_limits: true max_velocity: 2.2689280275926285 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 10.0 A5: has_velocity_limits: true max_velocity: 2.4434609527920612 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 10.0 A6: has_velocity_limits: true max_velocity: 3.1415926535897931 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 10.0 A7: has_velocity_limits: true max_velocity: 3.1415926535897931 - has_acceleration_limits: true - max_acceleration: 10.0 \ No newline at end of file + has_acceleration_limits: false + max_acceleration: 10.0 diff --git a/lbr_moveit_config/med7_moveit_config/config/med7.srdf b/lbr_moveit_config/med7_moveit_config/config/med7.srdf index 1b4932a5..56b8201d 100644 --- a/lbr_moveit_config/med7_moveit_config/config/med7.srdf +++ b/lbr_moveit_config/med7_moveit_config/config/med7.srdf @@ -24,9 +24,9 @@ - + - + diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml index 57d65c17..a909bb9f 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -6,7 +6,7 @@ moveit_simple_controller_manager: controller_names: - joint_trajectory_controller - joint_trajectory_controller: + arm_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true @@ -17,4 +17,4 @@ moveit_simple_controller_manager: - A4 - A5 - A6 - - A7 \ No newline at end of file + - A7 diff --git a/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py index cc87e1fa..cf30febb 100644 --- a/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py +++ b/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py @@ -4,7 +4,6 @@ from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_move_group_launch - def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("med7", package_name="med7_moveit_config") diff --git a/lbr_moveit_config/med7_moveit_config/package.xml b/lbr_moveit_config/med7_moveit_config/package.xml index dd007600..2d7de81a 100644 --- a/lbr_moveit_config/med7_moveit_config/package.xml +++ b/lbr_moveit_config/med7_moveit_config/package.xml @@ -6,7 +6,7 @@ An automatically generated package with all the configuration and launch files for using the med7 with the MoveIt Motion Planning Framework - mhubii + Omid BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - mhubii + Omid ament_cmake diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index cbb7d1db..7b791d38 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -103,7 +103,6 @@ - @@ -155,4 +154,4 @@ sim="${sim}" /> - \ No newline at end of file + diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index f8756b19..e1956d48 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -36,6 +36,9 @@ struct SystemInterfaceParameters { #endif #if FRICLIENT_VERSION_MAJOR == 2 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; #endif int32_t port_id{30200}; const char *remote_host{nullptr}; @@ -68,7 +71,7 @@ class SystemInterface : public hardware_interface::SystemInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface"; - static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7; + static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6; static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; static constexpr uint8_t LBR_FRI_SENSORS = 2; static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 4dae0dc0..206aa6d4 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -327,6 +327,9 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & #endif #if FRICLIENT_VERSION_MAJOR == 2 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; +#endif +#if FRICLIENT_VERSION_MAJOR == 3 + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; #endif } else if (client_command_mode == "torque") { parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::TORQUE; From 798f6081b7d63828a605275fcce7898cc25135fa Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 31 May 2024 08:59:36 +0100 Subject: [PATCH 2/7] simplified versioning --- .../lbr_demos_py/joint_trajectory_client.py | 8 ++++---- lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp | 8 +------- lbr_fri_ros2/src/async_client.cpp | 5 +---- lbr_fri_ros2/src/interfaces/position_command.cpp | 12 +----------- lbr_fri_ros2/test/test_command_interfaces.cpp | 5 +---- .../med7_moveit_config/.setup_assistant | 6 +++--- .../med7_moveit_config/config/joint_limits.yaml | 16 ++++++++-------- .../med7_moveit_config/config/med7.srdf | 4 ++-- .../launch/move_group.launch.py | 1 + lbr_moveit_config/med7_moveit_config/package.xml | 4 ++-- .../config/lbr_system_interface.xacro | 5 ++++- .../config/lbr_system_parameters.yaml | 3 +++ .../lbr_ros2_control/system_interface.hpp | 10 ++++++---- lbr_ros2_control/src/system_interface.cpp | 5 +---- 14 files changed, 38 insertions(+), 54 deletions(-) diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py index 303acc02..5bffbbde 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py @@ -72,13 +72,13 @@ def main(args: list = None) -> None: joint_trajectory_client.get_logger().info("Rotating odd joints.") joint_trajectory_client.execute( [ + 1.0, 0.0, + 1.0, 0.0, + 1.0, 0.0, - 0.0, - 0.0, - 0.0, - 0.5, + 1.0, ] ) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index d9276349..debb2e86 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -61,13 +61,7 @@ struct EnumMaps { case KUKA::FRI::EClientCommandMode::POSITION: return "POSITION"; #endif -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::JOINT_POSITION: - return "JOINT_POSITION"; - case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: - return "CARTESIAN_POSE"; -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: return "JOINT_POSITION"; case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index b3f1787a..5c572fcc 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -21,10 +21,7 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #if FRICLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: #endif -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::JOINT_POSITION: -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 73791832..0a81bfe6 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -19,17 +19,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } #endif -#if FRICLIENT_VERSION_MAJOR == 2 - if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { - std::string err = - "Expected robot in " + - EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + - " command mode."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); - throw std::runtime_error(err); - } -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { std::string err = "Expected robot in " + diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 12356a63..1b5b273c 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -36,10 +36,7 @@ class TestCommandInterfaces : public ::testing::Test { #if FRICLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: #endif -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::JOINT_POSITION: -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { diff --git a/lbr_moveit_config/med7_moveit_config/.setup_assistant b/lbr_moveit_config/med7_moveit_config/.setup_assistant index 3763934a..d788c22c 100644 --- a/lbr_moveit_config/med7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med7_moveit_config/.setup_assistant @@ -5,9 +5,9 @@ moveit_setup_assistant_config: srdf: relative_path: config/med7.srdf package_settings: - author_name: Omid - author_email: omid.rezayof@utexas.edu - generated_timestamp: 1716909457 + author_name: mhubii + author_email: m.huber_1994@hotmail.de + generated_timestamp: 1690640668 control_xacro: command: - position diff --git a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml index af74c44c..e97eee4e 100644 --- a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml @@ -11,35 +11,35 @@ joint_limits: A1: has_velocity_limits: true max_velocity: 1.7104226669544429 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 10.0 A2: has_velocity_limits: true max_velocity: 1.7104226669544429 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 10.0 A3: has_velocity_limits: true max_velocity: 1.7453292519943295 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 10.0 A4: has_velocity_limits: true max_velocity: 2.2689280275926285 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 10.0 A5: has_velocity_limits: true max_velocity: 2.4434609527920612 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 10.0 A6: has_velocity_limits: true max_velocity: 3.1415926535897931 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 10.0 A7: has_velocity_limits: true max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 10.0 + has_acceleration_limits: true + max_acceleration: 10.0 \ No newline at end of file diff --git a/lbr_moveit_config/med7_moveit_config/config/med7.srdf b/lbr_moveit_config/med7_moveit_config/config/med7.srdf index 56b8201d..1b4932a5 100644 --- a/lbr_moveit_config/med7_moveit_config/config/med7.srdf +++ b/lbr_moveit_config/med7_moveit_config/config/med7.srdf @@ -24,9 +24,9 @@ - + - + diff --git a/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py index cf30febb..cc87e1fa 100644 --- a/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py +++ b/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py @@ -4,6 +4,7 @@ from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_move_group_launch + def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("med7", package_name="med7_moveit_config") diff --git a/lbr_moveit_config/med7_moveit_config/package.xml b/lbr_moveit_config/med7_moveit_config/package.xml index 2d7de81a..dd007600 100644 --- a/lbr_moveit_config/med7_moveit_config/package.xml +++ b/lbr_moveit_config/med7_moveit_config/package.xml @@ -6,7 +6,7 @@ An automatically generated package with all the configuration and launch files for using the med7 with the MoveIt Motion Planning Framework - Omid + mhubii BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - Omid + mhubii ament_cmake diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 7b791d38..4aa16909 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -103,6 +103,9 @@ + + + @@ -154,4 +157,4 @@ sim="${sim}" /> - + \ No newline at end of file diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml index ff044a74..f094a67f 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -1,5 +1,8 @@ # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface hardware: + fri_client_sdk: + major_version: 1 + minor_version: 15 client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index e1956d48..9f4350cc 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -34,10 +34,7 @@ struct SystemInterfaceParameters { #if FRICLIENT_VERSION_MAJOR == 1 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION}; #endif -#if FRICLIENT_VERSION_MAJOR == 2 - KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; #endif int32_t port_id{30200}; @@ -71,7 +68,12 @@ class SystemInterface : public hardware_interface::SystemInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface"; +#if FRICLIENT_VERSION_MAJOR == 1 + static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7; +#endif +#if FRICLIENT_VERSION_MAJOR >= 2 static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6; +#endif static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; static constexpr uint8_t LBR_FRI_SENSORS = 2; static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 206aa6d4..6ed6d513 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -325,10 +325,7 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & #if FRICLIENT_VERSION_MAJOR == 1 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION; #endif -#if FRICLIENT_VERSION_MAJOR == 2 - parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; #endif } else if (client_command_mode == "torque") { From 9baf90b840ae9cc7d41c46bb6b0e04bb476f4b07 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 31 May 2024 09:00:09 +0100 Subject: [PATCH 3/7] added 2.7 build --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 01695888..084c868e 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -15,7 +15,7 @@ jobs: strategy: matrix: os: [ubuntu-latest] - fri_version: [1.11, 1.14, 1.15, 1.16, 2.5] + fri_version: [1.11, 1.14, 1.15, 1.16, 2.5, 2.7] steps: - uses: ros-tooling/setup-ros@v0.7 - uses: ros-tooling/action-ros-ci@v0.2 From 62835a7d9f03bd74f2dff0b417cc7fd3b62fdc12 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 31 May 2024 09:03:09 +0100 Subject: [PATCH 4/7] incorrect controller --- .../med7_moveit_config/config/moveit_controllers.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml index a909bb9f..57d65c17 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -6,7 +6,7 @@ moveit_simple_controller_manager: controller_names: - joint_trajectory_controller - arm_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true @@ -17,4 +17,4 @@ moveit_simple_controller_manager: - A4 - A5 - A6 - - A7 + - A7 \ No newline at end of file From 51653362f7f73db12cff9f2aad1107ad5a732afc Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 31 May 2024 09:04:05 +0100 Subject: [PATCH 5/7] added 2.7 repos yaml --- lbr_fri_ros2_stack/repos-fri-2.7.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 lbr_fri_ros2_stack/repos-fri-2.7.yaml diff --git a/lbr_fri_ros2_stack/repos-fri-2.7.yaml b/lbr_fri_ros2_stack/repos-fri-2.7.yaml new file mode 100644 index 00000000..90dcd423 --- /dev/null +++ b/lbr_fri_ros2_stack/repos-fri-2.7.yaml @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-2.7 + lbr_fri_idl: + type: git + url: https://github.com/lbr-stack/lbr_fri_idl + version: fri-2 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble From 65d6b4cfaa8c3388f747c6f9e175666e7df92776 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 31 May 2024 09:13:28 +0100 Subject: [PATCH 6/7] simplified directive --- lbr_fri_ros2/test/test_command_interfaces.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 1b5b273c..3ab403cb 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -146,10 +146,7 @@ TEST_F(TestCommandInterfaces, TestPositionCommandInterface) { #if FRICLIENT_VERSION_MAJOR == 1 set_up(KUKA::FRI::EClientCommandMode::POSITION); #endif -#if FRICLIENT_VERSION_MAJOR == 2 - set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); #endif test_simple(); From e9e82714f70752a39c9e72c165dabc557a33a027 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 31 May 2024 09:51:00 +0100 Subject: [PATCH 7/7] runtime check for fri client sdk version --- lbr_ros2_control/config/lbr_system_interface.xacro | 4 +++- lbr_ros2_control/config/lbr_system_parameters.yaml | 4 ++-- .../include/lbr_ros2_control/system_interface.hpp | 2 ++ lbr_ros2_control/src/system_interface.cpp | 14 ++++++++++++++ 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 4aa16909..3e9ec6cc 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -16,6 +16,8 @@ lbr_ros2_control::SystemInterface + ${system_parameters['hardware']['fri_client_sdk']['major_version']} + ${system_parameters['hardware']['fri_client_sdk']['minor_version']} ${system_parameters['hardware']['client_command_mode']} ${system_parameters['hardware']['port_id']} ${system_parameters['hardware']['remote_host']} @@ -104,7 +106,7 @@ - + diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml index f094a67f..91cbaeb6 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -1,6 +1,6 @@ # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface hardware: - fri_client_sdk: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro major_version: 1 minor_version: 15 client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] @@ -14,7 +14,7 @@ hardware: pid_i_min: 0.0 # min integral value for the joint position command pid_antiwindup: false # enable antiwindup for the joint position command command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] + external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] open_loop: true # KUKA works the best in open_loop control mode diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 9f4350cc..aace5b6a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -31,6 +31,8 @@ namespace lbr_ros2_control { struct SystemInterfaceParameters { + uint8_t fri_client_sdk_major_version{1}; + uint8_t fri_client_sdk_minor_version{15}; #if FRICLIENT_VERSION_MAJOR == 1 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION}; #endif diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 6ed6d513..517818d0 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -320,6 +320,20 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &system_info) { try { + parameters_.fri_client_sdk_major_version = + std::stoul(system_info.hardware_parameters.at("fri_client_sdk_major_version")); + parameters_.fri_client_sdk_minor_version = + std::stoul(system_info.hardware_parameters.at("fri_client_sdk_minor_version")); + if (parameters_.fri_client_sdk_major_version != FRICLIENT_VERSION_MAJOR) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Expected FRI client SDK version '" << FRICLIENT_VERSION_MAJOR << "', got '" + << std::to_string(parameters_.fri_client_sdk_major_version) + << "'. Update lbr_system_parameters.yaml or compile against correct FRI version." + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode"); if (client_command_mode == "position") { #if FRICLIENT_VERSION_MAJOR == 1