Refactor controller configuration and update planning parameters for improved performance

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200  # резервное значение; при запуске через launch: 2000 // fri_cycle_ms
+    update_rate: 200  # резервное значение; при запуске через launch: 1000 // fri_cycle_ms
 
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"
@@ -8,12 +8,6 @@ controller_manager:
     iiwa_arm_controller:
       type: "joint_trajectory_controller/JointTrajectoryController"
 
-    iiwa_arm_torque_controller:
-      type: "forward_command_controller/ForwardCommandController"
-
-    iiwa_joint_position_controller:
-      type: "iiwa_controller/IIWAJointPositionController"
-
 iiwa_arm_controller:
   ros__parameters:
     joints:
@@ -34,8 +28,8 @@ iiwa_arm_controller:
 
     # Интерполяция от реально измеренной позиции, а не от желаемой.
     # При true JTC стартует от последнего desired state, который может расходиться
-    # с реальным положением при старте или переподключении FRI → скачок → удар приводов.
+    # с measured_pos (= filtered_pos_ в open-loop режиме) → скачок команды → удар приводов.
-    interpolate_from_desired_state: true
+    interpolate_from_desired_state: false
 
     # Разрешить неполные goals
     allow_partial_joints_goal: false
@@ -59,27 +53,3 @@ iiwa_arm_controller:
     #   joint6: { trajectory: 0, goal: 0.01 }
     #   joint7: { trajectory: 0, goal: 0.01 }
 
-iiwa_joint_position_controller:
-  ros__parameters:
-    joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
-
-# Torque контроллер - прямое управление моментом
-# Активировать только при command_mode:=torque в launch файле
-iiwa_arm_torque_controller:
-  ros__parameters:
-    joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
-    interface_name: effort

src/iiwa_config/config/setting.yaml

@@ -4,7 +4,7 @@ robot:
   port: 30200
   command_mode: "position" # torque, position
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04  # постоянная времени фильтра позиций [с]: больше → плавнее, медленнее
+  joint_position_tau: 0.01  # постоянная времени фильтра позиций [с]: больше → плавнее, медленнее
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 

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).
+  // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_
-  // Сглаживает скачки команд от контроллера — устраняет писк и стук суставов.
+  // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом).
-  // При tau=0.04 с и dt=0.005 с: alpha≈0.11 (11% новой команды за цикл).
+  // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA.
-  const double dt = snapshot_.sample_time;
+  const double dt = robotState().getSampleTime();
-  const double alpha = dt / (joint_position_tau_ + dt);
+  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) {
-    const double pos = snap.measured_pos[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;
+  }
 
-    constexpr double kAlpha = 0.2;
+  for (size_t i = 0; i < N_JOINTS; ++i) {
-    const double dt = period.seconds();
+    set_state(h_pos_[i], snap.measured_pos[i], false);
-    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_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);

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -248,7 +248,7 @@ class IiwaMotionServer(Node):
             return response
 
         plan_params = self._make_plan_params(
-            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale
         )
         plan_result = self._arm.plan(single_plan_parameters=plan_params)
         if not plan_result: