From 29a16010610d705ab21a37d15693bf16e739c8d0 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:00:14 +1000 Subject: [PATCH] fix: smooth FRI position commands --- doc/lwc-doc/docs/getting-started/configuration.en.md | 2 +- doc/lwc-doc/docs/getting-started/configuration.md | 2 +- .../include/iiwa_controller/FRIClient.h | 3 +++ src/iiwa_controller/src/FRIClient.cpp | 8 +++++--- src/iiwa_controller/test/test_fri_client.cpp | 12 ++++++++---- 5 files changed, 18 insertions(+), 9 deletions(-) diff --git a/doc/lwc-doc/docs/getting-started/configuration.en.md b/doc/lwc-doc/docs/getting-started/configuration.en.md index 8e59d35..f909b66 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.en.md +++ b/doc/lwc-doc/docs/getting-started/configuration.en.md @@ -37,7 +37,7 @@ robot: | `fri_cycle_ms` | FRI cycle: `5` ms = 200 Hz, `10` ms = 100 Hz | Use `10` for stable operation or `5` for high-precision tasks | | `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 a 40 ms EMA time constant; 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 1b97fd2..a9ef0db 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.md +++ b/doc/lwc-doc/docs/getting-started/configuration.md @@ -39,7 +39,7 @@ robot: | `fri_cycle_ms` | Период цикла FRI: `5` мс = 200 Гц, `10` мс = 100 Гц | `10` для стабильной работы, `5` для высокоточных задач | | `description` | Путь к URDF-описанию робота | Не менять | -Управление движением всегда выполняет JointTrajectoryController. EMA-фильтры положения и скорости не применяются. +Управление движением всегда выполняет JointTrajectoryController. Перед отправкой в FRI позиционная команда сглаживается EMA с постоянной времени 40 мс; фильтр скорости не применяется. --- diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index 4c8dc79..b7cf637 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -61,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}; + // Matches the established lbr_fri_ros2_stack FRI position smoothing. + static constexpr double kPositionSmoothingTau = 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/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index 9d70d25..7c9a2ab 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -120,7 +120,7 @@ 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; @@ -132,7 +132,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; } @@ -149,13 +149,15 @@ void FRIClient::command() throw std::runtime_error("FRI command watchdog expired"); } const double dt = current_.sample_time; + const double alpha = dt / (kPositionSmoothingTau + 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()); diff --git a/src/iiwa_controller/test/test_fri_client.cpp b/src/iiwa_controller/test/test_fri_client.cpp index be42daf..3c2bc82 100644 --- a/src/iiwa_controller/test/test_fri_client.cpp +++ b/src/iiwa_controller/test/test_fri_client.cpp @@ -99,7 +99,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,7 +108,8 @@ 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); } @@ -199,7 +200,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 +230,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(frame) * 0.001; + expected = 0.2 * target + 0.8 * expected; + EXPECT_NEAR(sent[frame], expected, 1e-12); } }