Add joint_position_tau parameter for smoother joint control and update related configurations

src/iiwa_bringup/launch/iiwa.launch.py

@@ -66,6 +66,7 @@ def _runtime_setup(context, *args, **kwargs):
             "fri_port": str(settings.robot.port),
             "simulate": "false",
             "command_mode": settings.robot.command_mode,
+            "joint_position_tau": str(settings.robot.joint_position_tau),
         }
         use_sim_time = False
 
@@ -170,6 +171,7 @@ def _runtime_setup(context, *args, **kwargs):
             "command_mode": settings.robot.command_mode,
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
             "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
+            "joint_position_tau": str(settings.robot.joint_position_tau),
         }
 
     controllers_launch = IncludeLaunchDescription(

src/iiwa_bringup/launch/supported/controllers.launch.py

@@ -21,6 +21,7 @@ def _setup_controllers(context, *args, **kwargs):
     simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
     command_mode = LaunchConfiguration("command_mode").perform(context)
     fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
+    joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context)
     # JTC rate = FRI rate (1:1): каждый цикл JTC читает свежее состояние от FRI.
     # При 2:1 нечётные JTC-циклы видят устаревший снапшот → чередование скорости 0/v → писк.
     update_rate = 1000 // fri_cycle_ms
@@ -29,6 +30,8 @@ def _setup_controllers(context, *args, **kwargs):
 
     if simulate:
         xacro_args["simulate"] = "true"
+    else:
+        xacro_args["joint_position_tau"] = joint_position_tau
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -143,5 +146,6 @@ def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument("command_mode", default_value="position"),
         DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
+        DeclareLaunchArgument("joint_position_tau", default_value="0.04"),
         OpaqueFunction(function=_setup_controllers),
     ])

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -11,6 +11,9 @@ controller_manager:
     iiwa_arm_torque_controller:
       type: "forward_command_controller/ForwardCommandController"
 
+    iiwa_joint_position_controller:
+      type: "iiwa_controller/IIWAJointPositionController"
+
 iiwa_arm_controller:
   ros__parameters:
     joints:
@@ -56,6 +59,17 @@ 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:

src/iiwa_config/config/setting.yaml

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

src/iiwa_controller/CMakeLists.txt

@@ -8,10 +8,13 @@ set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(std_msgs REQUIRED)
 
 # FRI SDK
 set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI)
@@ -45,10 +48,11 @@ target_compile_options(fri_client_sdk PRIVATE -fpermissive -w)
 set_target_properties(fri_client_sdk PROPERTIES POSITION_INDEPENDENT_CODE ON)
 target_link_libraries(fri_client_sdk PUBLIC pthread)
 
-# Плагин hardware interface
+# Плагины hardware interface + controller
 add_library(${PROJECT_NAME} SHARED
   src/FRIClient.cpp
   src/IIWAHardwareInterface.cpp
+  src/IIWAJointPositionController.cpp
 )
 
 target_include_directories(${PROJECT_NAME} PUBLIC
@@ -58,10 +62,13 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 
 target_link_libraries(${PROJECT_NAME} PRIVATE
   fri_client_sdk
+  controller_interface::controller_interface
   hardware_interface::hardware_interface
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  realtime_tools::realtime_tools
+  ${std_msgs_TARGETS}
 )
 
 pluginlib_export_plugin_description_file(
@@ -69,6 +76,11 @@ 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}
@@ -85,6 +97,6 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(export_${PROJECT_NAME})
 ament_export_dependencies(
-  hardware_interface pluginlib rclcpp rclcpp_lifecycle)
+  controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs)
 
 ament_package()

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -35,7 +35,10 @@ class FRIClient : public KUKA::FRI::LBRClient
 public:
   static constexpr size_t N_JOINTS = 7;
 
-  explicit FRIClient(CommandMode mode = CommandMode::POSITION);
+  // joint_position_tau — постоянная времени экспоненциального фильтра позиций [с].
+  // Аналог joint_position_tau из lbr_fri_ros2_stack (по умолчанию 0.04 с = 40 мс).
+  // Сглаживает скачки команд перед отправкой роботу → убирает писк и стук суставов.
+  explicit FRIClient(CommandMode mode = CommandMode::POSITION, double joint_position_tau = 0.04);
   ~FRIClient() override = default;
 
   // Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
@@ -54,11 +57,15 @@ public:
 
 private:
   CommandMode cmd_mode_;
+  double joint_position_tau_;
   std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
 
   mutable std::mutex data_mutex_;
   std::array<double, N_JOINTS> target_pos_{};
   std::array<double, N_JOINTS> target_tau_{};
+  // Сглаженная позиция, которую реально отправляем роботу.
+  // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
+  std::array<double, N_JOINTS> filtered_pos_{};
   IIWAStateSnapshot snapshot_{};
 
   // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна)

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -56,6 +56,7 @@ private:
   int fri_port_{30200};
   bool simulate_{false};
   std::string cmd_mode_str_{"position"};
+  double joint_position_tau_{0.04};  // постоянная времени фильтра позиций [с]
 
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;

src/iiwa_controller/package.xml

@@ -9,10 +9,13 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/iiwa_controller/src/FRIClient.cpp

@@ -20,10 +20,12 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
   }
 }
 
-FRIClient::FRIClient(CommandMode mode) : cmd_mode_(mode)
+FRIClient::FRIClient(CommandMode mode, double joint_position_tau)
+: cmd_mode_(mode), joint_position_tau_(joint_position_tau)
 {
   target_pos_.fill(0.0);
   target_tau_.fill(0.0);
+  filtered_pos_.fill(0.0);
 }
 
 // Вызывается только в Monitor-состояниях.
@@ -73,10 +75,12 @@ void FRIClient::waitForCommand()
   std::lock_guard<std::mutex> lock(data_mutex_);
   captureCommandingData();
 
-  // Инициализируем цель IPO-позицией, иначе до первого write() будем посылать нули.
+  // Инициализируем цель и фильтр IPO-позицией.
+  // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE.
   std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
+  std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
 
-  robotCommand().setJointPosition(target_pos_.data());
+  robotCommand().setJointPosition(filtered_pos_.data());
 
   if (cmd_mode_ == CommandMode::TORQUE) {
     // Пока контроллер не синхронизирован, момент держим на нуле
@@ -91,7 +95,16 @@ void FRIClient::command()
   std::lock_guard<std::mutex> lock(data_mutex_);
   captureCommandingData();
 
-  robotCommand().setJointPosition(target_pos_.data());
+  // Экспоненциальный фильтр первого порядка: 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);
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
+  }
+
+  robotCommand().setJointPosition(filtered_pos_.data());
 
   if (cmd_mode_ == CommandMode::TORQUE) {
     // В режиме TORQUE позиция работает как feedforward удержания, момент добавляется поверх.

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -41,13 +41,15 @@ CallbackReturn IIWAHardwareInterface::on_init(
   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"));
 
   RCLCPP_INFO(
     rclcpp::get_logger("IIWAHardwareInterface"),
-    "on_init: ip=%s port=%d simulate=%s mode=%s",
+    "on_init: ip=%s port=%d simulate=%s mode=%s tau=%.3f",
     robot_ip_.c_str(), fri_port_,
     simulate_ ? "true" : "false",
-    cmd_mode_str_.c_str());
+    cmd_mode_str_.c_str(),
+    joint_position_tau_);
 
   if (info.joints.size() != N_JOINTS) {
     RCLCPP_FATAL(
@@ -114,7 +116,7 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     return CallbackReturn::SUCCESS;
   }
 
-  fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_));
+  fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_), joint_position_tau_);
   // 100 мс таймаут: если закрытие сокета не разблокирует recvfrom() мгновенно,
   // поток всё равно выйдет через одну итерацию.
   connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);

src/iiwa_description/urdf/iiwa7.urdf.xacro

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

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -16,6 +16,7 @@ class RobotCfg:
     command_mode: str
     description: str
     fri_cycle_ms: int
+    joint_position_tau: float
 
 
 @dataclass(frozen=True)
@@ -248,6 +249,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         command_mode=str(require(robot_raw, "command_mode")),
         description=resolve_path(str(require(robot_raw, "description")), settings_dir),
         fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)),
+        joint_position_tau=float(robot_raw.get("joint_position_tau", 0.04)),
     )
 
     # digital_twin