From 04b5c99ec8a9714129f72aff445f07cdfed3b09d 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: Mon, 18 May 2026 11:33:35 +1000 Subject: [PATCH] Update iiwa_controller and MoveIt configuration: Adjust joint velocity filter and parameters for improved performance --- .../config/moveit/iiwa_controller.yaml | 22 ++++++------- src/iiwa_config/config/moveit/moveit_cpp.yaml | 18 +++++----- src/iiwa_config/config/setting.yaml | 3 +- .../iiwa_controller/IIWAHardwareInterface.hpp | 8 +++-- .../src/IIWAHardwareInterface.cpp | 33 ++++++++++++++----- src/iiwa_description/urdf/iiwa7.urdf.xacro | 2 ++ 6 files changed, 54 insertions(+), 32 deletions(-) diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index d0bb476..1b34a4f 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/src/iiwa_config/config/moveit/iiwa_controller.yaml @@ -45,19 +45,19 @@ iiwa_arm_controller: allow_partial_joints_goal: false allow_nonzero_velocity_at_trajectory_end: false - state_publish_rate: 100.0 + state_publish_rate: 50.0 # половина от update_rate, как в референсе lbr_fri_ros2_stack action_monitor_rate: 20.0 - # constraints: - # stopped_velocity_tolerance: 0.01 - # goal_time: 1.0 - # joint1: { trajectory: 0, goal: 0.01 } - # joint2: { trajectory: 0, goal: 0.01 } - # joint3: { trajectory: 0, goal: 0.01 } - # joint4: { trajectory: 0, goal: 0.01 } - # joint5: { trajectory: 0, goal: 0.01 } - # joint6: { trajectory: 0, goal: 0.01 } - # joint7: { trajectory: 0, goal: 0.01 } + constraints: + stopped_velocity_tolerance: 0.01 # [рад/с] — допуск скорости в конечной точке + goal_time: 2.0 # [с] — допуск на время достижения цели + joint1: { trajectory: 0.1, goal: 0.01 } + joint2: { trajectory: 0.1, goal: 0.01 } + joint3: { trajectory: 0.1, goal: 0.01 } + joint4: { trajectory: 0.1, goal: 0.01 } + joint5: { trajectory: 0.1, goal: 0.01 } + joint6: { trajectory: 0.1, goal: 0.01 } + joint7: { trajectory: 0.1, goal: 0.01 } forward_position_controller: ros__parameters: diff --git a/src/iiwa_config/config/moveit/moveit_cpp.yaml b/src/iiwa_config/config/moveit/moveit_cpp.yaml index 5bebb98..2bd45a7 100644 --- a/src/iiwa_config/config/moveit/moveit_cpp.yaml +++ b/src/iiwa_config/config/moveit/moveit_cpp.yaml @@ -16,14 +16,14 @@ plan_request_params: max_velocity_scaling_factor: 0.5 max_acceleration_scaling_factor: 1.0 -ompl_rrtc: # Namespace for individual plan request - plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp - planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem - planning_pipeline: ompl # Name of the pipeline that is being used - planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline - max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning - max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning - planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned +ompl_rrtc: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.0 pilz_ptp: plan_request_params: @@ -49,4 +49,4 @@ chomp_planner: planning_pipeline: chomp max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 - planning_time: 1.5 \ No newline at end of file + planning_time: 1.5 diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 7ab15ec..7f5d950 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -4,7 +4,8 @@ robot: port: 30200 command_mode: "position" # torque, position fri_cycle_ms: 5 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц) - joint_position_tau: 0.04 # EMA фильтр позиций: 0 = выкл (без лага → без overshoot при торможении) + joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI + joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 4a6a05d..a523905 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -61,6 +61,9 @@ private: bool simulate_{false}; std::string cmd_mode_str_{"position"}; double joint_position_tau_{0.04}; + // EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей. + // joint_velocity_tau = 0 отключает фильтр (raw finite difference). + double joint_velocity_tau_{0.01}; // Объекты FRI SDK std::unique_ptr fri_client_; @@ -83,9 +86,10 @@ private: std::array h_cmd_pos_; std::array h_cmd_eff_; - // Вычисление скорости конечными разностями + // Вычисление скорости: конечные разности + EMA-фильтр std::array prev_pos_{}; - std::array velocity_{}; + std::array velocity_{}; // отфильтрованная скорость, публикуется в JTC + std::array velocity_raw_{}; // сырая скорость до фильтра unsigned int last_ts_sec_{0}; unsigned int last_ts_nsec_{0}; bool velocity_initialized_{false}; diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 5245f0e..d395cb3 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -43,18 +43,20 @@ CallbackReturn IIWAHardwareInterface::on_init( const auto & info = params.hardware_info; - robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); - fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); - simulate_ = (getParam(info, "simulate", "false") == "true"); - cmd_mode_str_ = getParam(info, "command_mode", "position"); - joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04")); + robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); + fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); + simulate_ = (getParam(info, "simulate", "false") == "true"); + cmd_mode_str_ = getParam(info, "command_mode", "position"); + joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04")); + joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01")); RCLCPP_INFO(rclcpp::get_logger(LOG), - "on_init: ip=%s port=%d simulate=%s mode=%s tau=%.3f", + "on_init: ip=%s port=%d simulate=%s mode=%s pos_tau=%.3f vel_tau=%.3f", robot_ip_.c_str(), fri_port_, simulate_ ? "true" : "false", cmd_mode_str_.c_str(), - joint_position_tau_); + joint_position_tau_, + joint_velocity_tau_); if (info.joints.size() != N_JOINTS) { RCLCPP_FATAL(rclcpp::get_logger(LOG), @@ -64,6 +66,7 @@ CallbackReturn IIWAHardwareInterface::on_init( prev_pos_.fill(0.0); velocity_.fill(0.0); + velocity_raw_.fill(0.0); return CallbackReturn::SUCCESS; } @@ -231,7 +234,10 @@ void IIWAHardwareInterface::friThreadFunc() } // ── compute_velocity_ ────────────────────────────────────────────────────────── -// Конечные разности с int64-вычитанием для точности при больших Unix-timestamp'ах. +// Конечные разности + EMA-фильтр. +// int64-вычитание timestamp'ов предотвращает потерю точности при больших Unix-значениях. +// EMA-фильтр (joint_velocity_tau) убирает одиночные выбросы, которые видит JTC как +// скачки состояния и компенсирует агрессивными командами → хруст двигателей. void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) { @@ -240,6 +246,7 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) last_ts_sec_ = snap.time_stamp_sec; last_ts_nsec_ = snap.time_stamp_nano_sec; velocity_.fill(0.0); + velocity_raw_.fill(0.0); velocity_initialized_ = true; return; } @@ -260,10 +267,18 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) static constexpr double kVelDeadband = 1e-4; if (dt > 0.0) { + // EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра) + const double vel_alpha = (joint_velocity_tau_ > 0.0) + ? dt / (joint_velocity_tau_ + dt) + : 1.0; + for (size_t i = 0; i < N_JOINTS; ++i) { const double raw = (snap.measured_pos[i] - prev_pos_[i]) / dt; const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]); - velocity_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped; + velocity_raw_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped; + + // EMA: velocity_[i] = alpha * raw + (1 - alpha) * prev_filtered + velocity_[i] = vel_alpha * velocity_raw_[i] + (1.0 - vel_alpha) * velocity_[i]; } } diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 0a58b53..71dec61 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -8,6 +8,7 @@ + @@ -148,6 +149,7 @@ false $(arg command_mode) $(arg joint_position_tau) + $(arg joint_velocity_tau)