feat: configure FRI position smoothing

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