docs: add FRI realtime scheduling guide

cobot-setting.yaml

@@ -3,6 +3,7 @@ robot:
   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
 
 

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

@@ -51,6 +51,7 @@ def _runtime_setup(context, *args, **kwargs):
             "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",
         }
 
@@ -117,6 +118,7 @@ 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),
         }
@@ -125,6 +127,7 @@ def _runtime_setup(context, *args, **kwargs):
         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}"
         )))
 

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

@@ -1,3 +1,5 @@
+import math
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
@@ -25,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}
@@ -35,6 +40,7 @@ def _setup_controllers(context, *args, **kwargs):
         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,
@@ -126,7 +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"),
+            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,
@@ -136,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_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

@@ -3,6 +3,7 @@ robot:
   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
 
 
@@ -76,4 +77,4 @@ foxglove:
     - assets
   include_hidden: false # Показывать клиенту скрытые топики и сервисы (начинаются с _)
   asset_uri_allowlist: ['^package://(?:[-\w%]+/)*[-\w%.]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$'] # Regex-список URI вида package://..., из которых bridge разрешает отдавать файлы-ассеты (URDF, mesh и т.п.)
-  ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте)
+  ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте)

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;
@@ -60,11 +61,13 @@ 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_{};

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -61,6 +61,8 @@ private:
   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_;
@@ -86,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();
@@ -116,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();
@@ -127,7 +136,7 @@ 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;
   }
@@ -144,16 +153,20 @@ void FRIClient::command()
     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();
 }
 

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,
@@ -87,6 +88,18 @@ CallbackReturn IIWAHardwareInterface::on_init(
     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");
@@ -151,14 +164,16 @@ 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) {
-    hardware_interface::InterfaceInfo if_info;
-    if_info.name          = "external_torque";
-    if_info.data_type     = "double";
-    if_info.initial_value = "0.0";
-    descs.emplace_back(info_.joints[i].name, if_info);
+    for (const auto * name : {"external_torque", "ipo_position", "fri_command_position"}) {
+      hardware_interface::InterfaceInfo if_info;
+      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;
@@ -175,7 +190,7 @@ 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_);
@@ -195,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_) {
@@ -233,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) {
@@ -291,6 +310,13 @@ 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()) {
@@ -379,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;
   }
@@ -407,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;

src/iiwa_controller/test/test_fri_client.cpp

@@ -23,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;
@@ -99,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);
@@ -108,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;
@@ -199,7 +213,7 @@ TEST(FRIClient, RejectsOverwritingAnUnconsumedPoint)
   q.fill(0.402);
   EXPECT_THROW(client.setTargetJointPositions(q), std::logic_error);
   client.cycle(COMMANDING_ACTIVE);
-  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.401);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4002);
 }
 
 TEST(FRIClient, SynchronizedCyclesPreserveEveryPointDespiteSchedulingJitter)
@@ -229,8 +243,11 @@ TEST(FRIClient, SynchronizedCyclesPreserveEveryPointDespiteSchedulingJitter)
   worker.join();
   ASSERT_TRUE(worker_ok);
   ASSERT_EQ(sent.size(), 9u);
+  double expected = 0.4;
   for (size_t frame = 1; frame < sent.size(); ++frame) {
-    EXPECT_NEAR(sent[frame] - sent[frame - 1], 0.001, 1e-12);
+    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);
   }
 }
 

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

@@ -7,6 +7,7 @@
     <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']}"/>
@@ -145,6 +146,8 @@
                 <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