From d0b48a77772f08641468ace6a8e39e9a7ac4a96b Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Mon, 19 Oct 2026 09:18:39 +1000 Subject: [PATCH 1/5] Add Kinova Kortex and the Gen3 MoveIt configs (linux-64) [skip ci] Ports the Kinova part of RoboStack/ros-jazzy#221 (the picknik and robotiq packages from it are already built). kortex_api downloads a linux-x86_64-only binary, hence linux-64 only. Co-authored-by: Ruben Arts <12893423+ruben-arts@users.noreply.github.com> Co-Authored-By: Claude Opus 5.5 --- shared/vinca.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/shared/vinca.yaml b/shared/vinca.yaml index c93bc085..da3c91ac 100644 --- a/shared/vinca.yaml +++ b/shared/vinca.yaml @@ -514,6 +514,10 @@ packages_select_by_deps: - if: linux and not aarch64 then: - franka_ros2 # Franka robots support; see PR #338 for macOS/Windows errors + - kinova_gen3_6dof_robotiq_2f_85_moveit_config # kortex_api ships a linux-x86_64 binary; requested in https://github.com/RoboStack/ros-jazzy/pull/221 + - kinova_gen3_7dof_robotiq_2f_85_moveit_config + - kinova_gen3_lite_moveit_config + - kortex_bringup - libfranka # conda-forge's libfranka is only published for linux-64, not linux-aarch64 - webots_ros2 # Cyberbotics From 7f7d7f8e05cf0f4816130a52b30e51580f0bf625 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Mon, 19 Oct 2026 22:44:45 +1000 Subject: [PATCH 2/5] rolling: kortex_driver without ament_target_dependencies [skip ci] ament_cmake 2.9 (rolling) no longer provides ament_target_dependencies(), so kortex_driver 0.2.5 fails to configure ("Unknown CMake command"). Take the CMake part of upstream's fix, Kinovarobotics/ros2_kortex#415 (9dd1bbb): link the namespaced targets and add kortex_api's headers as SYSTEM includes. The rest of that commit goes with its kortex_api update, which rolling's 0.2.5 release doesn't have. lyrical's ament_cmake (2.8.8) still has the function. Co-Authored-By: Claude Opus 5.5 --- .../rolling/patch/ros2-kortex-driver.patch | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 distros/rolling/patch/ros2-kortex-driver.patch diff --git a/distros/rolling/patch/ros2-kortex-driver.patch b/distros/rolling/patch/ros2-kortex-driver.patch new file mode 100644 index 00000000..5d28d906 --- /dev/null +++ b/distros/rolling/patch/ros2-kortex-driver.patch @@ -0,0 +1,31 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 6e47d729..687c241d 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -61,13 +61,20 @@ target_include_directories( + PRIVATE + include + ) +-# kortex_api is the headers for the Kortex API +-ament_target_dependencies( ++# ament_target_dependencies() was removed in Rolling; link the modern ++# namespaced CMake targets directly instead. ++target_link_libraries( + ${PROJECT_NAME} +- SYSTEM kortex_api +- hardware_interface +- pluginlib +- rclcpp ++ hardware_interface::hardware_interface ++ pluginlib::pluginlib ++ rclcpp::rclcpp ++) ++# kortex_api is a header-only ament package (no exported CMake target), so its ++# headers are added as SYSTEM includes the way "SYSTEM kortex_api" used to. ++target_include_directories( ++ ${PROJECT_NAME} ++ SYSTEM PUBLIC ++ ${kortex_api_INCLUDE_DIRS} + ) + + pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) From 15e799b067de1395ede7b120fe21e1ae69b01452 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Tue, 20 Oct 2026 01:22:46 +1000 Subject: [PATCH 3/5] lyrical: kortex_driver without ament_target_dependencies too [skip ci] lyrical's ament_cmake doesn't provide ament_target_dependencies() either ("Unknown CMake command" in a linux-64 build), so lyrical gets the same patch as rolling (the CMake part of Kinovarobotics/ros2_kortex#415). lyrical's hardware_interface (6.10) still has the pointer-based interfaces kortex_driver uses. Co-Authored-By: Claude Opus 5.5 --- .../lyrical/patch/ros2-kortex-driver.patch | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 distros/lyrical/patch/ros2-kortex-driver.patch diff --git a/distros/lyrical/patch/ros2-kortex-driver.patch b/distros/lyrical/patch/ros2-kortex-driver.patch new file mode 100644 index 00000000..5d28d906 --- /dev/null +++ b/distros/lyrical/patch/ros2-kortex-driver.patch @@ -0,0 +1,31 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 6e47d729..687c241d 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -61,13 +61,20 @@ target_include_directories( + PRIVATE + include + ) +-# kortex_api is the headers for the Kortex API +-ament_target_dependencies( ++# ament_target_dependencies() was removed in Rolling; link the modern ++# namespaced CMake targets directly instead. ++target_link_libraries( + ${PROJECT_NAME} +- SYSTEM kortex_api +- hardware_interface +- pluginlib +- rclcpp ++ hardware_interface::hardware_interface ++ pluginlib::pluginlib ++ rclcpp::rclcpp ++) ++# kortex_api is a header-only ament package (no exported CMake target), so its ++# headers are added as SYSTEM includes the way "SYSTEM kortex_api" used to. ++target_include_directories( ++ ${PROJECT_NAME} ++ SYSTEM PUBLIC ++ ${kortex_api_INCLUDE_DIRS} + ) + + pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) From ab6af64d75b2a7689d9a68af644b829e398e8f83 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Tue, 20 Oct 2026 05:08:18 +1000 Subject: [PATCH 4/5] rolling: leave Kortex out until kortex_driver supports ros2_control 6.12 [skip ci] kortex_driver 0.2.5 still uses the pointer-based StateInterface and CommandInterface constructors that ros2_control 6.12 (rolling) removed, and upstream main hasn't migrated (Kinovarobotics/ros2_kortex#430). Exclude it on rolling, with kortex_bringup and the three Gen3 MoveIt configs that need it; kortex_api and kortex_description, which only these pull in, aren't generated either. Checked with vinca: no kortex or kinova recipe is generated for rolling, and nothing else refers to them. The rolling CMake patch for kortex_driver goes again (nothing to apply it to); lyrical keeps its copy. Co-Authored-By: Claude Opus 5.5 --- .../rolling/patch/ros2-kortex-driver.patch | 31 ------------------- distros/rolling/vinca.yaml | 11 +++++++ 2 files changed, 11 insertions(+), 31 deletions(-) delete mode 100644 distros/rolling/patch/ros2-kortex-driver.patch diff --git a/distros/rolling/patch/ros2-kortex-driver.patch b/distros/rolling/patch/ros2-kortex-driver.patch deleted file mode 100644 index 5d28d906..00000000 --- a/distros/rolling/patch/ros2-kortex-driver.patch +++ /dev/null @@ -1,31 +0,0 @@ -diff --git a/CMakeLists.txt b/CMakeLists.txt -index 6e47d729..687c241d 100644 ---- a/CMakeLists.txt -+++ b/CMakeLists.txt -@@ -61,13 +61,20 @@ target_include_directories( - PRIVATE - include - ) --# kortex_api is the headers for the Kortex API --ament_target_dependencies( -+# ament_target_dependencies() was removed in Rolling; link the modern -+# namespaced CMake targets directly instead. -+target_link_libraries( - ${PROJECT_NAME} -- SYSTEM kortex_api -- hardware_interface -- pluginlib -- rclcpp -+ hardware_interface::hardware_interface -+ pluginlib::pluginlib -+ rclcpp::rclcpp -+) -+# kortex_api is a header-only ament package (no exported CMake target), so its -+# headers are added as SYSTEM includes the way "SYSTEM kortex_api" used to. -+target_include_directories( -+ ${PROJECT_NAME} -+ SYSTEM PUBLIC -+ ${kortex_api_INCLUDE_DIRS} - ) - - pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) diff --git a/distros/rolling/vinca.yaml b/distros/rolling/vinca.yaml index bc43347c..08ebb804 100644 --- a/distros/rolling/vinca.yaml +++ b/distros/rolling/vinca.yaml @@ -17,3 +17,14 @@ packages_skip: then: - mujoco_ros2_control_plugins + +packages_exclude: + # kortex_driver 0.2.5 still uses the pointer-based StateInterface/CommandInterface constructors + # that ros2_control 6.12 removed; upstream (Kinovarobotics/ros2_kortex) hasn't migrated yet + # (https://github.com/Kinovarobotics/ros2_kortex/issues/430). + # The rest needs the driver (kortex_api and kortex_description, which only these pull in, go too). + - kortex_driver + - kortex_bringup + - kinova_gen3_6dof_robotiq_2f_85_moveit_config + - kinova_gen3_7dof_robotiq_2f_85_moveit_config + - kinova_gen3_lite_moveit_config From 5405788b5051aaa3aca0e7d071d0224daf232e7a Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Tue, 20 Oct 2026 09:21:38 +1000 Subject: [PATCH 5/5] rolling: kortex_driver ported to ros2_control 6.12, Kortex selected again [skip ci] Instead of excluding Kortex on rolling, patch kortex_driver 0.2.5: the CMake part of Kinovarobotics/ros2_kortex#415 (no ament_target_dependencies in ament_cmake 2.9) and the handle-based interface export, the same C++ change as Kinovarobotics/ros2_kortex#431 (sent upstream for #430): unlisted interfaces for everything the URDF doesn't declare, same names and types, with the members mirrored around read()/write() and the driver's own command resets written to the handles. Built against hardware_interface 6.12.0 on linux-64 with kortex_bringup and the 7-DoF Gen3 MoveIt config on top; an offline check of the exported interface names matches 0.2.5's. Not tested on hardware. Co-Authored-By: Claude Opus 5.5 --- .../rolling/patch/ros2-kortex-driver.patch | 524 ++++++++++++++++++ distros/rolling/vinca.yaml | 12 - 2 files changed, 524 insertions(+), 12 deletions(-) create mode 100644 distros/rolling/patch/ros2-kortex-driver.patch diff --git a/distros/rolling/patch/ros2-kortex-driver.patch b/distros/rolling/patch/ros2-kortex-driver.patch new file mode 100644 index 00000000..812319ea --- /dev/null +++ b/distros/rolling/patch/ros2-kortex-driver.patch @@ -0,0 +1,524 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 6e47d72..687c241 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -61,13 +61,20 @@ target_include_directories( + PRIVATE + include + ) +-# kortex_api is the headers for the Kortex API +-ament_target_dependencies( ++# ament_target_dependencies() was removed in Rolling; link the modern ++# namespaced CMake targets directly instead. ++target_link_libraries( + ${PROJECT_NAME} +- SYSTEM kortex_api +- hardware_interface +- pluginlib +- rclcpp ++ hardware_interface::hardware_interface ++ pluginlib::pluginlib ++ rclcpp::rclcpp ++) ++# kortex_api is a header-only ament package (no exported CMake target), so its ++# headers are added as SYSTEM includes the way "SYSTEM kortex_api" used to. ++target_include_directories( ++ ${PROJECT_NAME} ++ SYSTEM PUBLIC ++ ${kortex_api_INCLUDE_DIRS} + ) + + pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) +diff --git a/include/kortex_driver/hardware_interface.hpp b/include/kortex_driver/hardware_interface.hpp +index 33f37fd..a978167 100644 +--- a/include/kortex_driver/hardware_interface.hpp ++++ b/include/kortex_driver/hardware_interface.hpp +@@ -31,6 +31,7 @@ + #include + #include + #include ++#include + #include + + #include "rclcpp/macros.hpp" +@@ -86,11 +87,22 @@ public: + KORTEX_DRIVER_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams & params) final; + ++ // Interfaces not declared in the URDF (e.g. tcp/twist.*, reset_fault/*, gripper speed/force, ++ // per-joint velocity/effort commands) are exported as unlisted interfaces with the same names. + KORTEX_DRIVER_PUBLIC +- std::vector export_state_interfaces() final; ++ std::vector ++ export_unlisted_state_interface_descriptions() final; + + KORTEX_DRIVER_PUBLIC +- std::vector export_command_interfaces() final; ++ std::vector ++ export_unlisted_command_interface_descriptions() final; ++ ++ KORTEX_DRIVER_PUBLIC ++ std::vector on_export_state_interfaces() ++ final; ++ ++ KORTEX_DRIVER_PUBLIC ++ std::vector on_export_command_interfaces() final; + + KORTEX_DRIVER_PUBLIC + return_type prepare_command_mode_switch( +@@ -205,11 +217,42 @@ private: + int num_turns_tmp_ = 0; + + // fault control +- double reset_fault_cmd_; +- double reset_fault_async_success_; +- double in_fault_; ++ double reset_fault_cmd_ = std::numeric_limits::quiet_NaN(); ++ double reset_fault_async_success_ = std::numeric_limits::quiet_NaN(); ++ double in_fault_ = 0.0; + static constexpr double NO_CMD = std::numeric_limits::quiet_NaN(); + ++ // ros2_control handle-based interfaces: the driver keeps working on the member variables above; ++ // command handles are copied into the members at the start of write(), the members are copied ++ // into the state handles at the end of read(), and wherever the driver itself changes a command ++ // member (mode switch, activation) the value is also written to the command handle. ++ using StateHandle = hardware_interface::StateInterface::SharedPtr; ++ using CommandHandle = hardware_interface::CommandInterface::SharedPtr; ++ // prefix, interface name and backing member of every interface this driver exports, in the ++ // order of the former export_state_interfaces() / export_command_interfaces() ++ struct InterfaceBinding ++ { ++ std::string prefix; ++ std::string name; ++ double * value; ++ }; ++ std::vector state_interface_bindings(); ++ std::vector command_interface_bindings(); ++ std::vector> state_push_list_; ++ std::vector> command_pull_list_; ++ std::vector arm_command_position_handles_; ++ std::vector arm_command_velocity_handles_; ++ std::vector arm_command_effort_handles_; ++ std::vector twist_command_handles_; ++ CommandHandle gripper_command_position_handle_; ++ CommandHandle reset_fault_cmd_handle_; ++ CommandHandle reset_fault_async_success_handle_; ++ void pushStates(); ++ void pullCommands(); ++ void pushArmCommands(); ++ void pushTwistCommands(); ++ void pushGripperPositionCommand(); ++ + void sendTwistCommand(); + void incrementId(); + void sendJointCommands(); +diff --git a/src/hardware_interface.cpp b/src/hardware_interface.cpp +index 4bbff4e..e3eb996 100644 +--- a/src/hardware_interface.cpp ++++ b/src/hardware_interface.cpp +@@ -22,12 +22,14 @@ + */ + //---------------------------------------------------------------------- + ++#include + #include + #include + #include + #include + #include + #include ++#include + #include + + #include "kortex_driver/hardware_interface.hpp" +@@ -291,10 +293,10 @@ CallbackReturn KortexMultiInterfaceHardware::on_init( + return CallbackReturn::SUCCESS; + } + +-std::vector +-KortexMultiInterfaceHardware::export_state_interfaces() ++std::vector ++KortexMultiInterfaceHardware::state_interface_bindings() + { +- std::vector state_interfaces; ++ std::vector bindings; + std::vector arm_joint_names; + + for (std::size_t i = 0; i < info_.joints.size(); i++) +@@ -302,10 +304,10 @@ KortexMultiInterfaceHardware::export_state_interfaces() + RCLCPP_DEBUG(LOGGER, "export_state_interfaces for joint: %s", info_.joints[i].name.c_str()); + if (info_.joints[i].name == gripper_joint_name_) + { +- state_interfaces.emplace_back(hardware_interface::StateInterface( +- info_.joints[i].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); +- state_interfaces.emplace_back(hardware_interface::StateInterface( +- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); ++ bindings.push_back( ++ {info_.joints[i].name, hardware_interface::HW_IF_POSITION, &gripper_position_}); ++ bindings.push_back( ++ {info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_}); + } + else + { +@@ -315,40 +317,34 @@ KortexMultiInterfaceHardware::export_state_interfaces() + + for (std::size_t i = 0; i < arm_joint_names.size(); i++) + { +- state_interfaces.emplace_back(hardware_interface::StateInterface( +- arm_joint_names[i], hardware_interface::HW_IF_POSITION, &arm_positions_[i])); +- state_interfaces.emplace_back(hardware_interface::StateInterface( +- arm_joint_names[i], hardware_interface::HW_IF_VELOCITY, &arm_velocities_[i])); +- state_interfaces.emplace_back(hardware_interface::StateInterface( +- arm_joint_names[i], hardware_interface::HW_IF_EFFORT, &arm_efforts_[i])); ++ bindings.push_back( ++ {arm_joint_names[i], hardware_interface::HW_IF_POSITION, &arm_positions_[i]}); ++ bindings.push_back( ++ {arm_joint_names[i], hardware_interface::HW_IF_VELOCITY, &arm_velocities_[i]}); ++ bindings.push_back({arm_joint_names[i], hardware_interface::HW_IF_EFFORT, &arm_efforts_[i]}); + } + + // state interface which reports if robot is faulted +- state_interfaces.emplace_back( +- hardware_interface::StateInterface("reset_fault", "internal_fault", &in_fault_)); ++ bindings.push_back({"reset_fault", "internal_fault", &in_fault_}); + +- return state_interfaces; ++ return bindings; + } + +-std::vector +-KortexMultiInterfaceHardware::export_command_interfaces() ++std::vector ++KortexMultiInterfaceHardware::command_interface_bindings() + { +- std::vector command_interfaces; ++ std::vector bindings; + std::vector arm_joint_names; + + for (std::size_t i = 0; i < info_.joints.size(); i++) + { + if (info_.joints[i].name == gripper_joint_name_) + { +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- info_.joints[i].name, hardware_interface::HW_IF_POSITION, &gripper_command_position_)); +- +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- info_.joints[i].name, "set_gripper_max_velocity", &gripper_speed_command_)); +- gripper_speed_command_ = gripper_command_max_velocity_; +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- info_.joints[i].name, "set_gripper_max_effort", &gripper_force_command_)); +- gripper_force_command_ = gripper_command_max_force_; ++ bindings.push_back( ++ {info_.joints[i].name, hardware_interface::HW_IF_POSITION, &gripper_command_position_}); ++ bindings.push_back( ++ {info_.joints[i].name, "set_gripper_max_velocity", &gripper_speed_command_}); ++ bindings.push_back({info_.joints[i].name, "set_gripper_max_effort", &gripper_force_command_}); + } + else + { +@@ -357,39 +353,192 @@ KortexMultiInterfaceHardware::export_command_interfaces() + } + for (std::size_t i = 0; i < arm_joint_names.size(); i++) + { ++ bindings.push_back( ++ {arm_joint_names[i], hardware_interface::HW_IF_POSITION, &arm_commands_positions_[i]}); ++ bindings.push_back( ++ {arm_joint_names[i], hardware_interface::HW_IF_VELOCITY, &arm_commands_velocities_[i]}); ++ bindings.push_back( ++ {arm_joint_names[i], hardware_interface::HW_IF_EFFORT, &arm_commands_efforts_[i]}); ++ } ++ ++ // twist command interfaces ++ bindings.push_back({"tcp", "twist.linear.x", &twist_commands_[0]}); ++ bindings.push_back({"tcp", "twist.linear.y", &twist_commands_[1]}); ++ bindings.push_back({"tcp", "twist.linear.z", &twist_commands_[2]}); ++ bindings.push_back({"tcp", "twist.angular.x", &twist_commands_[3]}); ++ bindings.push_back({"tcp", "twist.angular.y", &twist_commands_[4]}); ++ bindings.push_back({"tcp", "twist.angular.z", &twist_commands_[5]}); ++ ++ bindings.push_back({"reset_fault", "command", &reset_fault_cmd_}); ++ bindings.push_back({"reset_fault", "async_success", &reset_fault_async_success_}); ++ ++ return bindings; ++} ++ ++namespace ++{ ++hardware_interface::InterfaceDescription make_unlisted_description( ++ const std::string & prefix, const std::string & name) ++{ ++ hardware_interface::InterfaceInfo info; ++ info.name = name; ++ info.data_type = "double"; ++ info.size = 1; ++ info.enable_limits = false; ++ return hardware_interface::InterfaceDescription(prefix, info); ++} ++} // namespace ++ ++std::vector ++KortexMultiInterfaceHardware::export_unlisted_state_interface_descriptions() ++{ ++ // everything this driver exports that the URDF does not declare ++ std::vector descriptions; ++ for (const auto & b : state_interface_bindings()) ++ { ++ if (joint_state_interfaces_.count(b.prefix + "/" + b.name) == 0) + { +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- arm_joint_names[i], hardware_interface::HW_IF_POSITION, &arm_commands_positions_[i])); +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- arm_joint_names[i], hardware_interface::HW_IF_VELOCITY, &arm_commands_velocities_[i])); +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- arm_joint_names[i], hardware_interface::HW_IF_EFFORT, &arm_commands_efforts_[i])); ++ descriptions.push_back(make_unlisted_description(b.prefix, b.name)); + } + } ++ return descriptions; ++} + +- // register twist command interfaces +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("tcp", "twist.linear.x", &twist_commands_[0])); +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("tcp", "twist.linear.y", &twist_commands_[1])); +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("tcp", "twist.linear.z", &twist_commands_[2])); +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("tcp", "twist.angular.x", &twist_commands_[3])); +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("tcp", "twist.angular.y", &twist_commands_[4])); +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("tcp", "twist.angular.z", &twist_commands_[5])); ++std::vector ++KortexMultiInterfaceHardware::export_unlisted_command_interface_descriptions() ++{ ++ // everything this driver exports that the URDF does not declare ++ std::vector descriptions; ++ for (const auto & b : command_interface_bindings()) ++ { ++ if (joint_command_interfaces_.count(b.prefix + "/" + b.name) == 0) ++ { ++ descriptions.push_back(make_unlisted_description(b.prefix, b.name)); ++ } ++ } ++ return descriptions; ++} + +- command_interfaces.emplace_back( +- hardware_interface::CommandInterface("reset_fault", "command", &reset_fault_cmd_)); ++std::vector ++KortexMultiInterfaceHardware::on_export_state_interfaces() ++{ ++ auto state_interfaces = hardware_interface::SystemInterface::on_export_state_interfaces(); + +- command_interfaces.emplace_back(hardware_interface::CommandInterface( +- "reset_fault", "async_success", &reset_fault_async_success_)); ++ state_push_list_.clear(); ++ for (const auto & b : state_interface_bindings()) ++ { ++ state_push_list_.emplace_back(get_state_interface_handle(b.prefix + "/" + b.name), b.value); ++ } ++ return state_interfaces; ++} ++ ++std::vector ++KortexMultiInterfaceHardware::on_export_command_interfaces() ++{ ++ auto command_interfaces = hardware_interface::SystemInterface::on_export_command_interfaces(); ++ ++ gripper_speed_command_ = gripper_command_max_velocity_; ++ gripper_force_command_ = gripper_command_max_force_; ++ ++ command_pull_list_.clear(); ++ for (const auto & b : command_interface_bindings()) ++ { ++ command_pull_list_.emplace_back(get_command_interface_handle(b.prefix + "/" + b.name), b.value); ++ } ++ ++ const auto handle_of = [this](const double * value) -> CommandHandle ++ { ++ for (const auto & [handle, member] : command_pull_list_) ++ { ++ if (member == value) ++ { ++ return handle; ++ } ++ } ++ return nullptr; ++ }; ++ arm_command_position_handles_.clear(); ++ arm_command_velocity_handles_.clear(); ++ arm_command_effort_handles_.clear(); ++ for (std::size_t i = 0; i < actuator_count_; i++) ++ { ++ arm_command_position_handles_.push_back(handle_of(&arm_commands_positions_[i])); ++ arm_command_velocity_handles_.push_back(handle_of(&arm_commands_velocities_[i])); ++ arm_command_effort_handles_.push_back(handle_of(&arm_commands_efforts_[i])); ++ } ++ twist_command_handles_.clear(); ++ for (std::size_t i = 0; i < twist_commands_.size(); i++) ++ { ++ twist_command_handles_.push_back(handle_of(&twist_commands_[i])); ++ } ++ gripper_command_position_handle_ = handle_of(&gripper_command_position_); ++ reset_fault_cmd_handle_ = handle_of(&reset_fault_cmd_); ++ reset_fault_async_success_handle_ = handle_of(&reset_fault_async_success_); ++ ++ // initial command values (e.g. gripper max velocity/effort) as seen by the controllers ++ for (const auto & [handle, value] : command_pull_list_) ++ { ++ std::ignore = set_command(handle, *value, true); ++ } + + return command_interfaces; + } + ++void KortexMultiInterfaceHardware::pushStates() ++{ ++ for (const auto & [handle, value] : state_push_list_) ++ { ++ std::ignore = set_state(handle, *value, false); ++ } ++} ++ ++void KortexMultiInterfaceHardware::pullCommands() ++{ ++ for (const auto & [handle, value] : command_pull_list_) ++ { ++ std::ignore = get_command(handle, *value, false); ++ } ++} ++ ++void KortexMultiInterfaceHardware::pushArmCommands() ++{ ++ for (std::size_t i = 0; i < actuator_count_ && i < arm_command_position_handles_.size(); i++) ++ { ++ if (arm_command_position_handles_[i]) ++ { ++ std::ignore = set_command(arm_command_position_handles_[i], arm_commands_positions_[i], true); ++ } ++ if (arm_command_velocity_handles_[i]) ++ { ++ std::ignore = ++ set_command(arm_command_velocity_handles_[i], arm_commands_velocities_[i], true); ++ } ++ if (arm_command_effort_handles_[i]) ++ { ++ std::ignore = set_command(arm_command_effort_handles_[i], arm_commands_efforts_[i], true); ++ } ++ } ++} ++ ++void KortexMultiInterfaceHardware::pushTwistCommands() ++{ ++ for (std::size_t i = 0; i < twist_command_handles_.size(); i++) ++ { ++ if (twist_command_handles_[i]) ++ { ++ std::ignore = set_command(twist_command_handles_[i], twist_commands_[i], true); ++ } ++ } ++} ++ ++void KortexMultiInterfaceHardware::pushGripperPositionCommand() ++{ ++ if (gripper_command_position_handle_) ++ { ++ std::ignore = set_command(gripper_command_position_handle_, gripper_command_position_, true); ++ } ++} ++ + return_type KortexMultiInterfaceHardware::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +@@ -592,17 +741,20 @@ return_type KortexMultiInterfaceHardware::perform_command_mode_switch( + { + joint_based_controller_running_ = false; + arm_commands_positions_ = arm_positions_; +- arm_commands_velocities_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; ++ std::fill(arm_commands_velocities_.begin(), arm_commands_velocities_.end(), 0.0); ++ pushArmCommands(); + } + if (stop_twist_controller_) + { + twist_controller_running_ = false; + twist_commands_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; ++ pushTwistCommands(); + } + if (stop_gripper_controller_) + { + gripper_controller_running_ = false; + gripper_command_position_ = gripper_position_; ++ pushGripperPositionCommand(); + } + if (stop_fault_controller_) + { +@@ -616,7 +768,8 @@ return_type KortexMultiInterfaceHardware::perform_command_mode_switch( + arm_mode_ = k_api::Base::ServoingMode::LOW_LEVEL_SERVOING; + twist_controller_running_ = false; + arm_commands_positions_ = arm_positions_; +- arm_commands_velocities_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; ++ std::fill(arm_commands_velocities_.begin(), arm_commands_velocities_.end(), 0.0); ++ pushArmCommands(); + joint_based_controller_running_ = true; + // refresh feedback + feedback_ = base_cyclic_.RefreshFeedback(); +@@ -628,11 +781,13 @@ return_type KortexMultiInterfaceHardware::perform_command_mode_switch( + arm_mode_ = k_api::Base::ServoingMode::SINGLE_LEVEL_SERVOING; + joint_based_controller_running_ = false; + twist_commands_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; ++ pushTwistCommands(); + twist_controller_running_ = true; + } + if (start_gripper_controller_) + { + gripper_command_position_ = gripper_position_; ++ pushGripperPositionCommand(); + gripper_controller_running_ = true; + } + if (start_fault_controller_) +@@ -716,6 +871,9 @@ CallbackReturn KortexMultiInterfaceHardware::on_activate( + } + arm_joints_control_level_[i] = integration_lvl_t::UNDEFINED; + } ++ pushArmCommands(); ++ pushGripperPositionCommand(); ++ pushStates(); + + RCLCPP_INFO(LOGGER, "KortexMultiInterfaceHardware successfully activated!"); + return CallbackReturn::SUCCESS; +@@ -792,6 +950,8 @@ return_type KortexMultiInterfaceHardware::read( + // add mode that can't be easily reached + in_fault_ += (feedback_.base().active_state() == k_api::Common::ARMSTATE_SERVOING_READY); + ++ pushStates(); ++ + return return_type::OK; + } + +@@ -809,6 +969,8 @@ void KortexMultiInterfaceHardware::readGripperPosition() + return_type KortexMultiInterfaceHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + { ++ pullCommands(); ++ + if (block_write) + { + feedback_ = base_cyclic_.RefreshFeedback(); +@@ -852,6 +1014,15 @@ return_type KortexMultiInterfaceHardware::write( + reset_fault_async_success_ = 0.0; + } + reset_fault_cmd_ = NO_CMD; ++ if (reset_fault_cmd_handle_) ++ { ++ std::ignore = set_command(reset_fault_cmd_handle_, reset_fault_cmd_, true); ++ } ++ if (reset_fault_async_success_handle_) ++ { ++ std::ignore = ++ set_command(reset_fault_async_success_handle_, reset_fault_async_success_, true); ++ } + } + + if (in_fault_ == 0.0) diff --git a/distros/rolling/vinca.yaml b/distros/rolling/vinca.yaml index 08ebb804..2c141ef8 100644 --- a/distros/rolling/vinca.yaml +++ b/distros/rolling/vinca.yaml @@ -16,15 +16,3 @@ packages_skip: - if: osx or win then: - mujoco_ros2_control_plugins - - -packages_exclude: - # kortex_driver 0.2.5 still uses the pointer-based StateInterface/CommandInterface constructors - # that ros2_control 6.12 removed; upstream (Kinovarobotics/ros2_kortex) hasn't migrated yet - # (https://github.com/Kinovarobotics/ros2_kortex/issues/430). - # The rest needs the driver (kortex_api and kortex_description, which only these pull in, go too). - - kortex_driver - - kortex_bringup - - kinova_gen3_6dof_robotiq_2f_85_moveit_config - - kinova_gen3_7dof_robotiq_2f_85_moveit_config - - kinova_gen3_lite_moveit_config