feat: configure FRI position smoothing

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. The position command is smoothed before FRI transmission with a 40 ms EMA time constant; no velocity filter is 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. Перед отправкой в FRI позиционная команда сглаживается EMA с постоянной времени 40 мс; фильтр скорости не применяется.
+Управление движением всегда выполняет JointTrajectoryController. Перед отправкой в FRI позиционная команда сглаживается EMA с постоянной времени `robot.joint_position_tau`; фильтр скорости не применяется.
 
 ---
 

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_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
 
 

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -40,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;
@@ -67,8 +67,7 @@ private:
   Joints lower_, upper_, max_velocity_;
   bool initialized_{false};
   bool requested_{false};
-  // Matches the established lbr_fri_ros2_stack FRI position smoothing.
-  static constexpr double kPositionSmoothingTau = 0.04;
+  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,7 @@ 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

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());
@@ -149,7 +153,8 @@ void FRIClient::command()
     throw std::runtime_error("FRI command watchdog expired");
   }
   const double dt = current_.sample_time;
-  const double alpha = dt / (kPositionSmoothingTau + dt);
+  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])

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -88,6 +88,13 @@ 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) {
@@ -183,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_);

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;
@@ -113,6 +114,18 @@ TEST(FRIClient, PositionCommandsAreSmoothedBeforeSending)
   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;

src/iiwa_controller/test/test_hardware.cpp

@@ -53,6 +53,14 @@ TEST(Hardware, RejectsInvalidRealtimePriority)
   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,7 @@
                 <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