Refactor controller configuration and update planning parameters for improved performance

2026-05-14 07:29:47 +03:00
parent c3d1de7b01
commit 7eefdd66d5
8 changed files with 51 additions and 61 deletions
@@ -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 → скачок → удар приводов.
-    interpolate_from_desired_state: true
+    # с measured_pos (= filtered_pos_ в open-loop режиме) → скачок команды → удар приводов.
+    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
@@ -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


@@ -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}
@@ -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
@@ -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};
@@ -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(
@@ -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;
-    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);
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    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);
@@ -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: