From 7eefdd66d517bd6d7827bbd1a127dee07a3ff83f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Thu, 14 May 2026 07:29:47 +0300 Subject: [PATCH] Refactor controller configuration and update planning parameters for improved performance --- .../config/moveit/iiwa_controller.yaml | 36 ++----------------- src/iiwa_config/config/setting.yaml | 2 +- src/iiwa_controller/CMakeLists.txt | 6 ---- .../include/iiwa_controller/FRIClient.h | 4 ++- .../iiwa_controller/IIWAHardwareInterface.hpp | 4 ++- src/iiwa_controller/src/FRIClient.cpp | 23 +++++++----- .../src/IIWAHardwareInterface.cpp | 35 ++++++++++++------ .../scripts/move_to_pose_server.py | 2 +- 8 files changed, 51 insertions(+), 61 deletions(-) diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index 0981ac5..1615bdd 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/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 → скачок → удар приводов. - 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 \ No newline at end of file diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index cd10f43..a9d686b 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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 diff --git a/src/iiwa_controller/CMakeLists.txt b/src/iiwa_controller/CMakeLists.txt index 6d15230..63af7d4 100644 --- a/src/iiwa_controller/CMakeLists.txt +++ b/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} diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index 12ccad6..7a4d682 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -21,13 +21,15 @@ enum class CommandMode // и так же за один lock читается из read() в потоке управления. struct IIWAStateSnapshot { - std::array measured_pos{}; // измеренные позиции суставов [рад] + std::array measured_pos{}; // в Commanding = filtered_pos_ (open-loop) std::array measured_tau{}; // измеренные моменты [Нм] std::array external_tau{}; // внешние моменты без компенсации модели [Нм] std::array 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 diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 4be1395..0e80ca5 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -81,9 +81,11 @@ private: std::array h_cmd_pos_; std::array h_cmd_eff_; - // Предыдущие позиции и скорости после EMA-фильтра + // Предыдущие позиции и скорость (обновляются только при свежем FRI-пакете) std::array prev_pos_{}; std::array vel_filtered_{}; + unsigned int last_ts_sec_{0}; + unsigned int last_ts_nsec_{0}; // Отдельный объект часов для RCLCPP_*_THROTTLE — не создаём временный в FRI-потоке rclcpp::Clock throttle_clock_{RCL_STEADY_TIME}; diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index bf28cff..324a1db 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/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 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( diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index e2f8466..2fb006c 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/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(snap.time_stamp_sec) - static_cast(last_ts_sec_)) + + (static_cast(snap.time_stamp_nano_sec) - static_cast(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); diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index e46e526..4d26c46 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/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: