From 1780532f5141c7d699ad8e65675b156e1e6f3682 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 16 Sep 2026 15:06:17 +1000 Subject: [PATCH] feat: configure FRI position smoothing --- cobot-setting.yaml | 1 + .../docs/getting-started/configuration.en.md | 3 ++- doc/lwc-doc/docs/getting-started/configuration.md | 3 ++- src/iiwa_bringup/launch/iiwa.launch.py | 3 +++ .../launch/supported/controllers.launch.py | 10 +++++++++- src/iiwa_config/config/setting.yaml | 3 ++- .../include/iiwa_controller/FRIClient.h | 5 ++--- .../iiwa_controller/IIWAHardwareInterface.hpp | 1 + src/iiwa_controller/src/FRIClient.cpp | 9 +++++++-- src/iiwa_controller/src/IIWAHardwareInterface.cpp | 9 ++++++++- src/iiwa_controller/test/test_fri_client.cpp | 15 ++++++++++++++- src/iiwa_controller/test/test_hardware.cpp | 8 ++++++++ src/iiwa_description/urdf/iiwa7.urdf.xacro | 2 ++ src/iiwa_utils/iiwa_utils/setting_loader.py | 6 ++++++ 14 files changed, 67 insertions(+), 11 deletions(-) diff --git a/cobot-setting.yaml b/cobot-setting.yaml index 51ed54b..8cf012f 100644 --- a/cobot-setting.yaml +++ b/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 diff --git a/doc/lwc-doc/docs/getting-started/configuration.en.md b/doc/lwc-doc/docs/getting-started/configuration.en.md index f909b66..f92e684 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.en.md +++ b/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. --- diff --git a/doc/lwc-doc/docs/getting-started/configuration.md b/doc/lwc-doc/docs/getting-started/configuration.md index a9ef0db..c99e264 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.md +++ b/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`; фильтр скорости не применяется. --- diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index bbb066b..c9abd26 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/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}" ))) diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 060dd2f..06bad4d 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/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), diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 959a928..cc6b634 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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 # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) \ No newline at end of file + ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index b7cf637..acf14da 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/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; - 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_{}; diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 611ed43..a0df76a 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/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 diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index 7c9a2ab..b26246c 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/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::infinity()); upper_.fill(std::numeric_limits::infinity()); max_velocity_.fill(std::numeric_limits::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]) diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index aad6f28..087f258 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/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(); + fri_client_ = std::make_unique(joint_position_tau_); fri_client_->setLimits(lower_, upper_, max_velocity_); fri_client_->setExpectedSampleTime(fri_cycle_ms_ * 0.001); connection_ = std::make_unique(fri_running_); diff --git a/src/iiwa_controller/test/test_fri_client.cpp b/src/iiwa_controller/test/test_fri_client.cpp index 3c2bc82..12f4df6 100644 --- a/src/iiwa_controller/test/test_fri_client.cpp +++ b/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; diff --git a/src/iiwa_controller/test/test_hardware.cpp b/src/iiwa_controller/test/test_hardware.cpp index a63a9fc..d32d0b9 100644 --- a/src/iiwa_controller/test/test_hardware.cpp +++ b/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(); diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 9294b61..8ecb474 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -7,6 +7,7 @@ + @@ -145,6 +146,7 @@ $(arg robot_ip) $(arg fri_port) $(arg fri_cycle_ms) + $(arg joint_position_tau) 80 false diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index bfe4da0..f31f917 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/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