Refactor controller configuration and update planning parameters for improved performance

src/iiwa_controller/CMakeLists.txt

@@ -52,7 +52,6 @@ target_link_libraries(fri_client_sdk PUBLIC pthread)
 add_library(${PROJECT_NAME} SHARED
   src/FRIClient.cpp
   src/IIWAHardwareInterface.cpp
-  src/IIWAJointPositionController.cpp
 )
 
 target_include_directories(${PROJECT_NAME} PUBLIC
@@ -76,11 +75,6 @@ pluginlib_export_plugin_description_file(
   iiwa_hardware_interface_plugin.xml
 )
 
-pluginlib_export_plugin_description_file(
-  controller_interface
-  iiwa_controller_plugin.xml
-)
-
 # Установка — только библиотека и заголовки, без config/launch/urdf
 install(TARGETS ${PROJECT_NAME}
   EXPORT export_${PROJECT_NAME}

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -21,13 +21,15 @@ enum class CommandMode
 // и так же за один lock читается из read() в потоке управления.
 struct IIWAStateSnapshot
 {
-  std::array<double, 7> measured_pos{}; // измеренные позиции суставов [рад]
+  std::array<double, 7> measured_pos{}; // в Commanding = filtered_pos_ (open-loop)
   std::array<double, 7> measured_tau{}; // измеренные моменты [Нм]
   std::array<double, 7> external_tau{}; // внешние моменты без компенсации модели [Нм]
   std::array<double, 7> ipo_pos{};      // позиция интерполятора [рад], только в Commanding
   double sample_time{0.005};            // период цикла FRI [с]
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
   bool ipo_valid{false};                // в Monitor-режиме IPO недоступна
+  unsigned int time_stamp_sec{0};       // Unix-время пакета [с]
+  unsigned int time_stamp_nano_sec{0};  // наносекундная часть [нс]
 };
 
 class FRIClient : public KUKA::FRI::LBRClient

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -81,9 +81,11 @@ private:
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
 
-  // Предыдущие позиции и скорости после EMA-фильтра
+  // Предыдущие позиции и скорость (обновляются только при свежем FRI-пакете)
   std::array<double, N_JOINTS> prev_pos_{};
   std::array<double, N_JOINTS> vel_filtered_{};
+  unsigned int last_ts_sec_{0};
+  unsigned int last_ts_nsec_{0};
 
   // Отдельный объект часов для RCLCPP_*_THROTTLE — не создаём временный в FRI-потоке
   rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};

src/iiwa_controller/src/FRIClient.cpp

@@ -44,6 +44,8 @@ void FRIClient::captureMonitoringData()
   snapshot_.sample_time = robotState().getSampleTime();
   snapshot_.quality = robotState().getConnectionQuality();
   snapshot_.ipo_valid = false;
+  snapshot_.time_stamp_sec = robotState().getTimestampSec();
+  snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec();
 }
 
 // Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
@@ -55,6 +57,11 @@ void FRIClient::captureCommandingData()
     snapshot_.ipo_pos.data(),
     robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
   snapshot_.ipo_valid = true;
+
+  // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию.
+  // Это устраняет расхождение между лагающим реальным датчиком и сглаженной командой —
+  // JTC не генерирует коррекций для статичных осей при переходах между траекториями.
+  snapshot_.measured_pos = filtered_pos_;
 }
 
 // Вызывается в MONITORING_WAIT и MONITORING_READY
@@ -93,13 +100,12 @@ void FRIClient::waitForCommand()
 void FRIClient::command()
 {
   std::lock_guard<std::mutex> lock(data_mutex_);
-  captureCommandingData();
 
-  // Экспоненциальный фильтр первого порядка: alpha = dt / (tau + dt).
-  // Сглаживает скачки команд от контроллера — устраняет писк и стук суставов.
-  // При tau=0.04 с и dt=0.005 с: alpha≈0.11 (11% новой команды за цикл).
-  const double dt = snapshot_.sample_time;
-  const double alpha = dt / (joint_position_tau_ + dt);
+  // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_
+  // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом).
+  // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA.
+  const double dt = robotState().getSampleTime();
+  const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0;
   for (size_t i = 0; i < N_JOINTS; ++i) {
     filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
   }
@@ -107,10 +113,11 @@ void FRIClient::command()
   robotCommand().setJointPosition(filtered_pos_.data());
 
   if (cmd_mode_ == CommandMode::TORQUE) {
-    // В режиме TORQUE позиция работает как feedforward удержания, момент добавляется поверх.
-    // Кука выбрасывает CommandInvalidException если отклонение позиции превышает 10 градусов.
     robotCommand().setTorque(target_tau_.data());
   }
+
+  // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
+  captureCommandingData();
 }
 
 void FRIClient::onStateChange(

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -157,6 +157,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     const auto snap = fri_client_->getStateSnapshot();
     prev_pos_ = snap.measured_pos;
     vel_filtered_.fill(0.0);
+    last_ts_sec_  = snap.time_stamp_sec;
+    last_ts_nsec_ = snap.time_stamp_nano_sec;
   }
 
   return CallbackReturn::SUCCESS;
@@ -211,7 +213,7 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
 // Период JTC остаётся стабильным: при update_rate=400 и fri_cycle_ms=5 соотношение 2:1,
 // идентичное рабочей конфигурации fri_cycle_ms=10 + update_rate=200.
 hardware_interface::return_type IIWAHardwareInterface::read(
-  const rclcpp::Time &, const rclcpp::Duration & period)
+  const rclcpp::Time &, const rclcpp::Duration &)
 {
   if (simulate_) {
     for (size_t i = 0; i < N_JOINTS; ++i) {
@@ -227,16 +229,29 @@ hardware_interface::return_type IIWAHardwareInterface::read(
 
   const auto snap = fri_client_->getStateSnapshot();
 
+  // Обновляем скорость только при свежем FRI-пакете.
+  // measured_pos в Commanding = filtered_pos_ (open-loop), поэтому скорость — это
+  // производная сглаженной команды: гладкий сигнал без шума датчика и без лага.
+  const bool fresh = (snap.time_stamp_sec  != last_ts_sec_ ||
+                      snap.time_stamp_nano_sec != last_ts_nsec_);
+  if (fresh) {
+    const double dt =
+      (static_cast<double>(snap.time_stamp_sec)       - static_cast<double>(last_ts_sec_)) +
+      (static_cast<double>(snap.time_stamp_nano_sec)  - static_cast<double>(last_ts_nsec_)) * 1e-9;
+    if (dt > 0.0) {
+      for (size_t i = 0; i < N_JOINTS; ++i) {
+        vel_filtered_[i] = (snap.measured_pos[i] - prev_pos_[i]) / dt;
+      }
+    }
+    for (size_t i = 0; i < N_JOINTS; ++i) {
+      prev_pos_[i] = snap.measured_pos[i];
+    }
+    last_ts_sec_  = snap.time_stamp_sec;
+    last_ts_nsec_ = snap.time_stamp_nano_sec;
+  }
+
   for (size_t i = 0; i < N_JOINTS; ++i) {
-    const double pos = snap.measured_pos[i];
-
-    constexpr double kAlpha = 0.2;
-    const double dt = period.seconds();
-    const double vel_raw = (dt > 1e-9) ? (pos - prev_pos_[i]) / dt : 0.0;
-    vel_filtered_[i] = kAlpha * vel_raw + (1.0 - kAlpha) * vel_filtered_[i];
-    prev_pos_[i] = pos;
-
-    set_state(h_pos_[i], pos, false);
+    set_state(h_pos_[i], snap.measured_pos[i], false);
     set_state(h_vel_[i], vel_filtered_[i], false);
     set_state(h_eff_[i], snap.measured_tau[i], false);
     set_state(h_ext_[i], snap.external_tau[i], false);