diff --git a/src/iiwa_controller/README.en.md b/src/iiwa_controller/README.en.md deleted file mode 100644 index 2907b2c..0000000 --- a/src/iiwa_controller/README.en.md +++ /dev/null @@ -1,101 +0,0 @@ -# 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 worker owns SDK `step()`. Each step is followed by exactly one -`read → JTC update → write` exchange. `read()` waits for a fresh snapshot; -`write()` releases the next SDK step. Waits are bounded to 100 ms. The computed -command is sent in the next SDK step, a one-cycle pipeline without EMA or a -queue of skipped points. Network I/O stays in the worker. The real-hardware -launch requires Jazzy Controller Manager **4.48.0 or later** and enables -`hardware_synchronization.expect_blocking_read_write`, using its -[hardware-paced control loop](https://github.com/ros-controls/ros2_control/blob/jazzy/controller_manager/src/ros2_control_node.cpp) -without an independent timer. `robot.fri_cycle_ms` is checked against Sunrise -telemetry. 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, timestamp gaps in COMMANDING_ACTIVE, -mismatched sample periods, 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. - -Initial 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. - -2026-09-16 correction: reproduced a doubled 0.001 → 0.002 rad step when -independent cycles overwrite a command. Added synchronization, overwrite, -stop/timeout, sample-period and packet-gap tests. Synchronization prevents -inter-thread point loss; it does not by itself guarantee freedom from OS or -network delays. Elimination of physical drive knocking remains unverified; -the original hardware-test logs were not retained. diff --git a/src/iiwa_controller/README.md b/src/iiwa_controller/README.md deleted file mode 100644 index a6e7402..0000000 --- a/src/iiwa_controller/README.md +++ /dev/null @@ -1,109 +0,0 @@ -# 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()`. После каждого шага он -ожидает ровно один цикл `read → JTC update → write`; `read()` ожидает новый -снимок, `write()` разрешает следующий шаг SDK. Ожидания ограничены 100 мс. -Команда этого расчёта отправляется в следующем шаге SDK: задержка — один цикл, -без EMA и очереди пропущенных точек. Сетевой ввод-вывод остаётся в рабочем потоке. -Launch включает `hardware_synchronization.expect_blocking_read_write` и требует -Controller Manager **4.48.0 или новее** в Jazzy. Это режим -[синхронизации цикла с оборудованием](https://github.com/ros-controls/ros2_control/blob/jazzy/controller_manager/src/ros2_control_node.cpp), -без второго независимого таймера. Период из `robot.fri_cycle_ms` проверяется -по телеметрии Sunrise. До первого UDP-пакета допускается ожидание Sunrise -до 15 с; после первого пакета ошибка `step()` завершает поток. Активация требует -свежей сессии как минимум MONITORING_READY и качества GOOD/EXCELLENT. - -При отсутствии пакетов/команд 100 мс, пропуске timestamp в COMMANDING_ACTIVE, -несовпадении периода, нерастущем 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 не запускались. - -Исправление 16.09.2026: воспроизведено удвоение шага 0,001 → 0,002 рад при -перезаписи команды независимыми циклами. Добавлены тесты синхронизации, -запрета перезаписи, остановки/таймаута, периода и пропуска пакетов. Синхронизация -устраняет потерю точек между потоками, но сама по себе не гарантирует отсутствие -задержек планировщика ОС или сети. Исчезновение стука на физическом роботе пока -не подтверждено; исходные журналы испытания не сохранились.