Update iiwa_controller and MoveIt configuration: Adjust joint velocity filter and parameters for improved performance

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:
+    constraints:
-    #   stopped_velocity_tolerance: 0.01
+      stopped_velocity_tolerance: 0.01  # [рад/с] — допуск скорости в конечной точке
-    #   goal_time: 1.0
+      goal_time: 2.0                    # [с] — допуск на время достижения цели
-    #   joint1: { trajectory: 0, goal: 0.01 }
+      joint1: { trajectory: 0.1, goal: 0.01 }
-    #   joint2: { trajectory: 0, goal: 0.01 }
+      joint2: { trajectory: 0.1, goal: 0.01 }
-    #   joint3: { trajectory: 0, goal: 0.01 }
+      joint3: { trajectory: 0.1, goal: 0.01 }
-    #   joint4: { trajectory: 0, goal: 0.01 }
+      joint4: { trajectory: 0.1, goal: 0.01 }
-    #   joint5: { trajectory: 0, goal: 0.01 }
+      joint5: { trajectory: 0.1, goal: 0.01 }
-    #   joint6: { trajectory: 0, goal: 0.01 }
+      joint6: { trajectory: 0.1, goal: 0.01 }
-    #   joint7: { trajectory: 0, goal: 0.01 }
+      joint7: { trajectory: 0.1, goal: 0.01 }
 
 forward_position_controller:
   ros__parameters:

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
+ompl_rrtc:
-  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
+  plan_request_params:
-    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
+    planning_attempts: 1
-    planning_pipeline: ompl  # Name of the pipeline that is being used
+    planning_pipeline: ompl
-    planner_id: "RRTConnectkConfigDefault"  # Name of the specific planner to be used by the pipeline
+    planner_id: "RRTConnectkConfigDefault"
-    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    max_velocity_scaling_factor: 1.0
-    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    max_acceleration_scaling_factor: 1.0
-    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
+    planning_time: 1.0
 
 pilz_ptp:
   plan_request_params:

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
 

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<FRIClient> fri_client_;
@@ -83,9 +86,10 @@ private:
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
 
-  // Вычисление скорости конечными разностями
+  // Вычисление скорости: конечные разности + EMA-фильтр
   std::array<double, N_JOINTS> prev_pos_{};
-  std::array<double, N_JOINTS> velocity_{};
+  std::array<double, N_JOINTS> velocity_{};        // отфильтрованная скорость, публикуется в JTC
+  std::array<double, N_JOINTS> velocity_raw_{};    // сырая скорость до фильтра
   unsigned int last_ts_sec_{0};
   unsigned int last_ts_nsec_{0};
   bool velocity_initialized_{false};

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -48,13 +48,15 @@ CallbackReturn IIWAHardwareInterface::on_init(
   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];
     }
   }
 

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -8,6 +8,7 @@
     <xacro:arg name="fri_port" default="30200"/>
     <xacro:arg name="command_mode" default="position"/>
     <xacro:arg name="joint_position_tau" default="0.04"/>
+    <xacro:arg name="joint_velocity_tau" default="0.01"/>
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -148,6 +149,7 @@
                 <param name="simulate">false</param>
                 <param name="command_mode">$(arg command_mode)</param>
                 <param name="joint_position_tau">$(arg joint_position_tau)</param>
+                <param name="joint_velocity_tau">$(arg joint_velocity_tau)</param>
             </hardware>
 
             <joint name="joint1">