From b1fc2c5ff5e0670413b72edc4f4700ce6a4bf946 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Tue, 15 Sep 2026 14:10:10 +1000 Subject: [PATCH] fix: align iiwa controller with FRI lifecycle and use JTC without EMA --- cobot-setting.yaml | 3 - cobot/commands/robot_setup.py | 7 - .../docs/getting-started/configuration.en.md | 10 +- .../docs/getting-started/configuration.md | 10 +- src/iiwa_bringup/launch/iiwa.launch.py | 5 +- .../launch/supported/controllers.launch.py | 31 +- .../config/moveit/iiwa_controller.yaml | 14 - src/iiwa_config/config/setting.yaml | 3 - src/iiwa_controller/CMakeLists.txt | 10 +- src/iiwa_controller/README.en.md | 86 ++++ src/iiwa_controller/README.md | 95 +++++ .../include/iiwa_controller/FRIClient.h | 84 ++-- .../iiwa_controller/IIWAHardwareInterface.hpp | 30 +- src/iiwa_controller/package.xml | 1 + src/iiwa_controller/src/FRIClient.cpp | 291 +++++++------ .../src/IIWAHardwareInterface.cpp | 382 ++++++++++-------- src/iiwa_controller/test/smoke_jtc.py | 59 +++ src/iiwa_controller/test/test_fri_client.cpp | 174 ++++++++ src/iiwa_controller/test/test_hardware.cpp | 93 +++++ src/iiwa_description/urdf/iiwa7.urdf.xacro | 4 - src/iiwa_utils/iiwa_utils/setting_loader.py | 4 - 21 files changed, 978 insertions(+), 418 deletions(-) create mode 100644 src/iiwa_controller/README.en.md create mode 100644 src/iiwa_controller/README.md create mode 100644 src/iiwa_controller/test/smoke_jtc.py create mode 100644 src/iiwa_controller/test/test_fri_client.cpp create mode 100644 src/iiwa_controller/test/test_hardware.cpp diff --git a/cobot-setting.yaml b/cobot-setting.yaml index a2aef58..1138539 100644 --- a/cobot-setting.yaml +++ b/cobot-setting.yaml @@ -3,9 +3,6 @@ robot: ip: "192.170.10.2" port: 30200 fri_cycle_ms: 10 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц) - joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI - joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей - active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro diff --git a/cobot/commands/robot_setup.py b/cobot/commands/robot_setup.py index c70e2a4..46d184f 100644 --- a/cobot/commands/robot_setup.py +++ b/cobot/commands/robot_setup.py @@ -143,13 +143,6 @@ _BLOCKS: List[_Block] = [ _Field("fri_cycle_ms", "Цикл FRI (мс):", "10", note="5 мс = 200 Гц, 10 мс = 100 Гц", options=["10", "5"]), - _Field("active_controller", "Активный ROS-контроллер:", "jtc", - note="jtc = JointTrajectoryController (MoveIt), forward = ForwardCommandController", - options=["jtc", "forward"]), - _Field("joint_position_tau", "EMA tau фильтра положения (с):", "0.04", - note="Сглаживает команды положения перед отправкой в FRI"), - _Field("joint_velocity_tau", "EMA tau фильтра скорости (с):", "0.01", - note="Убирает выбросы из оценки скорости конечной разностью"), ], ), ] diff --git a/doc/lwc-doc/docs/getting-started/configuration.en.md b/doc/lwc-doc/docs/getting-started/configuration.en.md index 17d1c86..8e59d35 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.en.md +++ b/doc/lwc-doc/docs/getting-started/configuration.en.md @@ -26,9 +26,6 @@ robot: ip: "192.170.10.2" port: 30200 fri_cycle_ms: 10 - joint_position_tau: 0.04 - joint_velocity_tau: 0.01 - active_controller: "jtc" description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro ``` @@ -38,11 +35,10 @@ robot: | `ip` | KUKA controller IP address | **Change** to the actual controller address | | `port` | FRI UDP port | Default: `30200`; change only if the port conflicts | | `fri_cycle_ms` | FRI cycle: `5` ms = 200 Hz, `10` ms = 100 Hz | Use `10` for stable operation or `5` for high-precision tasks | -| `joint_position_tau` | Position EMA filter [s], smoothing commands before transmission | Decrease for a faster response; increase if vibration occurs | -| `joint_velocity_tau` | Velocity EMA filter [s], removing finite-difference spikes | Tune in the same way as `joint_position_tau` | -| `active_controller` | Control mode: `jtc` (MoveIt / JointTrajectory) or `forward` (direct control) | Use `jtc` for most tasks | | `description` | Path to the robot URDF | Do not change | +Motion always uses JointTrajectoryController. Position and velocity EMA filters are not applied. + --- ## `digital_twin` section — simulator @@ -70,6 +66,7 @@ digital_twin: | `webots.cameras` | List of YAML configurations for connected cameras | | `rviz.config` | Path to the RViz configuration | + --- ## `tool` section — active tool @@ -169,7 +166,6 @@ The remaining parameters (`tls`, `topic_whitelist`, `min_qos_depth`, and others) | ✅ | `robot.fri_cycle_ms` | Select `10` (standard) or `5` (high frequency) | | ✅ | `tool.active` | Set the active tool | | ✅ | `web.enabled` | Set to `false` if the web interface is not needed | -| ⚠️ | `robot.active_controller` | Change only when intentionally switching the control mode | | ⚠️ | `planning.*` | Change only when another planner or other parameters are required | | ❌ | `robot.description` | Do not change; this is the URDF path | | ❌ | `controller.moveit.*` | Do not change; these are package-internal MoveIt configuration paths | diff --git a/doc/lwc-doc/docs/getting-started/configuration.md b/doc/lwc-doc/docs/getting-started/configuration.md index b68d2ad..1b97fd2 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.md +++ b/doc/lwc-doc/docs/getting-started/configuration.md @@ -28,9 +28,6 @@ robot: ip: "192.170.10.2" port: 30200 fri_cycle_ms: 10 - joint_position_tau: 0.04 - joint_velocity_tau: 0.01 - active_controller: "jtc" description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro ``` @@ -40,11 +37,10 @@ robot: | `ip` | IP-адрес контроллера KUKA | **Изменить** на реальный IP вашего контроллера | | `port` | UDP-порт FRI | По умолчанию `30200` — менять только при конфликте портов | | `fri_cycle_ms` | Период цикла FRI: `5` мс = 200 Гц, `10` мс = 100 Гц | `10` для стабильной работы, `5` для высокоточных задач | -| `joint_position_tau` | EMA-фильтр позиций [с] — сглаживает команды перед отправкой | Уменьшать для более быстрой реакции, увеличивать при вибрациях | -| `joint_velocity_tau` | EMA-фильтр скорости [с] — убирает выбросы конечных разностей | Аналогично `joint_position_tau` | -| `active_controller` | Режим управления: `jtc` (MoveIt / JointTrajectory) или `forward` (прямое управление) | `jtc` для большинства задач | | `description` | Путь к URDF-описанию робота | Не менять | +Управление движением всегда выполняет JointTrajectoryController. EMA-фильтры положения и скорости не применяются. + --- ## Блок `digital_twin` — симулятор @@ -72,6 +68,7 @@ digital_twin: | `webots.cameras` | Список YAML-конфигов подключённых камер | | `rviz.config` | Путь к конфигурации RViz | + --- ## Блок `tool` — активный инструмент @@ -171,7 +168,6 @@ foxglove: | ✅ | `robot.fri_cycle_ms` | Выбрать `10` (стандарт) или `5` (высокая частота) | | ✅ | `tool.active` | Указать активный инструмент | | ✅ | `web.enabled` | Выключить (`false`), если веб-интерфейс не нужен | -| ⚠️ | `robot.active_controller` | Менять только при намеренном переключении режима управления | | ⚠️ | `planning.*` | Менять при необходимости другого планировщика или параметров | | ❌ | `robot.description` | Не трогать — путь к URDF | | ❌ | `controller.moveit.*` | Не трогать — пути к конфигам MoveIt внутри пакетов | diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 14340c0..5708f37 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -51,7 +51,6 @@ def _runtime_setup(context, *args, **kwargs): "robot_ip": settings.robot.ip, "fri_port": str(settings.robot.port), "simulate": "false", - "joint_position_tau": str(settings.robot.joint_position_tau), } robot_description = converter.load_robot_description( @@ -117,8 +116,8 @@ def _runtime_setup(context, *args, **kwargs): "rotation": str(settings.digital_twin.webots.rotation), "controller_timer": str(settings.digital_twin.webots.controller_timer), "fri_cycle_ms": str(settings.robot.fri_cycle_ms), - "joint_position_tau": str(settings.robot.joint_position_tau), - "controller": settings.robot.active_controller, + "robot_ip": settings.robot.ip, + "fri_port": str(settings.robot.port), } setup.append(IncludeLaunchDescription( diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 4e01598..4c1a77b 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -19,9 +19,9 @@ def _setup_controllers(context, *args, **kwargs): controller_timer = LaunchConfiguration("controller_timer").perform(context) controller_path = LaunchConfiguration("controller_path").perform(context) simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes") - controller = LaunchConfiguration("controller").perform(context) # "jtc" | "forward" fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context)) - joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context) + if not 1 <= fri_cycle_ms <= 100: + raise ValueError("fri_cycle_ms must be within the FRI 1.16 range 1..100 ms") update_rate = 1000 // fri_cycle_ms xacro_args = {"initial_positions_file": initial_positions_file} @@ -29,7 +29,8 @@ def _setup_controllers(context, *args, **kwargs): if simulate: xacro_args["simulate"] = "true" else: - xacro_args["joint_position_tau"] = joint_position_tau + xacro_args["robot_ip"] = LaunchConfiguration("robot_ip").perform(context) + xacro_args["fri_port"] = LaunchConfiguration("fri_port").perform(context) robot_description = converter.load_robot_description( model_path=description, @@ -98,33 +99,17 @@ def _setup_controllers(context, *args, **kwargs): cm = ["--controller-manager", "/controller_manager"] - jtc_args = ["iiwa_arm_controller"] + cm - if controller == "forward": - jtc_args += ["--inactive"] - - # ForwardCommandController: активен если controller=forward, иначе --inactive - forward_args = ["forward_position_controller"] + cm - if controller != "forward": - forward_args += ["--inactive"] - jtc = Node( package="controller_manager", executable="spawner", output="screen", - arguments=jtc_args, - ) - - forward_controller = Node( - package="controller_manager", - executable="spawner", - output="screen", - arguments=forward_args, + arguments=["iiwa_arm_controller"] + cm, ) jtc_after_jsb = RegisterEventHandler( OnProcessExit( target_action=jsb, - on_exit=[jtc, forward_controller], + on_exit=[jtc], ) ) @@ -138,7 +123,7 @@ def _setup_controllers(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("fri_cycle_ms", default_value="5"), - DeclareLaunchArgument("joint_position_tau", default_value="0.04"), - DeclareLaunchArgument("controller", default_value="jtc"), + DeclareLaunchArgument("robot_ip", default_value="192.170.10.2"), + DeclareLaunchArgument("fri_port", default_value="30200"), OpaqueFunction(function=_setup_controllers), ]) diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index 2a1e467..695ca94 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/src/iiwa_config/config/moveit/iiwa_controller.yaml @@ -8,8 +8,6 @@ controller_manager: iiwa_arm_controller: type: "joint_trajectory_controller/JointTrajectoryController" - forward_position_controller: - type: "forward_command_controller/ForwardCommandController" joint_state_broadcaster: ros__parameters: @@ -58,15 +56,3 @@ iiwa_arm_controller: # joint5: { trajectory: 0.1, goal: 0.01 } # joint6: { trajectory: 0.1, goal: 0.01 } # joint7: { trajectory: 0.1, goal: 0.01 } - -forward_position_controller: - ros__parameters: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - interface_name: position diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 2352374..25be294 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -3,9 +3,6 @@ robot: ip: "192.170.10.2" port: 30200 fri_cycle_ms: 10 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц) - joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI - joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей - active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro diff --git a/src/iiwa_controller/CMakeLists.txt b/src/iiwa_controller/CMakeLists.txt index 63af7d4..b668066 100644 --- a/src/iiwa_controller/CMakeLists.txt +++ b/src/iiwa_controller/CMakeLists.txt @@ -33,7 +33,7 @@ file(GLOB_RECURSE FRI_SOURCES add_library(fri_client_sdk STATIC ${FRI_SOURCES}) -target_include_directories(fri_client_sdk PUBLIC +target_include_directories(fri_client_sdk SYSTEM PUBLIC ${FRI_SDK_DIR}/include ${FRI_SDK_DIR}/src/nanopb-0.2.8 ${FRI_SDK_DIR}/src/protobuf @@ -93,4 +93,12 @@ ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_fri_client test/test_fri_client.cpp) + target_link_libraries(test_fri_client ${PROJECT_NAME} fri_client_sdk) + ament_add_gtest(test_hardware test/test_hardware.cpp) + target_link_libraries(test_hardware ${PROJECT_NAME} fri_client_sdk hardware_interface::hardware_interface) +endif() + ament_package() \ No newline at end of file diff --git a/src/iiwa_controller/README.en.md b/src/iiwa_controller/README.en.md new file mode 100644 index 0000000..cb8fa5c --- /dev/null +++ b/src/iiwa_controller/README.en.md @@ -0,0 +1,86 @@ +# iiwa_controller + +KUKA LBR iiwa 7 R800 `hardware_interface::SystemInterface`, targeting the bundled +**FRI C++ SDK 1.16**, ROS 2 Jazzy, **hardware_interface 4.48.0**, C++17. +[Русский](README.md). + +## Sources + +The SDK 1.16 Doxygen in the `kuka-fri` skill and Jazzy hardware documentation +were read before reviewing the implementation. Bundled SDK sources, +`ServerFriRos2.java`, the URDF and the full +[KUKA Sunrise.FRI 1.16 manual](../../doc/pdf/KUKA_SunriseFRI_116_en.pdf) +were then checked. Relevant manual sections: 2.1.4, 6.2.1–6.2.5 and 6.3.3. +Monitoring mirrors commanded position; COMMANDING_WAIT mirrors IPO. +Commanding states require continuous commands. Measured, commanded and IPO +positions are distinct quantities. + +The plugin uses framework-managed interfaces and the installed Jazzy headers, +as described in [Writing a Hardware Component](https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/writing_new_hardware_component.html). + +## Contract + +- Seven joints, `joint1`…`joint7`, in SDK A1…A7 order. Command: position [rad]. + States: encoder position [rad], finite-difference velocity [rad/s] calculated + with robot timestamps, measured effort and estimated external torque [Nm]. +- No position or velocity EMA. Commands retain a URDF velocity step limit + (`max_velocity * FRI period`); this is a rate limit, not an EMA filter. + Non-finite/out-of-position-range commands fault. Existing robot limits are + unchanged and come from Jazzy `HardwareInfo::limits`. +- POSITION with JOINT overlay supports position, joint impedance and Cartesian + impedance control used by `ServerFriRos2`. TORQUE/WRENCH are unsupported. + Monitoring without an overlay remains supported. +- The only launched motion controller is `iiwa_arm_controller`, a + JointTrajectoryController. Action: `/iiwa_arm_controller/follow_joint_trajectory`. + Existing joint-state broadcaster QoS is retained; no new topics or TF frames. +- Hardware IP/port reach the URDF from `robot.ip`/`robot.port`; project defaults + remain `192.170.10.2:30200`. Match `robot.fri_cycle_ms` to the Sunrise period. + +One thread owns SDK `step()`. Cyclic exchange uses `try_lock`, so a busy mutex +never stalls either cycle. Readers retain a complete previous snapshot with a +freshness check; writers retry next cycle. Startup allows 15 seconds for the +first UDP packet. After that, a failed `step()` is terminal. Activation requires +fresh telemetry, at least MONITORING_READY, and GOOD/EXCELLENT quality. + +Missing telemetry/commands for 100 ms, non-increasing timestamps, invalid +telemetry, unexpected commanding mode, a safety stop, inactive drives or poor +commanding quality stop command generation and surface ERROR to ROS. Leaving +COMMANDING_ACTIVE requires explicit lifecycle recovery and a new Sunrise +session. The 15 s/100 ms thresholds, command guards and explicit-recovery policy +are driver decisions, not universal KUKA requirements. Thread joining precedes +socket closure. Cleanup, error, shutdown and destruction release application, +connection, then client. Stopping FRI does not replace hardware emergency stop. + +Dependencies remain those in package.xml; SDK sources build as `fri_client_sdk`. +Relevant headers are `friLBRClient.h`, `friClientApplication.h`, +`friUdpConnection.h`, and `friException.h`. SDK `FRIException` is caught explicitly. +Bundled SDK files are unchanged. + +## Offline validation + +```bash +source /opt/ros/jazzy/setup.bash +colcon build --base-paths src --packages-select iiwa_controller iiwa_bringup iiwa_config iiwa_description iiwa_utils --symlink-install +source install/setup.bash +colcon test --base-paths src --packages-select iiwa_controller +colcon test-result --test-result-base build/iiwa_controller --verbose +/usr/bin/python3 src/iiwa_controller/test/smoke_jtc.py +``` + +Tests exercise real SDK callbacks with artificial data and the plugin's local +`simulate=true` path: measurements, IPO, no EMA, modes, limits, faults and +repeated lifecycle transitions. They do not establish physical FRI timing. + +Before hardware use, run `cobot run local`, choose **Webots simulator**, verify +that the broadcaster and JTC are active, execute and cancel a small valid +MoveIt trajectory, and check the action result and simulated stop. Webots uses +a separate hardware plugin; it does not validate UDP/FRI. No physical robot was +run for this revision. Hardware testing requires the networking, limits, +tool/load, mode, operator supervision and stop checks in AGENTS.md. + +Revision results: all five affected ROS packages built; 14 C++ cases in two +CTest checks passed. The local smoke script loaded the actual URDF with our +plugin in simulation mode, activated JSB/JTC and completed a +0.02 rad joint-1 +trajectory with SUCCEEDED. On SIGINT, Controller Manager's `pal_statistics` +reported an invalid shutdown context; hardware lifecycle shutdown succeeded. +Webots and physical FRI were not run. diff --git a/src/iiwa_controller/README.md b/src/iiwa_controller/README.md new file mode 100644 index 0000000..b7c5a51 --- /dev/null +++ b/src/iiwa_controller/README.md @@ -0,0 +1,95 @@ +# iiwa_controller + +Аппаратный `hardware_interface::SystemInterface` для KUKA LBR iiwa 7 R800. +Проверенная версия: встроенный **FRI C++ SDK 1.16**, ROS 2 Jazzy, +`hardware_interface` **4.48.0**, C++17. [English](README.en.md). + +## Основания реализации + +Перед ревизией реализации изучены Doxygen SDK 1.16 из навыка `kuka-fri` +и документация ros2_control Jazzy. Затем проверены поставляемые исходники SDK, +`ServerFriRos2.java`, URDF и полное руководство +[KUKA Sunrise.FRI 1.16](../../doc/pdf/KUKA_SunriseFRI_116_en.pdf). + +Требования KUKA: разделы 2.1.4, 6.2.1–6.2.5 и 6.3.3. `monitor()` зеркалирует +commanded position, `waitForCommand()` — IPO; в обоих commanding-состояниях +нужны непрерывные команды. Измеренная позиция, commanded position и IPO — разные +величины. Позиционный интерфейс содержит 7 значений A1…A7 в радианах. + +Интеграция ROS следует [Writing a Hardware Component (Jazzy)](https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/writing_new_hardware_component.html), +сигнатурам установленных заголовков и автоматически создаваемым интерфейсам. + +## Поведение + +- ROS-интерфейсы: `joint1`…`joint7`; команда `position` [рад]; состояния + `position` [рад], `velocity` [рад/с], `effort` и `external_torque` [Нм]. +- `position` всегда содержит измерение энкодеров. Скорость — конечная разность + измеренных позиций по временным меткам робота. EMA-фильтров нет. +- Команды передаются без EMA. Ограничение шага `max_velocity * FRI period` + сохраняется как ограничение скорости, а не сглаживающий фильтр. Позиция вне + ограничений и NaN/Inf приводят к ошибке. Пределы берутся из URDF через Jazzy + `HardwareInfo::limits`; численные пределы робота не изменены. +- Поддерживается POSITION + JOINT overlay с position, joint impedance или + Cartesian impedance control, согласно `ServerFriRos2`. TORQUE/WRENCH + намеренно не реализованы. Мониторинг без overlay поддерживается. +- Единственный запускаемый контроллер движения — `iiwa_arm_controller` + (`joint_trajectory_controller/JointTrajectoryController`). Action: + `/iiwa_arm_controller/follow_joint_trajectory`. Joint state broadcaster + публикует состояния с существующими настройками QoS. Новых тем и TF нет. +- IP и порт передаются из `robot.ip` / `robot.port` в аппаратный URDF; + исходные значения проекта — `192.170.10.2:30200`. Период manager задаётся + `robot.fri_cycle_ms`; его нужно согласовать с выбранным периодом Sunrise. + +Один рабочий поток владеет `ClientApplication::step()`. Межпоточный обмен +в цикле использует `try_lock`: при конкуренции цикл не ждёт mutex. `read()` +использует последний целый снимок с проверкой давности, `write()` повторяет +передачу в следующем цикле. До первого UDP-пакета допускается ожидание Sunrise +до 15 с; после первого пакета ошибка `step()` завершает поток. Активация требует +свежей сессии как минимум MONITORING_READY и качества GOOD/EXCELLENT. + +При отсутствии пакетов/команд 100 мс, нерастущем timestamp, нечисловой +телеметрии, недопустимом режиме, safety stop, неактивных приводах или плохом +качестве в commanding приложение прекращает выдачу команд; ROS получает ERROR. +Выход из COMMANDING_ACTIVE требует явного восстановления lifecycle и новой +сессии Sunrise. Пороги 15 с и 100 мс, ограничения команд и политика явного +восстановления — решения драйвера, **не универсальные требования KUKA**. +Остановка потока предшествует закрытию сокета. Объекты уничтожаются в порядке +application → connection → client; cleanup, error, shutdown и деструктор +выполняют это одинаково. Прекращение FRI не заменяет аппаратную аварийную остановку. + +Зависимости: `hardware_interface`, `pluginlib`, `rclcpp`, `rclcpp_lifecycle`, +существующие зависимости package.xml; SDK собирается как `fri_client_sdk`. +Публичные SDK заголовки: `friLBRClient.h`, `friClientApplication.h`, +`friUdpConnection.h`; исключения обрабатываются как `FRIException`. +Поставляемые файлы SDK не изменены. + +## Проверка без робота + +```bash +source /opt/ros/jazzy/setup.bash +colcon build --base-paths src --packages-select iiwa_controller iiwa_bringup iiwa_config iiwa_description iiwa_utils --symlink-install +source install/setup.bash +colcon test --base-paths src --packages-select iiwa_controller +colcon test-result --test-result-base build/iiwa_controller --verbose +/usr/bin/python3 src/iiwa_controller/test/smoke_jtc.py +``` + +Тесты используют реальный SDK с искусственными данными и локальный режим +`simulate=true` аппаратного плагина. Они проверяют измерения, IPO, отсутствие +EMA, режимы, ограничения, ошибки и повторные переходы lifecycle. Они не +доказывают временные характеристики реального FRI-соединения. + +Webots smoke-test перед аппаратным запуском: `cobot run local` → **Webots +simulator**; проверить, что `joint_state_broadcaster` и `iiwa_arm_controller` +активны, выполнить небольшую допустимую траекторию MoveIt и отменить её. +Проверить результат action и остановку симулятора. Webots использует отдельный +плагин и сам по себе не проверяет UDP/FRI. Реальный робот в этой ревизии не запускался. +Для аппаратной проверки нужны оператор, свободная рабочая зона, проверенные +сеть, tool/load data, режим, ограничения и процедура останова из AGENTS.md. + +Результаты ревизии: сборка пяти затронутых ROS-пакетов прошла; 14 C++-сценариев +в двух CTest-проверках прошли. `smoke_jtc.py` загрузил настоящий URDF с локальной +имитацией нашего плагина, активировал JSB/JTC и завершил траекторию +0.02 рад +сустава 1 со статусом SUCCEEDED. При SIGINT библиотека `pal_statistics` +Controller Manager вывела сообщение об остановленном контексте; lifecycle +аппаратного плагина завершился успешно. Webots и физический FRI не запускались. diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index fa112b5..1f7798a 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -2,6 +2,7 @@ #include #include +#include #include #include "friClientApplication.h" @@ -10,62 +11,65 @@ namespace iiwa_controller { - - -// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке -// и так же за один lock читается из read() в потоке управления. struct IIWAStateSnapshot { - std::array measured_pos{}; // в Commanding = filtered_pos_ (open-loop) - std::array measured_tau{}; // измеренные моменты [Нм] - std::array external_tau{}; // внешние моменты без компенсации модели [Нм] - std::array ipo_pos{}; // позиция интерполятора [рад], только в Commanding - double sample_time{0.005}; // период цикла FRI [с] + std::array measured_pos{}; // encoder position [rad], never a command + std::array measured_tau{}; // measured torque [Nm] + std::array external_tau{}; // estimated external torque [Nm] + std::array ipo_pos{}; // interpolator position [rad] + double sample_time{0.0}; + KUKA::FRI::ESessionState session{KUKA::FRI::IDLE}; KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR}; - bool ipo_valid{false}; // в Monitor-режиме IPO недоступна - unsigned int time_stamp_sec{0}; // Unix-время пакета [с] - unsigned int time_stamp_nano_sec{0}; // наносекундная часть [нс] + KUKA::FRI::ESafetyState safety{KUKA::FRI::NORMAL_OPERATION}; + KUKA::FRI::EDriveState drives{KUKA::FRI::OFF}; + KUKA::FRI::EOperationMode operation{KUKA::FRI::TEST_MODE_1}; + KUKA::FRI::EControlMode control{KUKA::FRI::NO_CONTROL}; + KUKA::FRI::EClientCommandMode command_mode{KUKA::FRI::NO_COMMAND_MODE}; + KUKA::FRI::EOverlayType overlay{KUKA::FRI::NO_OVERLAY}; + double tracking_performance{0.0}; + bool valid{false}; + bool ipo_valid{false}; + unsigned int time_stamp_sec{0}; + unsigned int time_stamp_nano_sec{0}; + std::chrono::steady_clock::time_point received_at{}; }; class FRIClient : public KUKA::FRI::LBRClient { public: - static constexpr size_t N_JOINTS = 7; - - // joint_position_tau — постоянная времени экспоненциального фильтра позиций [с]. - // Аналог joint_position_tau из lbr_fri_ros2_stack (по умолчанию 0.04 с = 40 мс). - // Сглаживает скачки команд перед отправкой роботу → убирает писк и стук суставов. - explicit FRIClient(double joint_position_tau = 0.04); - ~FRIClient() override = default; - - // Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step() + static constexpr size_t N_JOINTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS; + using Joints = std::array; + FRIClient(); void monitor() override; void waitForCommand() override; void command() override; - void onStateChange( - KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override; + void onStateChange(KUKA::FRI::ESessionState oldState, + KUKA::FRI::ESessionState newState) override; - // Потокобезопасное API для ros2_control, вызывается из read() и write() - void setTargetJointPositions(const std::array & q); - IIWAStateSnapshot getStateSnapshot() const; + // Configure before starting the FRI thread; limits come from the robot URDF. + void setLimits(const Joints & lower, const Joints & upper, const Joints & velocity); + void setTargetJointPositions(const Joints & q); + IIWAStateSnapshot getStateSnapshot() const; // lifecycle/non-RT use + bool tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const; bool isCommandingActive() const; KUKA::FRI::ESessionState getSessionState() const; private: - double joint_position_tau_; std::atomic session_state_{KUKA::FRI::IDLE}; - - mutable std::mutex data_mutex_; - std::array target_pos_{}; - // Сглаженная позиция, которую реально отправляем роботу. - // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте. - std::array filtered_pos_{}; - IIWAStateSnapshot snapshot_{}; - - // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна) - void captureMonitoringData(); - // Обновить snapshot_ вместе с ipo_pos (в Commanding-режиме) - void captureCommandingData(); + mutable std::mutex state_mutex_; + std::mutex command_mutex_; + Joints target_pos_{}; + Joints requested_pos_{}; + Joints sent_pos_{}; + Joints lower_, upper_, max_velocity_; + bool initialized_{false}; + bool requested_{false}; + std::chrono::steady_clock::time_point requested_at_{}; + std::chrono::steady_clock::time_point last_command_at_{}; + IIWAStateSnapshot snapshot_{}; // published under state_mutex_ + IIWAStateSnapshot current_{}; // owned exclusively by FRI thread + void captureData(bool commanding); + void publishState(); + void validateCommanding() const; }; - } // namespace iiwa_controller diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 79f231e..5cc7df1 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -8,10 +8,7 @@ #include #include -// В Jazzy 4.44.0+ нельзя переопределять export_state_interfaces() и export_command_interfaces(). -// Устаревший конструктор не регистрирует introspection-callback pal_statistics, -// из-за чего падает с segfault. Базовый класс сам создаёт интерфейсы из URDF. -// Данные читаем и пишем через handle-API: set_state() и get_command(). +// Jazzy 4.48: framework-managed interfaces and handle API. #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_component_interface_params.hpp" @@ -31,6 +28,7 @@ class IIWAHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface) + ~IIWAHardwareInterface() override; CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override; @@ -39,13 +37,16 @@ public: std::vector export_unlisted_state_interface_descriptions() override; - // Полный lifecycle: configure открывает сокет, activate запускает поток, + // Полный lifecycle: configure создаёт SDK, activate открывает сокет и запускает поток, // deactivate останавливает поток, cleanup освобождает FRI-объекты. CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -59,11 +60,6 @@ private: std::string robot_ip_; int fri_port_{30200}; bool simulate_{false}; - double joint_position_tau_{0.04}; - // EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей. - // joint_velocity_tau = 0 отключает фильтр (raw finite difference). - double joint_velocity_tau_{0.01}; - // Объекты FRI SDK std::unique_ptr fri_client_; std::unique_ptr connection_; @@ -74,6 +70,12 @@ private: std::thread fri_thread_; std::atomic fri_running_{false}; void friThreadFunc(); + void stopFRI(); + void releaseFRI(); + std::atomic fri_fault_{false}; + bool active_{false}; + IIWAStateSnapshot last_snapshot_{}; + std::array lower_, upper_, max_velocity_; // Хэндлы интерфейсов состояния, заполняются в on_activate std::array h_pos_; @@ -83,19 +85,15 @@ private: std::array h_cmd_pos_; - // Вычисление скорости: конечные разности + EMA-фильтр + // Вычисление скорости по измеренной позиции и меткам времени FRI. std::array prev_pos_{}; - std::array velocity_{}; // отфильтрованная скорость, публикуется в JTC - std::array velocity_raw_{}; // сырая скорость до фильтра + std::array velocity_{}; // измеренная скорость [рад/с] unsigned int last_ts_sec_{0}; unsigned int last_ts_nsec_{0}; bool velocity_initialized_{false}; void compute_velocity_(const IIWAStateSnapshot & snap); - // Отслеживание сессии FRI для обнаружения потери управления - KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE}; - rclcpp::Clock throttle_clock_{RCL_STEADY_TIME}; }; diff --git a/src/iiwa_controller/package.xml b/src/iiwa_controller/package.xml index 479aa24..efe48b0 100644 --- a/src/iiwa_controller/package.xml +++ b/src/iiwa_controller/package.xml @@ -17,6 +17,7 @@ realtime_tools std_msgs + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index bb856f5..056c682 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -1,155 +1,190 @@ #include "iiwa_controller/FRIClient.h" +#include #include -#include - -#include +#include +#include namespace iiwa_controller { +using namespace KUKA::FRI; -static const char * friStateName(KUKA::FRI::ESessionState s) +FRIClient::FRIClient() { - switch (s) { - case KUKA::FRI::IDLE: return "IDLE"; - case KUKA::FRI::MONITORING_WAIT: return "MONITORING_WAIT"; - case KUKA::FRI::MONITORING_READY: return "MONITORING_READY"; - case KUKA::FRI::COMMANDING_WAIT: return "COMMANDING_WAIT"; - case KUKA::FRI::COMMANDING_ACTIVE: return "COMMANDING_ACTIVE"; - default: return "UNKNOWN"; + lower_.fill(-std::numeric_limits::infinity()); + upper_.fill(std::numeric_limits::infinity()); + max_velocity_.fill(std::numeric_limits::infinity()); +} + +void FRIClient::setLimits(const Joints & lower, const Joints & upper, const Joints & velocity) +{ + lower_ = lower; + upper_ = upper; + max_velocity_ = velocity; +} + +void FRIClient::captureData(bool commanding) +{ + const auto & state = robotState(); + std::copy_n(state.getMeasuredJointPosition(), N_JOINTS, current_.measured_pos.begin()); + std::copy_n(state.getMeasuredTorque(), N_JOINTS, current_.measured_tau.begin()); + std::copy_n(state.getExternalTorque(), N_JOINTS, current_.external_tau.begin()); + current_.sample_time = state.getSampleTime(); + current_.session = state.getSessionState(); + current_.quality = state.getConnectionQuality(); + current_.safety = state.getSafetyState(); + current_.drives = state.getDriveState(); + current_.operation = state.getOperationMode(); + current_.control = state.getControlMode(); + current_.command_mode = commanding ? state.getClientCommandMode() : NO_COMMAND_MODE; + current_.overlay = commanding ? state.getOverlayType() : NO_OVERLAY; + current_.tracking_performance = commanding ? state.getTrackingPerformance() : 0.0; + current_.ipo_valid = commanding; + if (commanding) { + std::copy_n(state.getIpoJointPosition(), N_JOINTS, current_.ipo_pos.begin()); } -} - -FRIClient::FRIClient(double joint_position_tau) -: joint_position_tau_(joint_position_tau) -{ - target_pos_.fill(0.0); - filtered_pos_.fill(0.0); -} - -// Вызывается только в Monitor-состояниях. -// В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём. -void FRIClient::captureMonitoringData() -{ - std::memcpy( - snapshot_.measured_pos.data(), - robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double)); - std::memcpy( - snapshot_.measured_tau.data(), - robotState().getMeasuredTorque(), N_JOINTS * sizeof(double)); - std::memcpy( - snapshot_.external_tau.data(), - robotState().getExternalTorque(), N_JOINTS * sizeof(double)); - snapshot_.sample_time = robotState().getSampleTime(); - snapshot_.quality = robotState().getConnectionQuality(); - snapshot_.ipo_valid = false; - snapshot_.time_stamp_sec = robotState().getTimestampSec(); - snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec(); -} - -// Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE). -// В отличие от Monitor, здесь getIpoJointPosition() доступна. -void FRIClient::captureCommandingData() -{ - captureMonitoringData(); - std::memcpy( - snapshot_.ipo_pos.data(), - robotState().getIpoJointPosition(), N_JOINTS * sizeof(double)); - snapshot_.ipo_valid = true; - // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию — как в lbr_fri_ros2_stack. - // Благодаря этому JTC не видит расхождения и не генерирует коррекций. - snapshot_.measured_pos = filtered_pos_; -} - -// Вызывается в MONITORING_WAIT и MONITORING_READY -void FRIClient::monitor() -{ - std::lock_guard lock(data_mutex_); - captureMonitoringData(); -} - -// Вызывается в COMMANDING_WAIT. -// По документации FRI (п. 6.2.2) клиент должен отправлять команды в каждом цикле. -// Переход в COMMANDING_ACTIVE происходит только когда разница между commanded_position -// и IPO_position меньше 0.001 рад для всех суставов. -// Важно эхировать именно IPO-позицию, не measured. Если взять measured, -// статическое отклонение не даст выполниться этому условию. -void FRIClient::waitForCommand() -{ - std::lock_guard lock(data_mutex_); - captureCommandingData(); - - // Инициализируем цель и фильтр IPO-позицией. - // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE. - std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double)); - std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double)); - - robotCommand().setJointPosition(filtered_pos_.data()); -} - -// Вызывается в COMMANDING_ACTIVE, основной цикл управления -void FRIClient::command() -{ - std::lock_guard lock(data_mutex_); - - // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_ - // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом). - // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA. - const double dt = robotState().getSampleTime(); - const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0; - for (size_t i = 0; i < N_JOINTS; ++i) { - filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i]; - } - - robotCommand().setJointPosition(filtered_pos_.data()); - - // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил. - captureCommandingData(); -} - -void FRIClient::onStateChange( - KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) -{ - session_state_.store(newState, std::memory_order_relaxed); - - RCLCPP_INFO( - rclcpp::get_logger("FRIClient"), - "FRI смена состояния: %s, теперь %s", friStateName(oldState), friStateName(newState)); - - if (newState == KUKA::FRI::IDLE || - newState == KUKA::FRI::MONITORING_WAIT || - newState == KUKA::FRI::MONITORING_READY) + const auto sec = state.getTimestampSec(); + const auto nsec = state.getTimestampNanoSec(); + if (current_.valid && (sec < current_.time_stamp_sec || + (sec == current_.time_stamp_sec && nsec <= current_.time_stamp_nano_sec))) { + throw std::runtime_error("FRI timestamp did not advance"); } -} - -void FRIClient::setTargetJointPositions(const std::array & q) -{ - // До первой команды контроллера интерфейс содержит NaN. - // Если отправить NaN роботу в COMMANDING_ACTIVE, получим CK_COMPOUND_RETURN_ERROR. - for (const auto & v : q) { - if (!std::isfinite(v)) { - return; + if (nsec >= 1000000000 || !std::isfinite(current_.sample_time) || current_.sample_time <= 0.0) { + throw std::runtime_error("Invalid FRI timestamp/sample period"); + } + for (size_t i = 0; i < N_JOINTS; ++i) { + if (!std::isfinite(current_.measured_pos[i]) || !std::isfinite(current_.measured_tau[i]) || + !std::isfinite(current_.external_tau[i]) || + (commanding && !std::isfinite(current_.ipo_pos[i]))) + { + throw std::runtime_error("Non-finite FRI telemetry"); } } - std::lock_guard lock(data_mutex_); - target_pos_ = q; + current_.time_stamp_sec = sec; + current_.time_stamp_nano_sec = nsec; + current_.received_at = std::chrono::steady_clock::now(); + current_.valid = true; +} + +void FRIClient::publishState() +{ + std::unique_lock lock(state_mutex_, std::try_to_lock); + if (lock.owns_lock()) {snapshot_ = current_;} +} + +void FRIClient::validateCommanding() const +{ + if (current_.command_mode != POSITION || current_.overlay != JOINT || + (current_.control != POSITION_CONTROL_MODE && current_.control != JOINT_IMP_CONTROL_MODE && + current_.control != CART_IMP_CONTROL_MODE)) + { + throw std::runtime_error("Expected FRI POSITION command mode with JOINT overlay"); + } + if (current_.quality < GOOD || current_.safety != NORMAL_OPERATION || + current_.drives != ACTIVE) + { + throw std::runtime_error("FRI commanding unavailable: quality, safety or drives"); + } +} + +void FRIClient::monitor() +{ + captureData(false); + LBRClient::monitor(); + initialized_ = false; + publishState(); +} + +void FRIClient::waitForCommand() +{ + captureData(true); + validateCommanding(); + LBRClient::waitForCommand(); // SDK 1.16 mirrors IPO, not measured position. + sent_pos_ = target_pos_ = current_.ipo_pos; + initialized_ = true; + last_command_at_ = current_.received_at; + publishState(); +} + +void FRIClient::command() +{ + captureData(true); + validateCommanding(); + if (!initialized_) { + sent_pos_ = target_pos_ = current_.ipo_pos; + last_command_at_ = current_.received_at; + initialized_ = true; + } + { + std::unique_lock lock(command_mutex_, std::try_to_lock); + if (lock.owns_lock() && requested_ && requested_at_ > last_command_at_) { + target_pos_ = requested_pos_; + last_command_at_ = requested_at_; + } + } + // Local watchdog: a stalled ros2_control loop must not leave a moving target active. + if (current_.received_at - last_command_at_ > std::chrono::milliseconds(100)) { + throw std::runtime_error("FRI command watchdog expired"); + } + const double dt = current_.sample_time; + for (size_t i = 0; i < N_JOINTS; ++i) { + if (!std::isfinite(target_pos_[i]) || target_pos_[i] < lower_[i] || + target_pos_[i] > upper_[i]) + { + throw std::runtime_error("FRI position command outside configured limits"); + } + const double step = target_pos_[i] - sent_pos_[i]; + sent_pos_[i] += std::clamp(step, -max_velocity_[i] * dt, max_velocity_[i] * dt); + } + robotCommand().setJointPosition(sent_pos_.data()); + publishState(); +} + +void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) +{ + session_state_.store(newState); + if (oldState == COMMANDING_ACTIVE && newState != COMMANDING_ACTIVE) { + throw std::runtime_error("FRI left COMMANDING_ACTIVE; explicit recovery required"); + } +} + +void FRIClient::setTargetJointPositions(const Joints & q) +{ + for (size_t i = 0; i < N_JOINTS; ++i) { + if (!std::isfinite(q[i]) || q[i] < lower_[i] || q[i] > upper_[i]) { + throw std::invalid_argument("Invalid FRI position command"); + } + } + std::unique_lock lock(command_mutex_, std::try_to_lock); + if (lock.owns_lock()) { + requested_pos_ = q; + requested_at_ = std::chrono::steady_clock::now(); + requested_ = true; + } } IIWAStateSnapshot FRIClient::getStateSnapshot() const { - std::lock_guard lock(data_mutex_); + std::lock_guard lock(state_mutex_); return snapshot_; } +bool FRIClient::tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const +{ + std::unique_lock lock(state_mutex_, std::try_to_lock); + if (!lock.owns_lock()) {return false;} + snapshot = snapshot_; + return true; +} + bool FRIClient::isCommandingActive() const { - return session_state_.load(std::memory_order_relaxed) == KUKA::FRI::COMMANDING_ACTIVE; + return getSessionState() == COMMANDING_ACTIVE; } -KUKA::FRI::ESessionState FRIClient::getSessionState() const +ESessionState FRIClient::getSessionState() const { - return session_state_.load(std::memory_order_relaxed); + return session_state_.load(); } - } // namespace iiwa_controller diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 22ba74c..d6282ad 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -1,5 +1,10 @@ #include "iiwa_controller/IIWAHardwareInterface.hpp" +#include +#include +#include +#include +#include "friException.h" #include #include #include @@ -21,6 +26,32 @@ namespace iiwa_controller using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +// The SDK keeps a 100 ms receive bound after the first packet. During startup +// only, allow 15 s for the operator to start Sunrise without retrying failed SDK steps. +class StartupConnection : public KUKA::FRI::UdpConnection +{ +public: + explicit StartupConnection(const std::atomic & running) + : KUKA::FRI::UdpConnection(100), running_(running) {} + + int receive(char * buffer, int size) override + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15); + do { + const int received = KUKA::FRI::UdpConnection::receive(buffer, size); + if (received >= 0) { + started_ = true; + return received; + } + if (started_) {return received;} + } while (running_.load() && std::chrono::steady_clock::now() < deadline); + return -1; + } +private: + const std::atomic & running_; + bool started_{false}; +}; + static const char * LOG = "IIWAHardwareInterface"; static std::string getParam( @@ -43,18 +74,27 @@ CallbackReturn IIWAHardwareInterface::on_init( const auto & info = params.hardware_info; - robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); - fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); - simulate_ = (getParam(info, "simulate", "false") == "true"); - joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04")); - joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01")); - - RCLCPP_INFO(rclcpp::get_logger(LOG), - "on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f", - robot_ip_.c_str(), fri_port_, - simulate_ ? "true" : "false", - joint_position_tau_, - joint_velocity_tau_); + try { + robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); + size_t used = 0; + const auto port = getParam(info, "fri_port", "30200"); + fri_port_ = std::stoi(port, &used); + if (used != port.size() || fri_port_ < 1 || fri_port_ > 65535) { + throw std::invalid_argument("Invalid fri_port"); + } + const auto simulation = getParam(info, "simulate", "false"); + if (simulation != "true" && simulation != "false") { + throw std::invalid_argument("simulate must be true or false"); + } + simulate_ = simulation == "true"; + in_addr address{}; + if (!simulate_ && inet_pton(AF_INET, robot_ip_.c_str(), &address) != 1) { + throw std::invalid_argument("robot_ip must be an IPv4 address"); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Invalid hardware parameters: %s", e.what()); + return CallbackReturn::ERROR; + } if (info.joints.size() != N_JOINTS) { RCLCPP_FATAL(rclcpp::get_logger(LOG), @@ -62,9 +102,41 @@ CallbackReturn IIWAHardwareInterface::on_init( return CallbackReturn::ERROR; } + for (size_t i = 0; i < N_JOINTS; ++i) { + const auto & joint = info.joints[i]; + // SDK arrays are ordered A1..A7. Never silently permute axes. + if (joint.name != "joint" + std::to_string(i + 1) || + joint.command_interfaces.size() != 1 || joint.command_interfaces[0].name != "position") + { + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Expected joint1..joint7 with position commands"); + return CallbackReturn::ERROR; + } + for (const auto * name : {"position", "velocity", "effort"}) { + if (std::none_of(joint.state_interfaces.begin(), joint.state_interfaces.end(), + [&](const auto & interface) {return interface.name == name;})) + { + return CallbackReturn::ERROR; + } + } + const auto limits = info.limits.find(joint.name); + if (limits == info.limits.end() || !limits->second.has_position_limits || + !limits->second.has_velocity_limits) + { + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Missing position/velocity limits for %s", joint.name.c_str()); + return CallbackReturn::ERROR; + } + lower_[i] = limits->second.min_position; + upper_[i] = limits->second.max_position; + max_velocity_[i] = limits->second.max_velocity; + if (!std::isfinite(lower_[i]) || !std::isfinite(upper_[i]) || lower_[i] >= upper_[i] || + !std::isfinite(max_velocity_[i]) || max_velocity_[i] <= 0.0) + { + return CallbackReturn::ERROR; + } + } + prev_pos_.fill(0.0); velocity_.fill(0.0); - velocity_raw_.fill(0.0); return CallbackReturn::SUCCESS; } @@ -88,7 +160,7 @@ IIWAHardwareInterface::export_unlisted_state_interface_descriptions() } // ── on_configure ─────────────────────────────────────────────────────────────── -// Открывает UDP-сокет и создаёт FRI-объекты. Не запускает поток. +// Создаёт FRI-объекты. Сокет и поток запускаются при активации. CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State &) { @@ -97,143 +169,148 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State return CallbackReturn::SUCCESS; } - fri_client_ = std::make_unique(joint_position_tau_); - // 100 мс таймаут recvfrom — поток корректно завершится после disconnect(). - connection_ = std::make_unique(100); - app_ = std::make_unique(*connection_, *fri_client_); + releaseFRI(); + fri_client_ = std::make_unique(); + fri_client_->setLimits(lower_, upper_, max_velocity_); + connection_ = std::make_unique(fri_running_); + app_ = std::make_unique(*connection_, *fri_client_); + return CallbackReturn::SUCCESS; +} - if (!app_->connect(fri_port_, robot_ip_.c_str())) { - RCLCPP_FATAL(rclcpp::get_logger(LOG), - "Не удалось открыть UDP-сокет на порту %d (робот: %s)", fri_port_, robot_ip_.c_str()); +CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State & state) +{ + stopFRI(); + velocity_initialized_ = false; + last_snapshot_ = {}; + try { + for (size_t i = 0; i < N_JOINTS; ++i) { + const auto & name = info_.joints[i].name; + h_pos_[i] = get_state_interface_handle(name + "/position"); + h_vel_[i] = get_state_interface_handle(name + "/velocity"); + h_eff_[i] = get_state_interface_handle(name + "/effort"); + h_ext_[i] = get_state_interface_handle(name + "/external_torque"); + h_cmd_pos_[i] = get_command_interface_handle(name + "/position"); + } + if (!simulate_) { + // Recreate SDK sequence counters and buffers for every new session. + if (on_configure(state) != CallbackReturn::SUCCESS || + !app_->connect(fri_port_, robot_ip_.c_str())) + { + releaseFRI(); + return CallbackReturn::ERROR; + } + fri_fault_.store(false); + fri_running_.store(true); + fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15); + while (fri_running_.load() && std::chrono::steady_clock::now() < deadline) { + last_snapshot_ = fri_client_->getStateSnapshot(); + if (last_snapshot_.valid && last_snapshot_.quality >= KUKA::FRI::GOOD && + last_snapshot_.session >= KUKA::FRI::MONITORING_READY) {break;} + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + if (fri_fault_.load() || !last_snapshot_.valid || + last_snapshot_.quality < KUKA::FRI::GOOD || + last_snapshot_.session < KUKA::FRI::MONITORING_READY || + std::chrono::steady_clock::now() - last_snapshot_.received_at > std::chrono::milliseconds(100)) + { + stopFRI(); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI activation failed: no fresh ready session"); + return CallbackReturn::ERROR; + } + } + for (size_t i = 0; i < N_JOINTS; ++i) { + double position = 0.0; + if (simulate_) { + get_state(h_pos_[i], position, true); + if (!std::isfinite(position)) {position = 0.0;} + } else { + position = last_snapshot_.measured_pos[i]; + } + set_state(h_pos_[i], position, true); + set_state(h_vel_[i], 0.0, true); + set_state(h_eff_[i], simulate_ ? 0.0 : last_snapshot_.measured_tau[i], true); + set_state(h_ext_[i], simulate_ ? 0.0 : last_snapshot_.external_tau[i], true); + set_command(h_cmd_pos_[i], position, true); + } + } catch (const std::exception & e) { + stopFRI(); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Activation failed: %s", e.what()); return CallbackReturn::ERROR; } - - RCLCPP_INFO(rclcpp::get_logger(LOG), - "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...", - fri_port_, robot_ip_.c_str()); - + active_ = true; return CallbackReturn::SUCCESS; } -// ── on_activate ──────────────────────────────────────────────────────────────── -// Получает хэндлы интерфейсов, запускает FRI-поток и ждёт установки сессии. - -CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &) +void IIWAHardwareInterface::stopFRI() { - RCLCPP_INFO(rclcpp::get_logger(LOG), "Активация..."); - - for (size_t i = 0; i < N_JOINTS; ++i) { - const std::string & jn = info_.joints[i].name; - - h_pos_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION); - h_vel_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY); - h_eff_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT); - h_ext_[i] = get_state_interface_handle(jn + "/external_torque"); - h_cmd_pos_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION); - - if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i]) - { - RCLCPP_FATAL(rclcpp::get_logger(LOG), - "Не удалось получить хэндл интерфейса для сустава '%s'. " - "Проверьте объявление / в URDF.", - jn.c_str()); - return CallbackReturn::ERROR; - } - } - - if (simulate_) { - return CallbackReturn::SUCCESS; - } - - fri_running_.store(true, std::memory_order_relaxed); - fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this); - - constexpr int kTimeoutMs = 15000; - constexpr int kPollMs = 200; - for (int elapsed = 0; - fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs; - elapsed += kPollMs) - { - RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000, - "Ожидание FRI-сессии... (%d мс)", elapsed); - std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs)); - } - - if (fri_client_->getSessionState() == KUKA::FRI::IDLE) { - RCLCPP_ERROR(rclcpp::get_logger(LOG), - "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s", - kTimeoutMs / 1000, robot_ip_.c_str()); - } else { - RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI сессия установлена!"); - const auto snap = fri_client_->getStateSnapshot(); - prev_pos_ = snap.measured_pos; - last_ts_sec_ = snap.time_stamp_sec; - last_ts_nsec_ = snap.time_stamp_nano_sec; - velocity_.fill(0.0); - velocity_initialized_ = true; - } - - previous_session_state_ = fri_client_->getSessionState(); - return CallbackReturn::SUCCESS; + active_ = false; + fri_running_.store(false); + // Receive has a finite timeout. Never close a socket while step() uses it. + if (fri_thread_.joinable()) {fri_thread_.join();} + if (app_) {app_->disconnect();} } -// ── on_deactivate ────────────────────────────────────────────────────────────── -// Останавливает FRI-поток. FRI-объекты остаются — их очищает on_cleanup(). +void IIWAHardwareInterface::releaseFRI() +{ + stopFRI(); + // ClientApplication::~ClientApplication calls the connection by reference. + app_.reset(); + connection_.reset(); + fri_client_.reset(); +} + +IIWAHardwareInterface::~IIWAHardwareInterface() {releaseFRI();} CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация..."); - - if (!simulate_ && fri_running_.load()) { - fri_running_.store(false, std::memory_order_relaxed); - // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке. - // Только потом join(), иначе он зависнет навсегда. - if (app_) { app_->disconnect(); } - if (fri_thread_.joinable()) { fri_thread_.join(); } - RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен"); - } - - for (size_t i = 0; i < N_JOINTS; ++i) { - h_pos_[i] = h_vel_[i] = h_eff_[i] = h_ext_[i] = nullptr; - h_cmd_pos_[i] = nullptr; - } - + stopFRI(); velocity_initialized_ = false; return CallbackReturn::SUCCESS; } -// ── on_cleanup ───────────────────────────────────────────────────────────────── -// Освобождает FRI-объекты. Вызывается после on_deactivate(). - CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &) { - fri_client_.reset(); - connection_.reset(); - app_.reset(); + releaseFRI(); return CallbackReturn::SUCCESS; } -// ── friThreadFunc ────────────────────────────────────────────────────────────── +CallbackReturn IIWAHardwareInterface::on_shutdown(const rclcpp_lifecycle::State & state) +{ + return on_cleanup(state); +} + +CallbackReturn IIWAHardwareInterface::on_error(const rclcpp_lifecycle::State & state) +{ + return on_cleanup(state); +} void IIWAHardwareInterface::friThreadFunc() { - RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен"); - - while (fri_running_.load(std::memory_order_relaxed)) { - if (!app_->step()) { - RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000, - "FRI: step() вернул false, возможно потеряли соединение"); + try { + while (fri_running_.load()) { + if (!app_->step()) { + throw std::runtime_error("FRI step failed"); + } + if (fri_client_->getSessionState() == KUKA::FRI::IDLE) { + throw std::runtime_error("FRI session ended"); + } } + } catch (const KUKA::FRI::FRIException & e) { + fri_fault_.store(true); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI SDK: %s", e.getErrorMessage()); + } catch (const std::exception & e) { + fri_fault_.store(true); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI: %s", e.what()); + } catch (...) { + fri_fault_.store(true); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Unknown FRI failure"); } - - RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток завершён"); + fri_running_.store(false); } // ── compute_velocity_ ────────────────────────────────────────────────────────── -// Конечные разности + EMA-фильтр. -// int64-вычитание timestamp'ов предотвращает потерю точности при больших Unix-значениях. -// EMA-фильтр (joint_velocity_tau) убирает одиночные выбросы, которые видит JTC как -// скачки состояния и компенсирует агрессивными командами → хруст двигателей. +// Velocity is the finite difference of measured positions at robot timestamps. void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) { @@ -242,7 +319,6 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) last_ts_sec_ = snap.time_stamp_sec; last_ts_nsec_ = snap.time_stamp_nano_sec; velocity_.fill(0.0); - velocity_raw_.fill(0.0); velocity_initialized_ = true; return; } @@ -258,23 +334,10 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) (static_cast(snap.time_stamp_nano_sec) - static_cast(last_ts_nsec_)) * 1e-9; - static constexpr std::array kMaxVel = - {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14}; - static constexpr double kVelDeadband = 1e-4; if (dt > 0.0) { - // EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра) - const double vel_alpha = (joint_velocity_tau_ > 0.0) - ? dt / (joint_velocity_tau_ + dt) - : 1.0; - for (size_t i = 0; i < N_JOINTS; ++i) { - const double raw = (snap.measured_pos[i] - prev_pos_[i]) / dt; - const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]); - velocity_raw_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped; - - // EMA: velocity_[i] = alpha * raw + (1 - alpha) * prev_filtered - velocity_[i] = vel_alpha * velocity_raw_[i] + (1.0 - vel_alpha) * velocity_[i]; + velocity_[i] = (snap.measured_pos[i] - prev_pos_[i]) / dt; } } @@ -288,10 +351,13 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) hardware_interface::return_type IIWAHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { + if (!active_) {return hardware_interface::return_type::OK;} if (simulate_) { for (size_t i = 0; i < N_JOINTS; ++i) { double pos = 0.0; - get_command(h_cmd_pos_[i], pos, false); + if (!get_command(h_cmd_pos_[i], pos, false) || !std::isfinite(pos)) { + return hardware_interface::return_type::ERROR; + } set_state(h_pos_[i], pos, false); set_state(h_vel_[i], 0.0, false); set_state(h_eff_[i], 0.0, false); @@ -300,19 +366,17 @@ hardware_interface::return_type IIWAHardwareInterface::read( return hardware_interface::return_type::OK; } - const auto snap = fri_client_->getStateSnapshot(); - - // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE. - const auto current_state = fri_client_->getSessionState(); - if (previous_session_state_ == KUKA::FRI::COMMANDING_ACTIVE && - current_state != KUKA::FRI::COMMANDING_ACTIVE) + if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) { + return hardware_interface::return_type::ERROR; + } + fri_client_->tryGetStateSnapshot(last_snapshot_); + const auto & snap = last_snapshot_; + if (!snap.valid || std::chrono::steady_clock::now() - snap.received_at > + std::chrono::milliseconds(100)) { - RCLCPP_ERROR(rclcpp::get_logger(LOG), - "Робот вышел из COMMANDING_ACTIVE! Деактивируйте и повторно активируйте контроллер."); + fri_running_.store(false); return hardware_interface::return_type::ERROR; } - previous_session_state_ = current_state; - compute_velocity_(snap); for (size_t i = 0; i < N_JOINTS; ++i) { @@ -330,21 +394,23 @@ hardware_interface::return_type IIWAHardwareInterface::read( hardware_interface::return_type IIWAHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration &) { - if (simulate_) { - return hardware_interface::return_type::OK; + if (!active_ || simulate_) {return hardware_interface::return_type::OK;} + if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) { + return hardware_interface::return_type::ERROR; } - - if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) { - return hardware_interface::return_type::OK; - } - + if (!fri_client_->isCommandingActive()) {return hardware_interface::return_type::OK;} std::array pos_cmd{}; for (size_t i = 0; i < N_JOINTS; ++i) { - get_command(h_cmd_pos_[i], pos_cmd[i], false); + if (!get_command(h_cmd_pos_[i], pos_cmd[i], false)) { + return hardware_interface::return_type::OK; // retry next cycle; watchdog stays armed + } + if (!std::isfinite(pos_cmd[i]) || pos_cmd[i] < lower_[i] || pos_cmd[i] > upper_[i]) { + fri_fault_.store(true); + fri_running_.store(false); + return hardware_interface::return_type::ERROR; + } } - fri_client_->setTargetJointPositions(pos_cmd); - return hardware_interface::return_type::OK; } diff --git a/src/iiwa_controller/test/smoke_jtc.py b/src/iiwa_controller/test/smoke_jtc.py new file mode 100644 index 0000000..afc7a29 --- /dev/null +++ b/src/iiwa_controller/test/smoke_jtc.py @@ -0,0 +1,59 @@ +"""Manual local JTC integration check; never opens a FRI connection.""" +import os +import signal +import subprocess +import tempfile +from pathlib import Path +import xacro +import yaml + +root = Path(__file__).resolve().parents[3] +work = Path(tempfile.mkdtemp(prefix='iiwa-controller-smoke-')) +env = dict(os.environ, ROS_DOMAIN_ID='83', ROS_LOCALHOST_ONLY='1', ROS_LOG_DIR=str(work / 'logs'), ROS_HOME=str(work / 'ros')) +env['CYCLONEDDS_URI'] = 'false' +xml = xacro.process_file(str(root / 'src/iiwa_description/urdf/iiwa7.urdf.xacro')).toxml() +assert 'false' in xml +# Exercise our plugin's explicit local simulation path, with no FRI socket. +xml = xml.replace('false', 'true') +params = work / 'robot.yaml' +params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}})) +processes = [] +handles = [] +try: + for package, executable, extra in [ + ('robot_state_publisher', 'robot_state_publisher', []), + ('controller_manager', 'ros2_control_node', ['--params-file', str(root / 'src/iiwa_config/config/moveit/iiwa_controller.yaml')]), + ]: + log = (work / (executable + '.log')).open('w') + handles.append(log) + processes.append(subprocess.Popen(['ros2', 'run', package, executable, '--ros-args', '--params-file', str(params)] + extra, + env=env, stdout=log, stderr=subprocess.STDOUT, start_new_session=True)) + for name in ['joint_state_broadcaster', 'iiwa_arm_controller']: + result = subprocess.run(['ros2', 'run', 'controller_manager', 'spawner', name, '--controller-manager-timeout', '15'], + env=env, text=True, capture_output=True, timeout=25) + print(result.stdout, result.stderr, flush=True) + assert result.returncode == 0, name + result = subprocess.run(['ros2', 'control', 'list_controllers'], env=env, text=True, capture_output=True, timeout=10) + print(result.stdout, result.stderr, flush=True) + assert result.returncode == 0 and 'iiwa_arm_controller' in result.stdout and 'active' in result.stdout + initial = yaml.safe_load((root / 'src/iiwa_config/config/moveit/initial_positions.yaml').read_text())['initial_positions'] + positions = [float(initial[f'joint{i}']) for i in range(1, 8)] + positions[0] += 0.02 + goal = {'trajectory': {'joint_names': [f'joint{i}' for i in range(1, 8)], + 'points': [{'positions': positions, 'time_from_start': {'sec': 2, 'nanosec': 0}}]}} + result = subprocess.run(['ros2', 'action', 'send_goal', '/iiwa_arm_controller/follow_joint_trajectory', + 'control_msgs/action/FollowJointTrajectory', yaml.safe_dump(goal)], + env=env, text=True, capture_output=True, timeout=15) + print(result.stdout, result.stderr, flush=True) + assert result.returncode == 0 and 'SUCCEEDED' in result.stdout + print('SMOKE PASS: actual URDF, iiwa hardware plugin, broadcaster and JTC active; no FRI connection.', flush=True) +finally: + for process in processes: + os.killpg(process.pid, signal.SIGINT) + for process in processes: + try: process.wait(timeout=5) + except subprocess.TimeoutExpired: os.killpg(process.pid, signal.SIGKILL) + for handle in handles: handle.close() + for log in work.glob('*.log'): + print('\n' + log.name + '\n' + log.read_text()[-4500:]) + print('Logs:', work) diff --git a/src/iiwa_controller/test/test_fri_client.cpp b/src/iiwa_controller/test/test_fri_client.cpp new file mode 100644 index 0000000..76dfd50 --- /dev/null +++ b/src/iiwa_controller/test/test_fri_client.cpp @@ -0,0 +1,174 @@ +#include +#include +#include +#include +#include +#include +#include "iiwa_controller/FRIClient.h" +#include "friClientData.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + +class TestApplication : public ClientApplication +{ +public: + using ClientApplication::ClientApplication; + ClientData * data() {return _data;} +}; + +class ClientFixture : public iiwa_controller::FRIClient +{ +public: + ClientFixture() : FRIClient(), app(connection, *this), data(app.data()) + { + auto & m = data->monitoringMsg; + m.connectionInfo.sendPeriod = 10; + m.connectionInfo.quality = FRIConnectionQuality_GOOD; + m.robotInfo.safetyState = SafetyState_NORMAL_OPERATION; + m.robotInfo.operationMode = OperationMode_AUTOMATIC_MODE; + m.robotInfo.controlMode = ControlMode_POSITION_CONTROLMODE; + auto * drives = static_cast(m.robotInfo.driveState.arg); + std::fill_n(drives->value, 7, DriveState_ACTIVE); + m.ipoData.clientCommandMode = ClientCommandMode_POSITION; + m.ipoData.overlayType = OverlayType_JOINT; + m.ipoData.has_trackingPerformance = false; + m.monitorData.timestamp.sec = 1; + m.monitorData.timestamp.nanosec = 0; + fill(m.monitorData.measuredJointPosition, 0.2); + fill(m.monitorData.commandedJointPosition, 0.3); + fill(m.monitorData.measuredTorque, 1.0); + fill(m.monitorData.externalTorque, 0.1); + fill(m.ipoData.jointPosition, 0.4); + } + static void fill(JointValues & values, double value) + { + auto * args = static_cast(values.value.arg); + std::fill_n(args->value, 7, value); + args->size = 7; + } + void cycle(ESessionState state) + { + data->resetCommandMessage(); + data->monitoringMsg.ipoData.has_jointPosition = state >= COMMANDING_WAIT; + data->monitoringMsg.connectionInfo.sessionState = static_cast(state); + onStateChange(data->lastState, state); + data->lastState = state; + data->monitoringMsg.monitorData.timestamp.nanosec += 10000000; + if (state == COMMANDING_WAIT) waitForCommand(); + else if (state == COMMANDING_ACTIVE) command(); + else monitor(); + } + double sentPosition() + { + return static_cast( + data->commandMsg.commandData.jointPosition.value.arg)->value[0]; + } + UdpConnection connection; + TestApplication app; + ClientData * data; +}; + +TEST(FRIClient, ReportsMeasuredPositionInCommanding) +{ + ClientFixture client; + client.cycle(COMMANDING_WAIT); + EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4); + EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2); + client.cycle(COMMANDING_ACTIVE); + EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2); +} + +TEST(FRIClient, PreservesSdkMonitoringReplyWithoutIpo) +{ + ClientFixture client; + client.cycle(MONITORING_READY); + EXPECT_TRUE(client.data->commandMsg.commandData.has_jointPosition); + EXPECT_DOUBLE_EQ(client.sentPosition(), 0.3); + EXPECT_FALSE(client.getStateSnapshot().ipo_valid); +} + +TEST(FRIClient, RejectsUnsupportedCommandModeBeforeSending) +{ + ClientFixture client; + client.data->monitoringMsg.ipoData.clientCommandMode = ClientCommandMode_TORQUE; + EXPECT_THROW(client.cycle(COMMANDING_WAIT), std::runtime_error); + EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition); +} + +TEST(FRIClient, PositionCommandsHaveNoEmaDelay) +{ + ClientFixture client; + client.cycle(COMMANDING_WAIT); + client.cycle(COMMANDING_ACTIVE); + iiwa_controller::FRIClient::Joints q; + q.fill(0.41); + client.setTargetJointPositions(q); + client.cycle(COMMANDING_ACTIVE); + EXPECT_DOUBLE_EQ(client.sentPosition(), 0.41); + EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2); +} + +TEST(FRIClient, RejectsInvalidCommandsAndBoundsCommandRate) +{ + ClientFixture client; + iiwa_controller::FRIClient::Joints lower, upper, velocity, q; + lower.fill(-1.0); upper.fill(1.0); velocity.fill(1.0); + client.setLimits(lower, upper, velocity); + client.cycle(COMMANDING_WAIT); + client.cycle(COMMANDING_ACTIVE); + q.fill(0.8); + client.setTargetJointPositions(q); + client.cycle(COMMANDING_ACTIVE); + EXPECT_NEAR(client.sentPosition(), 0.41, 1e-12); // 1 rad/s * 10 ms + q[0] = 1.1; + EXPECT_THROW(client.setTargetJointPositions(q), std::invalid_argument); + q[0] = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(client.setTargetJointPositions(q), std::invalid_argument); +} + +TEST(FRIClient, SafetyStopCannotSendApplicationCommand) +{ + ClientFixture client; + client.cycle(COMMANDING_WAIT); + client.data->monitoringMsg.robotInfo.safetyState = SafetyState_SAFETY_STOP_LEVEL_1; + EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error); + EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition); +} + +TEST(FRIClient, DuplicateTimestampIsNotFreshTelemetry) +{ + ClientFixture client; + client.cycle(MONITORING_READY); + EXPECT_THROW(client.monitor(), std::runtime_error); +} + +TEST(FRIClient, LeavingCommandingRequiresExplicitRecovery) +{ + ClientFixture client; + client.cycle(COMMANDING_WAIT); + client.cycle(COMMANDING_ACTIVE); + EXPECT_THROW(client.cycle(MONITORING_READY), std::runtime_error); +} + +TEST(FRIClient, PositionOverlaySupportsSunriseImpedanceModes) +{ + for (auto mode : {ControlMode_JOINT_IMPEDANCE_CONTROLMODE, + ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE}) + { + ClientFixture client; + client.data->monitoringMsg.robotInfo.controlMode = mode; + EXPECT_NO_THROW(client.cycle(COMMANDING_WAIT)); + EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4); + } +} + +TEST(FRIClient, StalledRosCommandStreamStopsBeforeSending) +{ + ClientFixture client; + client.cycle(COMMANDING_WAIT); + client.cycle(COMMANDING_ACTIVE); + std::this_thread::sleep_for(std::chrono::milliseconds(110)); + EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error); + EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition); +} diff --git a/src/iiwa_controller/test/test_hardware.cpp b/src/iiwa_controller/test/test_hardware.cpp new file mode 100644 index 0000000..75e6f11 --- /dev/null +++ b/src/iiwa_controller/test/test_hardware.cpp @@ -0,0 +1,93 @@ +#include +#include "iiwa_controller/IIWAHardwareInterface.hpp" + +using Hardware = iiwa_controller::IIWAHardwareInterface; +using Return = hardware_interface::CallbackReturn; + +hardware_interface::HardwareComponentInterfaceParams parameters() +{ + hardware_interface::HardwareComponentInterfaceParams params; + auto & info = params.hardware_info; + info.name = "iiwa_test"; + info.type = "system"; + info.hardware_parameters["simulate"] = "true"; + for (int i = 1; i <= 7; ++i) { + hardware_interface::ComponentInfo joint; + joint.name = "joint" + std::to_string(i); + for (const auto * name : {"position", "velocity", "effort"}) { + hardware_interface::InterfaceInfo interface; + interface.name = name; + interface.data_type = "double"; + interface.initial_value = "0.2"; + joint.state_interfaces.push_back(interface); + } + hardware_interface::InterfaceInfo command; + command.name = "position"; + command.data_type = "double"; + joint.command_interfaces.push_back(command); + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -2.0; + limits.max_position = 2.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + info.limits[joint.name] = limits; + info.joints.push_back(joint); + } + return params; +} + +TEST(Hardware, RejectsMalformedPortWithoutThrowing) +{ + Hardware hardware; + auto params = parameters(); + params.hardware_info.hardware_parameters["fri_port"] = "30200oops"; + EXPECT_EQ(hardware.on_init(params), Return::ERROR); +} + +TEST(Hardware, RejectsWrongJointOrderAndMissingInterfaces) +{ + auto params = parameters(); + std::swap(params.hardware_info.joints[0], params.hardware_info.joints[1]); + Hardware wrong_order; + EXPECT_EQ(wrong_order.on_init(params), Return::ERROR); + params = parameters(); + params.hardware_info.joints[0].state_interfaces.pop_back(); + Hardware missing_interface; + EXPECT_EQ(missing_interface.on_init(params), Return::ERROR); +} + +TEST(Hardware, SimulationSeedsCommandsAndSupportsRepeatedLifecycle) +{ + Hardware hardware; + ASSERT_EQ(hardware.on_init(parameters()), Return::SUCCESS); + auto states = hardware.on_export_state_interfaces(); + auto commands = hardware.on_export_command_interfaces(); + const rclcpp_lifecycle::State state; + ASSERT_EQ(hardware.on_configure(state), Return::SUCCESS); + for (int cycle = 0; cycle < 2; ++cycle) { + ASSERT_EQ(hardware.on_activate(state), Return::SUCCESS); + const double command = hardware.get_command("joint1/position"); + EXPECT_DOUBLE_EQ(command, 0.2); + EXPECT_EQ(hardware.read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + hardware_interface::return_type::OK); + EXPECT_EQ(hardware.on_deactivate(state), Return::SUCCESS); + EXPECT_EQ(hardware.read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + hardware_interface::return_type::OK); + } + EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS); + EXPECT_EQ(hardware.on_shutdown(state), Return::SUCCESS); +} + +TEST(Hardware, ConfiguredRealHardwareCleansUpInDependencyOrder) +{ + Hardware hardware; + auto params = parameters(); + params.hardware_info.hardware_parameters["simulate"] = "false"; + params.hardware_info.hardware_parameters["robot_ip"] = "127.0.0.1"; + ASSERT_EQ(hardware.on_init(params), Return::SUCCESS); + const rclcpp_lifecycle::State state; + ASSERT_EQ(hardware.on_configure(state), Return::SUCCESS); + EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS); + EXPECT_EQ(hardware.on_error(state), Return::SUCCESS); +} diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 917ee7b..801b080 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -6,8 +6,6 @@ - - @@ -146,8 +144,6 @@ $(arg robot_ip) $(arg fri_port) false - $(arg joint_position_tau) - $(arg joint_velocity_tau) diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index 0161ab4..bfe4da0 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -15,8 +15,6 @@ class RobotCfg: port: int description: str fri_cycle_ms: int - joint_position_tau: float - active_controller: str # "jtc" | "forward" @dataclass(frozen=True) @@ -290,8 +288,6 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: port=int(require(robot_raw, "port")), description=resolve_path(str(require(robot_raw, "description")), settings_dir), fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)), - joint_position_tau=float(robot_raw.get("joint_position_tau", 0.04)), - active_controller=str(robot_raw.get("active_controller", "jtc")), ) # digital_twin