fix: smooth FRI position commands

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

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 мс; фильтр скорости не применяется.
 
 ---
 

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_{};

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());

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<double>(frame) * 0.001;
+    expected = 0.2 * target + 0.8 * expected;
+    EXPECT_NEAR(sent[frame], expected, 1e-12);
   }
 }