docs: add FRI realtime scheduling guide

cobot-setting.yaml

@@ -1,8 +1,9 @@
 robot:
   name: "iiwa7"
-  ip: "192.170.10.2"
+  ip: "192.168.21.147"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
+  joint_position_tau: 0.04  # EMA позиций FRI [с]; 0 отключает сглаживание
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 

cobot/commands/update.py

@@ -4,13 +4,23 @@ import argparse
 import subprocess
 from pathlib import Path
 
-from cobot import process, ui
+from cobot import process
 from cobot.ui import done
 from cobot.process import StepProgress
 
 _PROJECT_DIR = Path(__file__).parent.parent.parent
 
 
+def _plain(message: str) -> None:
+    """Print after replacing this command's own uv environment.
+
+    Rich modules loaded before ``uv tool install`` can no longer be complete after
+    uv replaces the editable tool environment. Do not render with Rich after that
+    command returns.
+    """
+    print(message, flush=True)
+
+
 def _git(*args: str) -> str:
     """Run a git command in the project dir and return its stripped stdout.
     Запускает git-команду в директории проекта и возвращает обрезанный stdout.
@@ -18,13 +28,12 @@ def _git(*args: str) -> str:
     return subprocess.check_output(["git", *args], cwd=_PROJECT_DIR, text=True).strip()
 
 
-def _update() -> None:
+def _update() -> int:
     """Fetch the current branch, show incoming commits, pull, and reinstall the cobot CLI.
     Progress: fetch (0-30 %), pull (30-80 %), reinstall (80-100 %).
     Получает текущую ветку, показывает входящие коммиты, делает pull и переустанавливает CLI.
     Прогресс: fetch (0-30 %), pull (30-80 %), переустановка (80-100 %).
     """
-    ok, fail_msg = True, ""
     with StepProgress("Обновление проекта") as p:
         try:
             branch = _git("rev-parse", "--abbrev-ref", "HEAD")
@@ -32,17 +41,15 @@ def _update() -> None:
 
             p.set(0, "Получение с удалённого репозитория...")
             rc = process.stream(["git", "fetch", "origin"], cwd=str(_PROJECT_DIR), on_line=p.log)
-            if rc not in (0, -9, -15):
+            if rc != 0:
-                done(False, "git fetch завершился с ошибкой")
+                done(False, f"git fetch завершился с кодом {rc}")
-                return
+                return 1
             p.set(30)
 
             behind = _git("rev-list", f"HEAD..origin/{branch}", "--count")
             if behind == "0":
-                p.set(100, "Уже актуально")
+                p.log("Исходники уже актуальны; проверка установки CLI...")
-                done(True, "Уже актуальная версия")
+            else:
-                return
-
                 p.raw(f"\n[bold]{behind} новых коммит(ов):[/bold]")
                 for line in _git("log", f"HEAD..origin/{branch}", "--oneline").splitlines():
                     p.log(line)
@@ -50,24 +57,33 @@ def _update() -> None:
                 p.set(30, "Применение изменений...")
                 rc = process.stream(["git", "pull", "origin", branch],
                                    cwd=str(_PROJECT_DIR), on_line=p.log)
-            if rc not in (0, -9, -15):
+                if rc != 0:
-                done(False, "git pull завершился с ошибкой")
+                    done(False, f"git pull завершился с кодом {rc}")
-                return
+                    return 1
             p.set(80)
+        except (subprocess.CalledProcessError, OSError) as exc:
+            done(False, str(exc))
+            return 1
 
-            p.set(80, "Переустановка cobot CLI...")
+    # uv can replace the Rich package of this running CLI. Keep this stage outside
-            p.raw("\n[cyan]▸[/cyan] Переустановка cobot CLI...")
+    # StepProgress and only use built-in print afterwards.
-            rc = process.stream(["uv", "tool", "install", "--editable", str(_PROJECT_DIR)],
+    _plain("\n▸ Переустановка cobot CLI...")
-                               on_line=p.log)
+    try:
-            if rc in (0, -9, -15):
+        rc = process.stream(
-                p.raw("[green]✓[/green] cobot переустановлен")
+            ["uv", "tool", "install", "--editable", str(_PROJECT_DIR)],
-            else:
+            on_line=lambda line: _plain(f"  {line}"),
-                p.raw("[yellow]Предупреждение:[/yellow] переустановка не удалась")
+        )
-            p.set(100, "Готово")
+    except OSError as exc:
-        except subprocess.CalledProcessError as exc:
+        _plain(f"✗ Не удалось запустить установщик: {exc}")
-            ok, fail_msg = False, str(exc)
+        return 1
-
+    if rc != 0:
-    done(ok, "Проект обновлён" if ok else fail_msg)
+        _plain(
+            f"✗ Установка CLI завершилась с кодом {rc}. Исходники обновлены, "
+            "но установка не завершена. Сохраните вывод выше и повторите cobot update."
+        )
+        return 1
+    _plain("✓ Проект обновлён и cobot переустановлен")
+    return 0
 
 
 def register(subparsers: argparse._SubParsersAction) -> None:
@@ -76,4 +92,4 @@ def register(subparsers: argparse._SubParsersAction) -> None:
 
 
 def run(args: argparse.Namespace) -> None:
-    _update()
+    raise SystemExit(_update())

cobot/test/test_update.py

@@ -0,0 +1,71 @@
+"""Update recovery checks; external git and installer operations are isolated."""
+
+import io
+import unittest
+from unittest.mock import patch
+
+from rich.console import Console
+
+from cobot.commands import update
+
+
+class UpdateTests(unittest.TestCase):
+    def run_update(self, codes, behind="1"):
+        output = io.StringIO()
+        commands = []
+
+        def stream(command, **kwargs):
+            commands.append(command)
+            result = next(codes, 0)
+            if isinstance(result, Exception):
+                raise result
+            return result
+
+        def git(*args):
+            return {"rev-parse": "controller", "rev-list": behind,
+                    "log": "abc123 Update CLI"}[args[0]]
+
+        console = Console(file=output, color_system=None)
+        with patch.object(update, "_git", side_effect=git), \
+                patch.object(update.process, "stream", side_effect=stream), \
+                patch.object(update, "_plain"), \
+                patch("cobot.process.console", console), \
+                patch("cobot.ui.console", console):
+            code = update._update()
+        return code, output.getvalue(), commands
+
+    def test_install_failure_reports_failure(self):
+        for install_code in (1, -9, -15):
+            with self.subTest(install_code=install_code):
+                code, output, _ = self.run_update(iter([0, 0, install_code]))
+                self.assertEqual(code, 1)
+                self.assertNotIn("Проект обновлён", output)
+
+    def test_current_checkout_still_retries_installation(self):
+        code, _, commands = self.run_update(iter([0, 0]), behind="0")
+        self.assertEqual([command[:2] for command in commands],
+                         [["git", "fetch"], ["uv", "tool"]])
+        self.assertEqual(code, 0)
+
+    def test_killed_git_stops_before_next_step(self):
+        for codes, expected_count in [([-9], 1), ([0, -15], 2)]:
+            with self.subTest(codes=codes):
+                code, output, commands = self.run_update(iter(codes))
+                self.assertNotEqual(code, 0)
+                self.assertEqual(len(commands), expected_count)
+                self.assertNotIn("Проект обновлён", output)
+
+    def test_missing_installer_reports_failure_without_traceback(self):
+        code, _, _ = self.run_update(
+            iter([0, 0, FileNotFoundError("uv not found")]))
+        self.assertNotEqual(code, 0)
+
+    def test_command_exits_nonzero_after_failed_installation(self):
+        with patch.object(update, "_update", return_value=1):
+            with self.assertRaises(SystemExit) as result:
+                update.run(None)
+        self.assertEqual(result.exception.code, 1)
+
+
+if __name__ == "__main__":
+    unittest.main()

doc/lwc-doc/docs/getting-started/configuration.en.md

@@ -35,9 +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` | FRI command EMA time constant, s | `0.04`; `0` disables smoothing |
 | `description` | Path to the robot URDF | Do not change |
 
-Motion always uses JointTrajectoryController. Position and velocity EMA filters are not applied.
+Motion always uses JointTrajectoryController. The position command is smoothed before FRI transmission with `robot.joint_position_tau`; no velocity filter is applied.
 
 ---
 

doc/lwc-doc/docs/getting-started/configuration.md

@@ -37,9 +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 команд FRI, с | `0.04`; `0` отключает сглаживание |
 | `description` | Путь к URDF-описанию робота | Не менять |
 
-Управление движением всегда выполняет JointTrajectoryController. EMA-фильтры положения и скорости не применяются.
+Управление движением всегда выполняет JointTrajectoryController. Перед отправкой в FRI позиционная команда сглаживается EMA с постоянной времени `robot.joint_position_tau`; фильтр скорости не применяется.
 
 ---
 

doc/lwc-doc/docs/realtime-fri.en.md

@@ -0,0 +1,84 @@
+# FIFO priority for FRI
+
+## Why it is needed
+
+FRI exchanges a position command on every cycle. With a 10 ms period, a new packet should be handled about every 10 ms. The regular Linux scheduler can delay the control thread behind other processes. The log then contains messages such as:
+
+```text
+Could not enable FIFO RT scheduling: Operation not permitted
+Overrun might occur ... Read time: 11500 us
+```
+
+`SCHED_FIFO` is Linux scheduling for threads with fixed priority. This project requests priority 50 for `controller_manager` and priority 80 for the FRI worker. The FRI worker receives the higher priority so it can receive a KUKA packet and send the reply on time.
+
+Configure this on the **ROS 2 computer** that runs `cobot run`. It does not require changes to the Sunrise Cabinet or `cobot-setting.yaml`.
+
+## Ubuntu 24.04 setup
+
+On the ROS 2 computer, run these commands as the user who starts `cobot`:
+
+```bash
+sudo groupadd -f realtime
+sudo usermod -aG realtime "$USER"
+sudo tee /etc/security/limits.d/99-ros2-realtime.conf >/dev/null <<'EOF'
+@realtime soft rtprio 99
+@realtime soft priority 99
+@realtime soft memlock unlimited
+@realtime hard rtprio 99
+@realtime hard priority 99
+@realtime hard memlock unlimited
+EOF
+```
+
+Log out of the desktop session completely and log in again, or reboot. An already open terminal does not receive the new group and limits.
+
+## Verification
+
+In a new terminal, verify the group and limits:
+
+```bash
+id -nG | tr ' ' '\n' | grep -x realtime
+ulimit -r
+ulimit -l
+```
+
+The expected output is `realtime`, then `99`, then `unlimited`.
+
+Start FRI without commanding robot motion and check the log for:
+
+```text
+FRI worker uses FIFO priority 80
+```
+
+You can also inspect the policies of all threads in the process:
+
+```bash
+PID=$(pgrep -n ros2_control_node)
+ps -L -p "$PID" -o pid,tid,cls,rtprio,pri,comm
+```
+
+Realtime threads have `FF` in the `CLS` column. The FRI worker has `RTPRIO` 80 and the `controller_manager` main loop has 50.
+
+!!! warning "Before the first motion"
+    First confirm that FRI connects and that the log contains `FRI worker uses FIFO priority 80`. Then make a short move at speed 0.1 with an operator supervising. FIFO reduces jitter on the computer; it does not replace network checks, robot limits, or a clear work area.
+
+## If setup does not work
+
+- If `realtime` is missing from `id`, log out and log in again.
+- If `ulimit -r` is less than 80, check `/etc/security/limits.d/99-ros2-realtime.conf` and that PAM loads `pam_limits.so`.
+- If the log still says `Operation not permitted`, make sure `cobot run` is started by the same user passed to `usermod`.
+- A Python virtualenv needs no extra setup: permissions are assigned to the user and inherited by Python.
+- Docker requires `--cap-add=sys_nice --ulimit rtprio=99 --ulimit memlock=-1`.
+
+## Rollback
+
+To remove the setup, run:
+
+```bash
+sudo rm /etc/security/limits.d/99-ros2-realtime.conf
+sudo gpasswd -d "$USER" realtime
+```
+
+After logging in again, processes use the regular Linux scheduler.
+
+See the official [ros2_control documentation](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism) for controller-manager priorities and limits.

doc/lwc-doc/docs/realtime-fri.md

@@ -0,0 +1,84 @@
+# FIFO-приоритет для FRI
+
+## Зачем это нужно
+
+FRI передаёт позиционную команду каждый цикл. При выбранном периоде 10 мс новый пакет должен обрабатываться примерно каждые 10 мс. Обычный планировщик Linux может задержать поток другим процессом. В журнале это видно по сообщениям:
+
+```text
+Could not enable FIFO RT scheduling: Operation not permitted
+Overrun might occur ... Read time: 11500 us
+```
+
+`SCHED_FIFO` — политика планировщика Linux для потоков с фиксированным приоритетом. В проекте `controller_manager` запрашивает приоритет 50, а FRI-поток — 80. Поток FRI получает более высокий приоритет, чтобы вовремя принять пакет KUKA и отправить ответ.
+
+Настройку выполняют **на компьютере с ROS 2**, с которого запускается `cobot run`. Sunrise Cabinet и `cobot-setting.yaml` менять для этого не нужно.
+
+## Настройка Ubuntu 24.04
+
+Выполните на компьютере с ROS 2 под пользователем, который запускает `cobot`:
+
+```bash
+sudo groupadd -f realtime
+sudo usermod -aG realtime "$USER"
+sudo tee /etc/security/limits.d/99-ros2-realtime.conf >/dev/null <<'EOF'
+@realtime soft rtprio 99
+@realtime soft priority 99
+@realtime soft memlock unlimited
+@realtime hard rtprio 99
+@realtime hard priority 99
+@realtime hard memlock unlimited
+EOF
+```
+
+Затем полностью выйдите из графического сеанса и войдите снова либо перезагрузите компьютер. Открытый терминал не получает новую группу и лимиты.
+
+## Проверка
+
+В новом терминале проверьте группу и лимиты:
+
+```bash
+id -nG | tr ' ' '\n' | grep -x realtime
+ulimit -r
+ulimit -l
+```
+
+Ожидаемый результат: первая команда выводит `realtime`, вторая — `99`, третья — `unlimited`.
+
+Запустите FRI без движения робота и убедитесь, что в журнале есть:
+
+```text
+FRI worker uses FIFO priority 80
+```
+
+После запуска можно дополнительно посмотреть политики всех потоков процесса:
+
+```bash
+PID=$(pgrep -n ros2_control_node)
+ps -L -p "$PID" -o pid,tid,cls,rtprio,pri,comm
+```
+
+Для потоков реального времени в столбце `CLS` будет `FF`; у FRI-потока `RTPRIO` равен 80, у основного цикла `controller_manager` — 50.
+
+!!! warning "Перед первым движением"
+    Сначала проверьте успешное подключение FRI и строку `FRI worker uses FIFO priority 80`. Затем выполните короткое перемещение на скорости 0.1 при наблюдении оператора. FIFO уменьшает jitter компьютера, но не отменяет контроль сети, ограничений робота и рабочей зоны.
+
+## Если настройка не сработала
+
+- Если `realtime` отсутствует в выводе `id`, выйдите из системы и войдите снова.
+- Если `ulimit -r` меньше 80, проверьте файл `/etc/security/limits.d/99-ros2-realtime.conf` и наличие `pam_limits.so` в конфигурации PAM.
+- Если лог всё ещё сообщает `Operation not permitted`, убедитесь, что `cobot run` запущен тем же пользователем, для которого выполнен `usermod`.
+- В Python virtualenv дополнительных действий не требуется: права задаются пользователю и наследуются процессом Python.
+- Для Docker нужны права контейнера: `--cap-add=sys_nice --ulimit rtprio=99 --ulimit memlock=-1`.
+
+## Откат
+
+Чтобы удалить настройку, выполните:
+
+```bash
+sudo rm /etc/security/limits.d/99-ros2-realtime.conf
+sudo gpasswd -d "$USER" realtime
+```
+
+После повторного входа процессы снова будут работать с обычным планировщиком Linux.
+
+Подробнее о лимитах и приоритете `controller_manager` — в [документации ros2_control](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism).

doc/lwc-doc/mkdocs.yml

@@ -141,5 +141,6 @@ nav:
         - Меню IO Group:       sunrise/kuka/features/io-group.md
 
   - Решение проблем:
+    - Реальное время FRI: realtime-fri.md
     - Ошибка конфигурации: troubleshooting/config-error.md
     - Ошибка SSL (rosdep/setup): troubleshooting/ssl-error.md

src/iiwa_bringup/launch/iiwa.launch.py

@@ -4,7 +4,7 @@ import sys
 sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
@@ -50,6 +50,8 @@ def _runtime_setup(context, *args, **kwargs):
             "initial_positions_file": settings.controller.moveit.initial_positions,
             "robot_ip": settings.robot.ip,
             "fri_port": str(settings.robot.port),
+            "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
+            "joint_position_tau": str(settings.robot.joint_position_tau),
             "simulate": "false",
         }
 
@@ -116,10 +118,19 @@ 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),
             "robot_ip": settings.robot.ip,
             "fri_port": str(settings.robot.port),
         }
 
+    if not simulate:
+        setup.append(LogInfo(msg=(
+            "FRI configured from cobot-setting.yaml: "
+            f"period={settings.robot.fri_cycle_ms} ms, "
+            f"position_tau={settings.robot.joint_position_tau:.3f} s, "
+            f"endpoint={settings.robot.ip}:{settings.robot.port}"
+        )))
+
     setup.append(IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution([

src/iiwa_bringup/launch/supported/controllers.launch.py

@@ -1,10 +1,15 @@
+import math
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
 from launch.event_handlers import OnProcessExit
 from launch.actions import RegisterEventHandler
 from launch_ros.actions import Node
 from webots_ros2_driver.urdf_spawner import URDFSpawner
+from ament_index_python.packages import get_package_share_directory
+from pathlib import Path
+from xml.etree import ElementTree
 
 
 from iiwa_utils import converter
@@ -22,6 +27,9 @@ def _setup_controllers(context, *args, **kwargs):
     fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").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")
+    joint_position_tau = float(LaunchConfiguration("joint_position_tau").perform(context))
+    if not math.isfinite(joint_position_tau) or joint_position_tau < 0.0:
+        raise ValueError("joint_position_tau must be finite and non-negative")
     update_rate = 1000 // fri_cycle_ms
 
     xacro_args = {"initial_positions_file": initial_positions_file}
@@ -31,6 +39,8 @@ def _setup_controllers(context, *args, **kwargs):
     else:
         xacro_args["robot_ip"] = LaunchConfiguration("robot_ip").perform(context)
         xacro_args["fri_port"] = LaunchConfiguration("fri_port").perform(context)
+        xacro_args["fri_cycle_ms"] = str(fri_cycle_ms)
+        xacro_args["joint_position_tau"] = str(joint_position_tau)
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -76,6 +86,13 @@ def _setup_controllers(context, *args, **kwargs):
 
     # FRI
     else:
+        manifest = Path(get_package_share_directory("controller_manager")) / "package.xml"
+        version = ElementTree.parse(manifest).findtext("version", "0.0.0")
+        if tuple(int(part) for part in version.split(".")[:3]) < (4, 48, 0):
+            raise RuntimeError(
+                "FRI synchronization requires controller_manager >= 4.48.0 (ROS 2 Jazzy); "
+                f"installed: {version}"
+            )
         ros2_control_node = Node(
             package="controller_manager",
             executable="ros2_control_node",
@@ -84,6 +101,7 @@ def _setup_controllers(context, *args, **kwargs):
                 {"robot_description": robot_description},
                 controller_path,
                 {"update_rate": update_rate},
+                {"hardware_synchronization.expect_blocking_read_write": True},
             ],
         )
 
@@ -114,6 +132,8 @@ def _setup_controllers(context, *args, **kwargs):
         )
 
         return [
+            LogInfo(msg=(f"FRI controller period={fri_cycle_ms} ms; Controller Manager rate={update_rate} Hz; "
+                         f"position smoothing tau={joint_position_tau:.3f} s")),
             ros2_control_node,
             jsb,
             jtc_after_jsb,
@@ -123,6 +143,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("robot_ip", default_value="192.170.10.2"),
         DeclareLaunchArgument("fri_port", default_value="30200"),
         OpaqueFunction(function=_setup_controllers),

src/iiwa_bringup/launch/supported/webots_spawn.launch.py

@@ -39,6 +39,13 @@ def _spawn_setup(context, *args, **kwargs):
     with open(controller, "r") as f:
         controller_cfg = yaml.safe_load(f) or {}
 
+    # FRI sets the hardware cycle dynamically in controllers.launch.py. Webots
+    # has no FRI period, so preserve its established 200 Hz controller cycle.
+    manager_params = controller_cfg.setdefault("controller_manager", {}).setdefault(
+        "ros__parameters", {}
+    )
+    manager_params["update_rate"] = 200
+
     combined = {
         "/**": {"ros__parameters": {
             "robot_description": robot_description,

src/iiwa_bringup/package.xml

@@ -26,7 +26,7 @@
   <exec_depend>rviz2</exec_depend>
 
   <!-- ros2_control -->
-  <exec_depend>controller_manager</exec_depend>
+  <exec_depend version_gte="4.48.0">controller_manager</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>forward_command_controller</exec_depend>

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -1,6 +1,8 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200  # резервное значение; при запуске через launch: 1000 // fri_cycle_ms
+    # `update_rate` задаётся FRI launch-файлом как 1000 / robot.fri_cycle_ms.
+    # Не фиксировать его здесь: KLI Position всегда выдаёт 10 мс (100 Гц),
+    # а KONI может работать с 5 мс (200 Гц).
 
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"

src/iiwa_config/config/setting.yaml

@@ -1,8 +1,9 @@
 robot:
   name: "iiwa7"
-  ip: "192.170.10.2"
+  ip: "192.168.21.147"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
+  joint_position_tau: 0.04  # EMA позиций FRI [с]; 0 отключает сглаживание
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 

src/iiwa_controller/CMakeLists.txt

@@ -94,6 +94,11 @@ ament_export_dependencies(
   controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs)
 
 if(BUILD_TESTING)
+  # Manual bounded loopback integration harness; no physical hardware access.
+  add_executable(fake_fri_server test/fake_fri_server.cpp)
+  target_link_libraries(fake_fri_server fri_client_sdk)
+  add_library(fri_loopback_bind SHARED test/loopback_bind.cpp)
+  target_link_libraries(fri_loopback_bind dl)
   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)

src/iiwa_controller/README.en.md

@@ -1,86 +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 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.

src/iiwa_controller/README.md

@@ -1,95 +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()`. Межпоточный обмен
-в цикле использует `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 не запускались.

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -17,6 +17,7 @@ struct IIWAStateSnapshot
   std::array<double, 7> measured_tau{};  // measured torque [Nm]
   std::array<double, 7> external_tau{};  // estimated external torque [Nm]
   std::array<double, 7> ipo_pos{};       // interpolator position [rad]
+  std::array<double, 7> command_pos{};   // position sent in the current FRI reply [rad]
   double sample_time{0.0};
   KUKA::FRI::ESessionState session{KUKA::FRI::IDLE};
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
@@ -39,7 +40,7 @@ class FRIClient : public KUKA::FRI::LBRClient
 public:
   static constexpr size_t N_JOINTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS;
   using Joints = std::array<double, N_JOINTS>;
-  FRIClient();
+  explicit FRIClient(double position_smoothing_tau = 0.04);
   void monitor() override;
   void waitForCommand() override;
   void command() override;
@@ -48,6 +49,7 @@ public:
 
   // Configure before starting the FRI thread; limits come from the robot URDF.
   void setLimits(const Joints & lower, const Joints & upper, const Joints & velocity);
+  void setExpectedSampleTime(double seconds) {expected_sample_time_ = seconds;}
   void setTargetJointPositions(const Joints & q);
   IIWAStateSnapshot getStateSnapshot() const;  // lifecycle/non-RT use
   bool tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const;
@@ -59,11 +61,14 @@ private:
   mutable std::mutex state_mutex_;
   std::mutex command_mutex_;
   Joints target_pos_{};
+  Joints filtered_pos_{};
   Joints requested_pos_{};
   Joints sent_pos_{};
   Joints lower_, upper_, max_velocity_;
   bool initialized_{false};
   bool requested_{false};
+  double position_smoothing_tau_{0.04};
+  double expected_sample_time_{0.0};
   std::chrono::steady_clock::time_point requested_at_{};
   std::chrono::steady_clock::time_point last_command_at_{};
   IIWAStateSnapshot snapshot_{};  // published under state_mutex_

src/iiwa_controller/include/iiwa_controller/FRICycleGate.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+
+namespace iiwa_controller
+{
+// One SDK step -> one read/update/write -> next SDK step. No queued commands,
+// no independent ROS timer and no overwrite of an unconsumed setpoint.
+class FRICycleGate
+{
+public:
+  void reset()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    stopped_ = available_ = acquired_ = completed_ = false;
+  }
+  void stop()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    stopped_ = true;
+    cv_.notify_all();
+  }
+  bool publishAndWait()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    if (stopped_) {return false;}
+    available_ = true;
+    completed_ = false;
+    cv_.notify_all();
+    if (!cv_.wait_for(lock, timeout_, [&] {return stopped_ || completed_;})) {
+      stopped_ = true;
+      cv_.notify_all();
+    }
+    return !stopped_;
+  }
+  bool waitReady(std::chrono::milliseconds timeout)
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return cv_.wait_for(lock, timeout, [&] {return stopped_ || available_;}) && !stopped_;
+  }
+  bool acquire()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    if (acquired_ || !cv_.wait_for(lock, timeout_, [&] {return stopped_ || available_;}) ||
+      stopped_) {return false;}
+    available_ = false;
+    acquired_ = true;
+    return true;
+  }
+  bool complete()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (stopped_ || !acquired_) {return false;}
+    acquired_ = false;
+    completed_ = true;
+    cv_.notify_all();
+    return true;
+  }
+private:
+  static constexpr std::chrono::milliseconds timeout_{100};
+  std::mutex mutex_;
+  std::condition_variable cv_;
+  bool stopped_{true}, available_{false}, acquired_{false}, completed_{false};
+};
+}  // namespace iiwa_controller

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 
 #include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/FRICycleGate.hpp"
 
 namespace iiwa_controller
 {
@@ -59,6 +60,9 @@ private:
   // Параметры из секции <hardware><param> в URDF
   std::string robot_ip_;
   int fri_port_{30200};
+  int fri_cycle_ms_{5};
+  double joint_position_tau_{0.04};
+  int rt_prio_{80};
   bool simulate_{false};
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
@@ -66,8 +70,10 @@ private:
   std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
   // FRI работает в отдельном потоке: step() блокируется в recvfrom().
-  // read() лишь читает готовый снимок — без блокировки RT-потока.
+  // read/write rendezvous once per SDK step; network I/O stays in this worker.
+  // Requires controller_manager hardware_synchronization mode (Jazzy 4.48+).
   std::thread fri_thread_;
+  FRICycleGate cycle_gate_;
   std::atomic<bool> fri_running_{false};
   void friThreadFunc();
   void stopFRI();
@@ -82,6 +88,8 @@ private:
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_vel_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_eff_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ext_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ipo_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_fri_cmd_;
 
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
 

src/iiwa_controller/src/FRIClient.cpp

@@ -9,8 +9,12 @@ namespace iiwa_controller
 {
 using namespace KUKA::FRI;
 
-FRIClient::FRIClient()
+FRIClient::FRIClient(double position_smoothing_tau)
+: position_smoothing_tau_(position_smoothing_tau)
 {
+  if (!std::isfinite(position_smoothing_tau_) || position_smoothing_tau_ < 0.0) {
+    throw std::invalid_argument("Position smoothing tau must be finite and non-negative");
+  }
   lower_.fill(-std::numeric_limits<double>::infinity());
   upper_.fill(std::numeric_limits<double>::infinity());
   max_velocity_.fill(std::numeric_limits<double>::infinity());
@@ -29,6 +33,10 @@ void FRIClient::captureData(bool commanding)
   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());
+  if (!commanding) {
+    // No application command exists outside a commanding FRI state.
+    current_.command_pos = current_.measured_pos;
+  }
   current_.sample_time = state.getSampleTime();
   current_.session = state.getSessionState();
   current_.quality = state.getConnectionQuality();
@@ -53,6 +61,21 @@ void FRIClient::captureData(bool commanding)
   if (nsec >= 1000000000 || !std::isfinite(current_.sample_time) || current_.sample_time <= 0.0) {
     throw std::runtime_error("Invalid FRI timestamp/sample period");
   }
+  if (expected_sample_time_ > 0.0 &&
+    std::abs(current_.sample_time - expected_sample_time_) > 1e-9)
+  {
+    throw std::runtime_error(
+            "Sunrise FRI period is " + std::to_string(current_.sample_time * 1000.0) +
+            " ms, but robot.fri_cycle_ms is " +
+            std::to_string(expected_sample_time_ * 1000.0) + " ms");
+  }
+  if (current_.valid && current_.session == COMMANDING_ACTIVE) {
+    const double elapsed = static_cast<double>(sec - current_.time_stamp_sec) +
+      (static_cast<double>(nsec) - current_.time_stamp_nano_sec) * 1e-9;
+    if (std::abs(elapsed - current_.sample_time) > 1e-6) {
+      throw std::runtime_error("FRI commanding packet gap; refusing to catch up trajectory");
+    }
+  }
   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]) ||
@@ -101,7 +124,8 @@ void FRIClient::waitForCommand()
   captureData(true);
   validateCommanding();
   LBRClient::waitForCommand();  // SDK 1.16 mirrors IPO, not measured position.
-  sent_pos_ = target_pos_ = current_.ipo_pos;
+  sent_pos_ = target_pos_ = filtered_pos_ = current_.ipo_pos;
+  current_.command_pos = sent_pos_;
   initialized_ = true;
   last_command_at_ = current_.received_at;
   publishState();
@@ -112,32 +136,37 @@ void FRIClient::command()
   captureData(true);
   validateCommanding();
   if (!initialized_) {
-    sent_pos_ = target_pos_ = current_.ipo_pos;
+    sent_pos_ = target_pos_ = filtered_pos_ = current_.ipo_pos;
     last_command_at_ = current_.received_at;
     initialized_ = true;
   }
   {
-    std::unique_lock<std::mutex> lock(command_mutex_, std::try_to_lock);
+    std::lock_guard<std::mutex> lock(command_mutex_);
-    if (lock.owns_lock() && requested_ && requested_at_ > last_command_at_) {
+    if (requested_ && requested_at_ > last_command_at_) {
       target_pos_ = requested_pos_;
       last_command_at_ = requested_at_;
     }
+    requested_ = false;
   }
   // 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;
+  const double alpha = position_smoothing_tau_ == 0.0 ? 1.0 :
+    dt / (position_smoothing_tau_ + dt);
   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];
+    filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
+    const double step = filtered_pos_[i] - sent_pos_[i];
     sent_pos_[i] += std::clamp(step, -max_velocity_[i] * dt, max_velocity_[i] * dt);
   }
   robotCommand().setJointPosition(sent_pos_.data());
+  current_.command_pos = sent_pos_;
   publishState();
 }
 
@@ -156,13 +185,14 @@ void FRIClient::setTargetJointPositions(const Joints & q)
       throw std::invalid_argument("Invalid FRI position command");
     }
   }
-  std::unique_lock<std::mutex> lock(command_mutex_, std::try_to_lock);
+  std::lock_guard<std::mutex> lock(command_mutex_);
-  if (lock.owns_lock()) {
+  if (requested_) {
+    throw std::logic_error("Previous FRI command has not been consumed");
+  }
   requested_pos_ = q;
   requested_at_ = std::chrono::steady_clock::now();
   requested_ = true;
 }
-}
 
 IIWAStateSnapshot FRIClient::getStateSnapshot() const
 {

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -15,6 +15,7 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_helpers.hpp"
 
 PLUGINLIB_EXPORT_CLASS(
   iiwa_controller::IIWAHardwareInterface,
@@ -82,6 +83,23 @@ CallbackReturn IIWAHardwareInterface::on_init(
     if (used != port.size() || fri_port_ < 1 || fri_port_ > 65535) {
       throw std::invalid_argument("Invalid fri_port");
     }
+    const auto cycle = getParam(info, "fri_cycle_ms", "5");
+    fri_cycle_ms_ = std::stoi(cycle, &used);
+    if (used != cycle.size() || fri_cycle_ms_ < 1 || fri_cycle_ms_ > 100) {
+      throw std::invalid_argument("Invalid fri_cycle_ms (expected 1..100)");
+    }
+    const auto position_tau = getParam(info, "joint_position_tau", "0.04");
+    joint_position_tau_ = std::stod(position_tau, &used);
+    if (used != position_tau.size() || !std::isfinite(joint_position_tau_) ||
+      joint_position_tau_ < 0.0)
+    {
+      throw std::invalid_argument("Invalid joint_position_tau (expected >= 0)");
+    }
+    const auto rt_prio = getParam(info, "rt_prio", "80");
+    rt_prio_ = std::stoi(rt_prio, &used);
+    if (used != rt_prio.size() || rt_prio_ < 1 || rt_prio_ > 99) {
+      throw std::invalid_argument("Invalid rt_prio (expected 1..99)");
+    }
     const auto simulation = getParam(info, "simulate", "false");
     if (simulation != "true" && simulation != "false") {
       throw std::invalid_argument("simulate must be true or false");
@@ -146,15 +164,17 @@ std::vector<hardware_interface::InterfaceDescription>
 IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 {
   std::vector<hardware_interface::InterfaceDescription> descs;
-  descs.reserve(N_JOINTS);
+  descs.reserve(3 * N_JOINTS);
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
+    for (const auto * name : {"external_torque", "ipo_position", "fri_command_position"}) {
       hardware_interface::InterfaceInfo if_info;
-    if_info.name          = "external_torque";
+      if_info.name = name;
       if_info.data_type = "double";
       if_info.initial_value = "0.0";
       descs.emplace_back(info_.joints[i].name, if_info);
     }
+  }
 
   return descs;
 }
@@ -170,8 +190,9 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State
   }
 
   releaseFRI();
-  fri_client_ = std::make_unique<FRIClient>();
+  fri_client_ = std::make_unique<FRIClient>(joint_position_tau_);
   fri_client_->setLimits(lower_, upper_, max_velocity_);
+  fri_client_->setExpectedSampleTime(fri_cycle_ms_ * 0.001);
   connection_ = std::make_unique<StartupConnection>(fri_running_);
   app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
   return CallbackReturn::SUCCESS;
@@ -189,6 +210,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
       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_ipo_[i] = get_state_interface_handle(name + "/ipo_position");
+      h_fri_cmd_[i] = get_state_interface_handle(name + "/fri_command_position");
       h_cmd_pos_[i] = get_command_interface_handle(name + "/position");
     }
     if (!simulate_) {
@@ -201,15 +224,11 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
       }
       fri_fault_.store(false);
       fri_running_.store(true);
+      cycle_gate_.reset();
       fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
-      const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
+      const bool ready = cycle_gate_.waitReady(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 &&
+      if (!ready || fri_fault_.load() || !last_snapshot_.valid ||
-          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))
@@ -231,6 +250,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
       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_state(h_ipo_[i], position, true);
+      set_state(h_fri_cmd_[i], position, true);
       set_command(h_cmd_pos_[i], position, true);
     }
   } catch (const std::exception & e) {
@@ -246,6 +267,7 @@ void IIWAHardwareInterface::stopFRI()
 {
   active_ = false;
   fri_running_.store(false);
+  cycle_gate_.stop();
   // Receive has a finite timeout. Never close a socket while step() uses it.
   if (fri_thread_.joinable()) {fri_thread_.join();}
   if (app_) {app_->disconnect();}
@@ -288,6 +310,15 @@ CallbackReturn IIWAHardwareInterface::on_error(const rclcpp_lifecycle::State & s
 void IIWAHardwareInterface::friThreadFunc()
 {
   try {
+    if (!realtime_tools::configure_sched_fifo(rt_prio_)) {
+      RCLCPP_WARN(rclcpp::get_logger(LOG),
+        "FRI worker could not enable FIFO priority %d; configure realtime permissions for this user",
+        rt_prio_);
+    } else {
+      RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI worker uses FIFO priority %d", rt_prio_);
+    }
+    bool synchronized = false;
+    const auto startup_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
     while (fri_running_.load()) {
       if (!app_->step()) {
         throw std::runtime_error("FRI step failed");
@@ -295,6 +326,17 @@ void IIWAHardwareInterface::friThreadFunc()
       if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
         throw std::runtime_error("FRI session ended");
       }
+      const auto snapshot = fri_client_->getStateSnapshot();
+      synchronized = synchronized || (snapshot.quality >= KUKA::FRI::GOOD &&
+        snapshot.session >= KUKA::FRI::MONITORING_READY);
+      if (synchronized) {
+        if (!cycle_gate_.publishAndWait()) {
+          if (!fri_running_.load()) {break;}
+          throw std::runtime_error("FRI cycle was not completed by ros2_control within 100 ms");
+        }
+      } else if (std::chrono::steady_clock::now() >= startup_deadline) {
+        throw std::runtime_error("FRI did not reach MONITORING_READY within 15 s");
+      }
     }
   } catch (const KUKA::FRI::FRIException & e) {
     fri_fault_.store(true);
@@ -307,6 +349,7 @@ void IIWAHardwareInterface::friThreadFunc()
     RCLCPP_ERROR(rclcpp::get_logger(LOG), "Unknown FRI failure");
   }
   fri_running_.store(false);
+  cycle_gate_.stop();
 }
 
 // ── compute_velocity_ ──────────────────────────────────────────────────────────
@@ -362,6 +405,8 @@ hardware_interface::return_type IIWAHardwareInterface::read(
       set_state(h_vel_[i], 0.0, false);
       set_state(h_eff_[i], 0.0, false);
       set_state(h_ext_[i], 0.0, false);
+      set_state(h_ipo_[i], pos, false);
+      set_state(h_fri_cmd_[i], pos, false);
     }
     return hardware_interface::return_type::OK;
   }
@@ -369,7 +414,13 @@ hardware_interface::return_type IIWAHardwareInterface::read(
   if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) {
     return hardware_interface::return_type::ERROR;
   }
-  fri_client_->tryGetStateSnapshot(last_snapshot_);
+  if (!cycle_gate_.acquire()) {
+    fri_fault_.store(true);
+    fri_running_.store(false);
+    cycle_gate_.stop();
+    return hardware_interface::return_type::ERROR;
+  }
+  last_snapshot_ = fri_client_->getStateSnapshot();
   const auto & snap = last_snapshot_;
   if (!snap.valid || std::chrono::steady_clock::now() - snap.received_at >
     std::chrono::milliseconds(100))
@@ -384,6 +435,8 @@ hardware_interface::return_type IIWAHardwareInterface::read(
     set_state(h_vel_[i], velocity_[i],         false);
     set_state(h_eff_[i], snap.measured_tau[i], false);
     set_state(h_ext_[i], snap.external_tau[i], false);
+    set_state(h_ipo_[i], snap.ipo_valid ? snap.ipo_pos[i] : snap.measured_pos[i], false);
+    set_state(h_fri_cmd_[i], snap.command_pos[i], false);
   }
 
   return hardware_interface::return_type::OK;
@@ -398,11 +451,15 @@ hardware_interface::return_type IIWAHardwareInterface::write(
   if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) {
     return hardware_interface::return_type::ERROR;
   }
-  if (!fri_client_->isCommandingActive()) {return hardware_interface::return_type::OK;}
+  if (!fri_client_->isCommandingActive()) {
+    return cycle_gate_.complete() ? hardware_interface::return_type::OK :
+           hardware_interface::return_type::ERROR;
+  }
   std::array<double, N_JOINTS> pos_cmd{};
   for (size_t i = 0; i < N_JOINTS; ++i) {
     if (!get_command(h_cmd_pos_[i], pos_cmd[i], false)) {
-      return hardware_interface::return_type::OK;  // retry next cycle; watchdog stays armed
+      cycle_gate_.stop();
+      return hardware_interface::return_type::ERROR;
     }
     if (!std::isfinite(pos_cmd[i]) || pos_cmd[i] < lower_[i] || pos_cmd[i] > upper_[i]) {
       fri_fault_.store(true);
@@ -410,8 +467,14 @@ hardware_interface::return_type IIWAHardwareInterface::write(
       return hardware_interface::return_type::ERROR;
     }
   }
+  try {
     fri_client_->setTargetJointPositions(pos_cmd);
-  return hardware_interface::return_type::OK;
+  } catch (const std::exception &) {
+    cycle_gate_.stop();
+    return hardware_interface::return_type::ERROR;
+  }
+  return cycle_gate_.complete() ? hardware_interface::return_type::OK :
+         hardware_interface::return_type::ERROR;
 }
 
 }  // namespace iiwa_controller

src/iiwa_controller/test/fake_fri_server.cpp

@@ -0,0 +1,112 @@
+// Offline protocol peer, not a dynamics or Sunrise safety simulator.
+// Both endpoints are hardcoded loopback addresses. Never accepts a robot IP.
+#include <arpa/inet.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <thread>
+#include "FRIMessages.pb.h"
+#include "pb_encode.h"
+#include "pb_decode.h"
+#include "pb_frimessages_callbacks.h"
+
+int main(int argc, char ** argv)
+{
+  if (argc != 2) {return 2;}
+  std::ofstream csv(argv[1]);
+  csv << "sequence,session,receive_seconds,position,delta\n";
+  csv.flush();
+  int fd = socket(AF_INET, SOCK_DGRAM, 0);
+  sockaddr_in local{}, remote{};
+  local.sin_family = remote.sin_family = AF_INET;
+  local.sin_port = remote.sin_port = htons(30283);
+  inet_pton(AF_INET, "127.0.0.2", &local.sin_addr);
+  inet_pton(AF_INET, "127.0.0.1", &remote.sin_addr);
+  if (fd < 0 || bind(fd, reinterpret_cast<sockaddr *>(&local), sizeof(local))) {
+    perror("fake FRI loopback bind"); return 2;
+  }
+  timeval timeout{0, 20000};
+  setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
+  std::array<double, 7> q{}, zero{}, received{};
+  std::array<int64_t, 7> drives{};
+  drives.fill(DriveState_ACTIVE);
+  tRepeatedDoubleArguments positions{7, 7, q.data()}, zeros{7, 7, zero.data()};
+  tRepeatedIntArguments drive_values{7, 7, drives.data()};
+  FRIMonitoringMessage m{};
+  m.header.messageIdentifier = 0x245142;  // Bundled SDK LBRMONITORMESSAGEID.
+  m.has_robotInfo = m.has_monitorData = m.has_connectionInfo = true;
+  m.robotInfo.has_numberOfJoints = m.robotInfo.has_safetyState = true;
+  m.robotInfo.has_operationMode = m.robotInfo.has_controlMode = true;
+  m.robotInfo.numberOfJoints = 7;
+  m.robotInfo.safetyState = SafetyState_NORMAL_OPERATION;
+  m.robotInfo.operationMode = OperationMode_AUTOMATIC_MODE;
+  m.robotInfo.controlMode = ControlMode_POSITION_CONTROLMODE;
+  m.robotInfo.driveState.funcs.encode = encode_repeatedInt;
+  m.robotInfo.driveState.arg = &drive_values;
+  m.connectionInfo.quality = FRIConnectionQuality_GOOD;
+  m.connectionInfo.has_sendPeriod = m.connectionInfo.has_receiveMultiplier = true;
+  m.connectionInfo.sendPeriod = 10;
+  m.connectionInfo.receiveMultiplier = 1;
+  auto & md = m.monitorData;
+  md.has_timestamp = md.has_measuredJointPosition = md.has_commandedJointPosition = true;
+  md.has_measuredTorque = md.has_commandedTorque = md.has_externalTorque = true;
+  for (auto * values : {&md.measuredJointPosition, &md.commandedJointPosition, &m.ipoData.jointPosition}) {
+    values->value.funcs.encode = encode_repeatedDouble;
+    values->value.arg = &positions;
+  }
+  for (auto * values : {&md.measuredTorque, &md.commandedTorque, &md.externalTorque}) {
+    values->value.funcs.encode = encode_repeatedDouble;
+    values->value.arg = &zeros;
+  }
+  m.ipoData.has_jointPosition = m.ipoData.has_clientCommandMode = true;
+  m.ipoData.has_overlayType = m.ipoData.has_trackingPerformance = true;
+  m.ipoData.clientCommandMode = ClientCommandMode_POSITION;
+  m.ipoData.overlayType = OverlayType_JOINT;
+  m.ipoData.trackingPerformance = 1.0;
+  using Clock = std::chrono::steady_clock;
+  const auto start = Clock::now();
+  auto next = start;
+  unsigned replies = 0;
+  for (unsigned seq = 0; Clock::now() - start < std::chrono::seconds(45); ++seq) {
+    m.header.sequenceCounter = seq;
+    m.connectionInfo.sessionState = replies < 20 ? FRISessionState_MONITORING_READY :
+      replies < 40 ? FRISessionState_COMMANDING_WAIT : FRISessionState_COMMANDING_ACTIVE;
+    m.has_ipoData = replies >= 20;
+    md.timestamp.sec = 1 + seq / 100;
+    md.timestamp.nanosec = (seq % 100) * 10000000;
+    uint8_t buffer[2048];
+    auto output = pb_ostream_from_buffer(buffer, sizeof(buffer));
+    if (!pb_encode(&output, FRIMonitoringMessage_fields, &m)) {return 3;}
+    sendto(fd, buffer, output.bytes_written, 0, reinterpret_cast<sockaddr *>(&remote), sizeof(remote));
+    auto count = recv(fd, buffer, sizeof(buffer), 0);
+    if (count > 0) {
+      FRICommandMessage command{};
+      tRepeatedDoubleArguments target{0, 7, received.data()};
+      command.commandData.jointPosition.value.funcs.decode = decode_repeatedDouble;
+      command.commandData.jointPosition.value.arg = &target;
+      auto input = pb_istream_from_buffer(buffer, count);
+      if (!pb_decode(&input, FRICommandMessage_fields, &command) ||
+        !command.has_commandData || !command.commandData.has_jointPosition) {return 4;}
+      for (double value : received) {if (!std::isfinite(value)) {return 5;}}
+      const double delta = received[0] - q[0];
+      csv << seq << ',' << m.connectionInfo.sessionState << ',' <<
+        std::chrono::duration<double>(Clock::now() - start).count() << ',' << received[0] << ',' << delta << '\n';
+      csv.flush();
+      q = received;
+      m.header.reflectedSequenceCounter = command.header.sequenceCounter;
+      ++replies;
+    } else if (replies) {
+      std::cerr << "Reply timeout after " << replies << " frames\n";
+      close(fd); return 6;
+    }
+    // Before connection, avoid catch-up bursts after receive timeouts.
+    next = replies ? next + std::chrono::milliseconds(10) : Clock::now();
+    std::this_thread::sleep_until(next);
+  }
+  close(fd);
+  return 0;
+}

src/iiwa_controller/test/loopback_bind.cpp

@@ -0,0 +1,20 @@
+// Test-only interposer: allow the unmodified SDK and fake robot to share one host.
+// The SDK otherwise binds all local addresses at the robot's source port.
+#include <arpa/inet.h>
+#include <dlfcn.h>
+#include <sys/socket.h>
+
+extern "C" int bind(int fd, const sockaddr * address, socklen_t length)
+{
+  using Bind = int (*)(int, const sockaddr *, socklen_t);
+  static auto real_bind = reinterpret_cast<Bind>(dlsym(RTLD_NEXT, "bind"));
+  if (address->sa_family == AF_INET && length == sizeof(sockaddr_in)) {
+    auto local = *reinterpret_cast<const sockaddr_in *>(address);
+    // Dedicated smoke-test port only; never alter DDS or other application binds.
+    if (local.sin_port == htons(30283) && local.sin_addr.s_addr == htonl(INADDR_ANY)) {
+      inet_pton(AF_INET, "127.0.0.1", &local.sin_addr);
+      return real_bind(fd, reinterpret_cast<const sockaddr *>(&local), sizeof(local));
+    }
+  }
+  return real_bind(fd, address, length);
+}

src/iiwa_controller/test/smoke_fri.py

@@ -0,0 +1,83 @@
+"""Manual bounded real-SDK/JTC integration check over loopback only.
+
+Run after building/sourcing iiwa_controller; never connects physical hardware.
+"""
+import csv
+import sys
+import time
+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-fri-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'] = '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="lo"/></Interfaces><AllowMulticast>false</AllowMulticast></General><Discovery><Peers><Peer Address="127.0.0.1"/></Peers></Discovery></Domain></CycloneDDS>'
+xml = xacro.process_file(str(root / 'src/iiwa_description/urdf/iiwa7.urdf.xacro'), mappings={'robot_ip': '127.0.0.2', 'fri_port': '30283', 'fri_cycle_ms': '10'}).toxml()
+assert '<param name="simulate">false</param>' in xml
+assert '<param name="robot_ip">127.0.0.2</param>' in xml
+assert '<param name="fri_port">30283</param>' in xml
+build = Path(sys.argv[1]) if len(sys.argv) > 1 else root / 'build/iiwa_controller'
+server = build / 'fake_fri_server'
+shim = build / 'libfri_loopback_bind.so'
+assert server.is_file() and shim.is_file(), 'Build the test harness first'
+trace = work / 'commands.csv'
+params = work / 'robot.yaml'
+params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}}))
+processes = []
+handles = []
+try:
+    server_log = (work / 'fake_fri_server.log').open('w')
+    handles.append(server_log)
+    processes.append(subprocess.Popen([str(server), str(trace)], stdout=server_log, stderr=subprocess.STDOUT, start_new_session=True))
+    time.sleep(0.1)
+    assert processes[0].poll() is None, 'Fake peer failed to bind'
+    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'), '-p', 'update_rate:=100', '-p', 'hardware_synchronization.expect_blocking_read_write:=true']),
+    ]:
+        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=dict(env, **({'LD_PRELOAD': str(shim)} if executable == 'ros2_control_node' else {})), 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
+    rows = list(csv.DictReader(trace.open()))
+    assert len(rows) > 100 and processes[0].poll() is None, 'FRI stream stopped'
+    active = [r for r in rows if int(r['session']) == 4]
+    assert active and abs(float(active[-1]['position']) - 0.02) < 0.001
+    deltas = [abs(float(r['delta'])) for r in active]
+    intervals = [float(b['receive_seconds']) - float(a['receive_seconds']) for a, b in zip(active, active[1:])]
+    assert max(deltas) <= 1.71 * 0.01 + 1e-6, 'Command exceeds per-frame velocity limit'
+    print(f'SMOKE PASS: real SDK loopback + actual CM/JTC action succeeded; {len(active)} active frames, max delta={max(deltas):.8f} rad; receive interval min/max={min(intervals):.6f}/{max(intervals):.6f} s.', flush=True)
+finally:
+    for process in processes:
+        if process.poll() is None:
+            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)

src/iiwa_controller/test/test_fri_client.cpp

@@ -5,6 +5,9 @@
 #include <stdexcept>
 #include <thread>
 #include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/FRICycleGate.hpp"
+#include <atomic>
+#include <vector>
 #include "friClientData.h"
 #include "pb_frimessages_callbacks.h"
 
@@ -20,7 +23,8 @@ public:
 class ClientFixture : public iiwa_controller::FRIClient
 {
 public:
-  ClientFixture() : FRIClient(), app(connection, *this), data(app.data())
+  explicit ClientFixture(double position_smoothing_tau = 0.04)
+  : FRIClient(position_smoothing_tau), app(connection, *this), data(app.data())
   {
     auto & m = data->monitoringMsg;
     m.connectionInfo.sendPeriod = 10;
@@ -96,7 +100,7 @@ TEST(FRIClient, RejectsUnsupportedCommandModeBeforeSending)
   EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
 }
 
-TEST(FRIClient, PositionCommandsHaveNoEmaDelay)
+TEST(FRIClient, PositionCommandsAreSmoothedBeforeSending)
 {
   ClientFixture client;
   client.cycle(COMMANDING_WAIT);
@@ -105,10 +109,23 @@ TEST(FRIClient, PositionCommandsHaveNoEmaDelay)
   q.fill(0.41);
   client.setTargetJointPositions(q);
   client.cycle(COMMANDING_ACTIVE);
-  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.41);
+  // tau=40 ms, dt=10 ms: alpha=dt/(tau+dt)=0.2.
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.402);
   EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
 }
 
+TEST(FRIClient, ZeroPositionSmoothingTauForwardsStepImmediately)
+{
+  ClientFixture client(0.0);
+  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);
+}
+
 TEST(FRIClient, RejectsInvalidCommandsAndBoundsCommandRate)
 {
   ClientFixture client;
@@ -143,6 +160,18 @@ TEST(FRIClient, DuplicateTimestampIsNotFreshTelemetry)
   EXPECT_THROW(client.monitor(), std::runtime_error);
 }
 
+TEST(FRIClient, RejectsMismatchedPeriodAndMissingCommandingPackets)
+{
+  ClientFixture mismatch;
+  mismatch.setExpectedSampleTime(0.005);
+  EXPECT_THROW(mismatch.cycle(MONITORING_READY), std::runtime_error);
+  ClientFixture gap;
+  gap.cycle(COMMANDING_WAIT);
+  gap.data->monitoringMsg.monitorData.timestamp.nanosec += 10000000;
+  EXPECT_THROW(gap.cycle(COMMANDING_ACTIVE), std::runtime_error);
+  EXPECT_FALSE(gap.data->commandMsg.commandData.has_jointPosition);
+}
+
 TEST(FRIClient, LeavingCommandingRequiresExplicitRecovery)
 {
   ClientFixture client;
@@ -172,3 +201,79 @@ TEST(FRIClient, StalledRosCommandStreamStopsBeforeSending)
   EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error);
   EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
 }
+
+TEST(FRIClient, RejectsOverwritingAnUnconsumedPoint)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  iiwa_controller::FRIClient::Joints q;
+  q.fill(0.401);
+  client.setTargetJointPositions(q);
+  q.fill(0.402);
+  EXPECT_THROW(client.setTargetJointPositions(q), std::logic_error);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4002);
+}
+
+TEST(FRIClient, SynchronizedCyclesPreserveEveryPointDespiteSchedulingJitter)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  iiwa_controller::FRICycleGate gate;
+  gate.reset();
+  std::atomic<bool> worker_ok{true};
+  std::vector<double> sent;
+  std::thread worker([&] {
+    for (int frame = 0; frame <= 8; ++frame) {
+      client.cycle(COMMANDING_ACTIVE);
+      sent.push_back(client.sentPosition());
+      if (frame < 8 && !gate.publishAndWait()) {worker_ok = false; break;}
+    }
+  });
+  for (int frame = 1; frame <= 8; ++frame) {
+    if (!gate.acquire()) {worker_ok = false; break;}
+    // Variable controller execution time cannot cause a point to be overwritten.
+    std::this_thread::sleep_for(std::chrono::milliseconds(frame % 3));
+    iiwa_controller::FRIClient::Joints q;
+    q.fill(0.4 + frame * 0.001);
+    client.setTargetJointPositions(q);
+    if (!gate.complete()) {worker_ok = false; break;}
+  }
+  worker.join();
+  ASSERT_TRUE(worker_ok);
+  ASSERT_EQ(sent.size(), 9u);
+  double expected = 0.4;
+  for (size_t frame = 1; frame < sent.size(); ++frame) {
+    const double target = 0.4 + static_cast<double>(frame) * 0.001;
+    expected = 0.2 * target + 0.8 * expected;
+    EXPECT_NEAR(sent[frame], expected, 1e-12);
+  }
+}
+
+TEST(FRICycleGate, DuplicateReadOrWriteCannotAdvanceWorker)
+{
+  iiwa_controller::FRICycleGate gate;
+  gate.reset();
+  EXPECT_FALSE(gate.complete());
+  std::thread worker([&] {EXPECT_TRUE(gate.publishAndWait());});
+  EXPECT_TRUE(gate.acquire());
+  EXPECT_FALSE(gate.acquire());
+  EXPECT_TRUE(gate.complete());
+  EXPECT_FALSE(gate.complete());
+  worker.join();
+}
+
+TEST(FRICycleGate, StopWakesWorkerAndMissingWriteTimesOut)
+{
+  iiwa_controller::FRICycleGate gate;
+  gate.reset();
+  std::thread worker([&] {EXPECT_FALSE(gate.publishAndWait());});
+  EXPECT_TRUE(gate.acquire());
+  gate.stop();
+  worker.join();
+  gate.reset();
+  EXPECT_FALSE(gate.publishAndWait());
+  EXPECT_FALSE(gate.acquire());
+  EXPECT_FALSE(gate.complete());
+}

src/iiwa_controller/test/test_hardware.cpp

@@ -45,6 +45,22 @@ TEST(Hardware, RejectsMalformedPortWithoutThrowing)
   EXPECT_EQ(hardware.on_init(params), Return::ERROR);
 }
 
+TEST(Hardware, RejectsInvalidRealtimePriority)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["rt_prio"] = "0";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, RejectsInvalidPositionSmoothingTau)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["joint_position_tau"] = "-0.01";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
 TEST(Hardware, RejectsWrongJointOrderAndMissingInterfaces)
 {
   auto params = parameters();

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -6,6 +6,8 @@
     <xacro:arg name="simulate" default="false"/>
     <xacro:arg name="robot_ip" default="192.170.10.2"/>
     <xacro:arg name="fri_port" default="30200"/>
+    <xacro:arg name="fri_cycle_ms" default="5"/>
+    <xacro:arg name="joint_position_tau" default="0.04"/>
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -143,6 +145,9 @@
                 <plugin>iiwa_controller/IIWAHardwareInterface</plugin>
                 <param name="robot_ip">$(arg robot_ip)</param>
                 <param name="fri_port">$(arg fri_port)</param>
+                <param name="fri_cycle_ms">$(arg fri_cycle_ms)</param>
+                <param name="joint_position_tau">$(arg joint_position_tau)</param>
+                <param name="rt_prio">80</param>
                 <param name="simulate">false</param>
             </hardware>
 

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -1,3 +1,4 @@
+import math
 import os
 from dataclasses import dataclass, fields, is_dataclass
 from typing import Any, Dict, List, Optional, Type, TypeVar
@@ -15,6 +16,7 @@ class RobotCfg:
     port: int
     description: str
     fri_cycle_ms: int
+    joint_position_tau: float
 
 
 @dataclass(frozen=True)
@@ -282,12 +284,16 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
 
     # robot
     robot_raw = require(raw, "robot")
+    joint_position_tau = float(robot_raw.get("joint_position_tau", 0.04))
+    if not math.isfinite(joint_position_tau) or joint_position_tau < 0.0:
+        raise ValueError("robot.joint_position_tau must be a finite value >= 0")
     robot = RobotCfg(
         name=str(require(robot_raw, "name")),
         ip=str(require(robot_raw, "ip")),
         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=joint_position_tau,
     )
 
     # digital_twin