Refactor iiwa_controller_v2: Remove obsolete files and update URDF parameters

- Deleted `system_interface_type_values.hpp`, `package.xml`, `fri_client.cpp`, and `system_interface.cpp` as part of the cleanup process.
- Updated `iiwa7.urdf.xacro` to remove deprecated parameters and adjust command interface settings.
- Modified joint definitions in `joints.xacro` to include new friction and soft limit parameters.
- Enhanced `macros.xacro` to support additional joint properties for friction and safety control.
- Adjusted mass properties in `params.xacro` to reflect accurate values for the robot's components.

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -18,8 +18,10 @@ joint_state_broadcaster:
       - velocity
       - effort
       - external_torque
-      - commanded_torque
+      # commanded_torque и ipo_joint_position доступны только при активном v2
-      - ipo_joint_position
+      # при переключении на v2 — раскомментировать:
+      # - commanded_torque
+      # - ipo_joint_position
 
 iiwa_arm_controller:
   ros__parameters:

src/iiwa_config/config/setting.yaml

@@ -4,7 +4,7 @@ robot:
   port: 30200
   command_mode: "position" # torque, position
   fri_cycle_ms: 5          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.15   # EMA фильтр позиций: 0 = выкл (без лага → без overshoot при торможении)
+  joint_position_tau: 0.04   # EMA фильтр позиций: 0 = выкл (без лага → без overshoot при торможении)
   active_controller: "jtc"  # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -2,6 +2,7 @@
 
 #include <array>
 #include <atomic>
+#include <cstdint>
 #include <memory>
 #include <string>
 #include <thread>
@@ -34,13 +35,16 @@ public:
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override;
 
-  // external_torque не объявлен в URDF, поэтому регистрируем его здесь как unlisted.
+  // external_torque не объявлен в URDF — регистрируем вручную как unlisted.
-  // Стандартные интерфейсы (position, velocity, effort) базовый класс берёт из URDF сам.
   std::vector<hardware_interface::InterfaceDescription>
   export_unlisted_state_interface_descriptions() override;
 
+  // Полный lifecycle: configure открывает сокет, activate запускает поток,
+  // deactivate останавливает поток, cleanup освобождает FRI-объекты.
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
   hardware_interface::return_type read(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
@@ -56,17 +60,15 @@ private:
   int fri_port_{30200};
   bool simulate_{false};
   std::string cmd_mode_str_{"position"};
-  double joint_position_tau_{0.04};  // постоянная времени фильтра позиций [с]
+  double joint_position_tau_{0.04};
 
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
   std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
   std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
-  // FRI работает в отдельном потоке: step() блокируется в recvfrom() и не жрёт CPU.
+  // FRI работает в отдельном потоке: step() блокируется в recvfrom().
   // read() лишь читает готовый снимок — без блокировки RT-потока.
-  // Соотношение update_rate:FRI_rate = 2:1 → JTC работает вдвое быстрее FRI,
-  // как при fri_cycle_ms=10. Это естественно «усредняет» команды и убирает дребезг.
   std::thread fri_thread_;
   std::atomic<bool> fri_running_{false};
   void friThreadFunc();
@@ -81,13 +83,17 @@ private:
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
 
-  // Предыдущие позиции и скорость (обновляются только при свежем FRI-пакете)
+  // Вычисление скорости конечными разностями
   std::array<double, N_JOINTS> prev_pos_{};
-  std::array<double, N_JOINTS> vel_filtered_{};
+  std::array<double, N_JOINTS> velocity_{};
   unsigned int last_ts_sec_{0};
   unsigned int last_ts_nsec_{0};
+  bool velocity_initialized_{false};
+  void compute_velocity_(const IIWAStateSnapshot & snap);
+
+  // Отслеживание сессии FRI для обнаружения потери управления
+  KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE};
 
-  // Отдельный объект часов для RCLCPP_*_THROTTLE — не создаём временный в FRI-потоке
   rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};
 
   CommandMode parseCommandMode(const std::string & mode_str) const;

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -2,6 +2,7 @@
 
 #include <chrono>
 #include <cmath>
+#include <cstdint>
 #include <thread>
 
 #include "hardware_interface/hardware_info.hpp"
@@ -20,6 +21,8 @@ namespace iiwa_controller
 using CallbackReturn =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
+static const char * LOG = "IIWAHardwareInterface";
+
 static std::string getParam(
   const hardware_interface::HardwareInfo & info,
   const std::string & name,
@@ -29,6 +32,8 @@ static std::string getParam(
   return (it != info.hardware_parameters.end()) ? it->second : default_val;
 }
 
+// ── on_init ────────────────────────────────────────────────────────────────────
+
 CallbackReturn IIWAHardwareInterface::on_init(
   const hardware_interface::HardwareComponentInterfaceParams & params)
 {
@@ -38,14 +43,13 @@ CallbackReturn IIWAHardwareInterface::on_init(
 
   const auto & info = params.hardware_info;
 
-  robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
+  robot_ip_           = getParam(info, "robot_ip", "192.170.10.2");
-  fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
+  fri_port_           = std::stoi(getParam(info, "fri_port", "30200"));
-  simulate_ = (getParam(info, "simulate", "false") == "true");
+  simulate_           = (getParam(info, "simulate", "false") == "true");
-  cmd_mode_str_ = getParam(info, "command_mode", "position");
+  cmd_mode_str_       = getParam(info, "command_mode", "position");
   joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04"));
 
-  RCLCPP_INFO(
+  RCLCPP_INFO(rclcpp::get_logger(LOG),
-    rclcpp::get_logger("IIWAHardwareInterface"),
     "on_init: ip=%s port=%d simulate=%s mode=%s tau=%.3f",
     robot_ip_.c_str(), fri_port_,
     simulate_ ? "true" : "false",
@@ -53,18 +57,18 @@ CallbackReturn IIWAHardwareInterface::on_init(
     joint_position_tau_);
 
   if (info.joints.size() != N_JOINTS) {
-    RCLCPP_FATAL(
+    RCLCPP_FATAL(rclcpp::get_logger(LOG),
-      rclcpp::get_logger("IIWAHardwareInterface"),
       "URDF содержит %zu суставов, ожидается %zu", info.joints.size(), N_JOINTS);
     return CallbackReturn::ERROR;
   }
 
   prev_pos_.fill(0.0);
+  velocity_.fill(0.0);
   return CallbackReturn::SUCCESS;
 }
 
-// external_torque не объявлен в URDF, поэтому добавляем его вручную как unlisted.
+// ── export_unlisted_state_interface_descriptions ────────────────────────────────
-// Стандартные интерфейсы (position, velocity, effort) базовый класс берёт из URDF сам.
+
 std::vector<hardware_interface::InterfaceDescription>
 IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 {
@@ -73,8 +77,8 @@ IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
     hardware_interface::InterfaceInfo if_info;
-    if_info.name = "external_torque";
+    if_info.name          = "external_torque";
-    if_info.data_type = "double";
+    if_info.data_type     = "double";
     if_info.initial_value = "0.0";
     descs.emplace_back(info_.joints[i].name, if_info);
   }
@@ -82,29 +86,55 @@ IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
   return descs;
 }
 
-CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str) const
+// ── on_configure ───────────────────────────────────────────────────────────────
+// Открывает UDP-сокет и создаёт FRI-объекты. Не запускает поток.
+
+CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State &)
 {
-  return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
+  if (simulate_) {
+    RCLCPP_WARN(rclcpp::get_logger(LOG), "РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
+    return CallbackReturn::SUCCESS;
+  }
+
+  fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_), joint_position_tau_);
+  // 100 мс таймаут recvfrom — поток корректно завершится после disconnect().
+  connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
+  app_        = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
+
+  if (!app_->connect(fri_port_, robot_ip_.c_str())) {
+    RCLCPP_FATAL(rclcpp::get_logger(LOG),
+      "Не удалось открыть UDP-сокет на порту %d (робот: %s)", fri_port_, robot_ip_.c_str());
+    return CallbackReturn::ERROR;
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger(LOG),
+    "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
+    fri_port_, robot_ip_.c_str());
+
+  return CallbackReturn::SUCCESS;
 }
 
+// ── on_activate ────────────────────────────────────────────────────────────────
+// Получает хэндлы интерфейсов, запускает FRI-поток и ждёт установки сессии.
+
 CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Активация...");
+  RCLCPP_INFO(rclcpp::get_logger(LOG), "Активация...");
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
     const std::string & jn = info_.joints[i].name;
 
-    h_pos_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
+    h_pos_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
-    h_vel_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY);
+    h_vel_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY);
-    h_eff_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
+    h_eff_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
-    h_ext_[i] = get_state_interface_handle(jn + "/external_torque");
+    h_ext_[i]     = get_state_interface_handle(jn + "/external_torque");
-
     h_cmd_pos_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
     h_cmd_eff_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
 
-    if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i] || !h_cmd_eff_[i]) {
+    if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] ||
-      RCLCPP_FATAL(
+        !h_cmd_pos_[i] || !h_cmd_eff_[i])
-        rclcpp::get_logger("IIWAHardwareInterface"),
+    {
+      RCLCPP_FATAL(rclcpp::get_logger(LOG),
         "Не удалось получить хэндл интерфейса для сустава '%s'. "
         "Проверьте объявление <state_interface>/<command_interface> в URDF.",
         jn.c_str());
@@ -113,93 +143,55 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
   }
 
   if (simulate_) {
-    RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"), "РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
     return CallbackReturn::SUCCESS;
   }
 
-  fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_), joint_position_tau_);
-  // 100 мс таймаут: если закрытие сокета не разблокирует recvfrom() мгновенно,
-  // поток всё равно выйдет через одну итерацию.
-  connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
-  app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
-
-  if (!app_->connect(fri_port_, robot_ip_.c_str())) {
-    RCLCPP_FATAL(
-      rclcpp::get_logger("IIWAHardwareInterface"),
-      "Не удалось подключиться к %s:%d", robot_ip_.c_str(), fri_port_);
-    return CallbackReturn::ERROR;
-  }
-
-  RCLCPP_INFO(
-    rclcpp::get_logger("IIWAHardwareInterface"),
-    "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
-    fri_port_, robot_ip_.c_str());
-
   fri_running_.store(true, std::memory_order_relaxed);
   fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
 
-  // Ждём пока FRI-сессия установится, максимум 15 секунд.
   constexpr int kTimeoutMs = 15000;
-  constexpr int kPollMs = 100;
+  constexpr int kPollMs    = 200;
-  int elapsed = 0;
+  for (int elapsed = 0;
-  while (fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs) {
+       fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs;
+       elapsed += kPollMs)
+  {
+    RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
+      "Ожидание FRI-сессии... (%d мс)", elapsed);
     std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
-    elapsed += kPollMs;
   }
 
   if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      rclcpp::get_logger("IIWAHardwareInterface"),
       "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
       kTimeoutMs / 1000, robot_ip_.c_str());
   } else {
-    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI сессия установлена!");
+    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI сессия установлена!");
-
     const auto snap = fri_client_->getStateSnapshot();
-    prev_pos_ = snap.measured_pos;
+    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;
+    velocity_.fill(0.0);
+    velocity_initialized_ = true;
   }
 
+  previous_session_state_ = fri_client_->getSessionState();
   return CallbackReturn::SUCCESS;
 }
 
-// Отдельный поток для FRI: step() блокируется в recvfrom() пока не придёт UDP-пакет,
+// ── on_deactivate ──────────────────────────────────────────────────────────────
-// потом вызывает нужный callback и отправляет ответ роботу.
+// Останавливает FRI-поток. FRI-объекты остаются — их очищает on_cleanup().
-// read() лишь читает готовый снимок — без блокировки RT-потока и без нарушения периода.
-void IIWAHardwareInterface::friThreadFunc()
-{
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI поток запущен");
-
-  while (fri_running_.load(std::memory_order_relaxed)) {
-    if (!app_->step()) {
-      RCLCPP_WARN_THROTTLE(
-        rclcpp::get_logger("IIWAHardwareInterface"),
-        throttle_clock_, 2000,
-        "FRI: step() вернул false, возможно потеряли соединение");
-    }
-  }
-
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI поток завершён");
-}
 
 CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Деактивация...");
+  RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация...");
 
-  if (!simulate_) {
+  if (!simulate_ && fri_running_.load()) {
     fri_running_.store(false, std::memory_order_relaxed);
-    // Сначала закрываем сокет, это разблокирует recvfrom() в FRI-потоке.
+    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
-    // Только после этого ждём завершения потока. Если сделать наоборот,
+    // Только потом join(), иначе он зависнет навсегда.
-    // join() зависнет навсегда потому что поток заблокирован в recvfrom().
+    if (app_) { app_->disconnect(); }
-    if (app_) {
+    if (fri_thread_.joinable()) { fri_thread_.join(); }
-      app_->disconnect();
+    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен");
-    }
-    if (fri_thread_.joinable()) {
-      fri_thread_.join();
-    }
-    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI отключён");
   }
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
@@ -207,12 +199,81 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
     h_cmd_pos_[i] = h_cmd_eff_[i] = nullptr;
   }
 
+  velocity_initialized_ = false;
   return CallbackReturn::SUCCESS;
 }
 
-// read() не блокируется — берёт последний снимок от FRI-потока через мьютекс.
+// ── on_cleanup ─────────────────────────────────────────────────────────────────
-// Период JTC остаётся стабильным: при update_rate=400 и fri_cycle_ms=5 соотношение 2:1,
+// Освобождает FRI-объекты. Вызывается после on_deactivate().
-// идентичное рабочей конфигурации fri_cycle_ms=10 + update_rate=200.
+
+CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  fri_client_.reset();
+  connection_.reset();
+  app_.reset();
+  return CallbackReturn::SUCCESS;
+}
+
+// ── friThreadFunc ──────────────────────────────────────────────────────────────
+
+void IIWAHardwareInterface::friThreadFunc()
+{
+  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен");
+
+  while (fri_running_.load(std::memory_order_relaxed)) {
+    if (!app_->step()) {
+      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
+        "FRI: step() вернул false, возможно потеряли соединение");
+    }
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток завершён");
+}
+
+// ── compute_velocity_ ──────────────────────────────────────────────────────────
+// Конечные разности с int64-вычитанием для точности при больших Unix-timestamp'ах.
+
+void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
+{
+  if (!velocity_initialized_) {
+    prev_pos_     = snap.measured_pos;
+    last_ts_sec_  = snap.time_stamp_sec;
+    last_ts_nsec_ = snap.time_stamp_nano_sec;
+    velocity_.fill(0.0);
+    velocity_initialized_ = true;
+    return;
+  }
+
+  if (snap.time_stamp_sec == last_ts_sec_ && snap.time_stamp_nano_sec == last_ts_nsec_) {
+    return;  // нового FRI-пакета ещё нет
+  }
+
+  const double dt =
+    static_cast<double>(
+      static_cast<int64_t>(snap.time_stamp_sec) -
+      static_cast<int64_t>(last_ts_sec_)) +
+    (static_cast<double>(snap.time_stamp_nano_sec) -
+     static_cast<double>(last_ts_nsec_)) * 1e-9;
+
+  static constexpr std::array<double, N_JOINTS> kMaxVel =
+    {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
+  static constexpr double kVelDeadband = 1e-4;
+
+  if (dt > 0.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;
+    }
+  }
+
+  prev_pos_     = snap.measured_pos;
+  last_ts_sec_  = snap.time_stamp_sec;
+  last_ts_nsec_ = snap.time_stamp_nano_sec;
+}
+
+// ── read ───────────────────────────────────────────────────────────────────────
+
 hardware_interface::return_type IIWAHardwareInterface::read(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
@@ -230,38 +291,22 @@ hardware_interface::return_type IIWAHardwareInterface::read(
 
   const auto snap = fri_client_->getStateSnapshot();
 
-  // Обновляем скорость только при свежем FRI-пакете.
+  // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE.
-  // measured_pos в Commanding = filtered_pos_ (open-loop), поэтому скорость — это
+  const auto current_state = fri_client_->getSessionState();
-  // производная сглаженной команды: гладкий сигнал без шума датчика и без лага.
+  if (previous_session_state_ == KUKA::FRI::COMMANDING_ACTIVE &&
-  const bool fresh = (snap.time_stamp_sec  != last_ts_sec_ ||
+      current_state != KUKA::FRI::COMMANDING_ACTIVE)
-                      snap.time_stamp_nano_sec != last_ts_nsec_);
+  {
-  if (fresh) {
+    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-    const double dt =
+      "Робот вышел из COMMANDING_ACTIVE! Деактивируйте и повторно активируйте контроллер.");
-      (static_cast<double>(snap.time_stamp_sec)       - static_cast<double>(last_ts_sec_)) +
+    return hardware_interface::return_type::ERROR;
-      (static_cast<double>(snap.time_stamp_nano_sec)  - static_cast<double>(last_ts_nsec_)) * 1e-9;
-
-    // iiwa7 physical velocity limits [rad/s], used to clamp impossible spikes
-    static constexpr std::array<double, N_JOINTS> kMaxVel =
-      {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
-    static constexpr double kVelDeadband = 1e-4;
-
-    if (dt > 0.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]);
-        vel_filtered_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
-      }
-    }
-    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;
   }
+  previous_session_state_ = current_state;
+
+  compute_velocity_(snap);
 
   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_vel_[i], velocity_[i],         false);
     set_state(h_eff_[i], snap.measured_tau[i], false);
     set_state(h_ext_[i], snap.external_tau[i], false);
   }
@@ -269,6 +314,8 @@ hardware_interface::return_type IIWAHardwareInterface::read(
   return hardware_interface::return_type::OK;
 }
 
+// ── write ──────────────────────────────────────────────────────────────────────
+
 hardware_interface::return_type IIWAHardwareInterface::write(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
@@ -276,6 +323,10 @@ hardware_interface::return_type IIWAHardwareInterface::write(
     return hardware_interface::return_type::OK;
   }
 
+  if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) {
+    return hardware_interface::return_type::OK;
+  }
+
   std::array<double, N_JOINTS> pos_cmd{}, tau_cmd{};
   for (size_t i = 0; i < N_JOINTS; ++i) {
     get_command(h_cmd_pos_[i], pos_cmd[i], false);
@@ -288,4 +339,11 @@ hardware_interface::return_type IIWAHardwareInterface::write(
   return hardware_interface::return_type::OK;
 }
 
+// ── parseCommandMode ───────────────────────────────────────────────────────────
+
+CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str) const
+{
+  return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
+}
+
 }  // namespace iiwa_controller

src/iiwa_controller_v2/CMakeLists.txt

@@ -1,93 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(iiwa_controller_v2)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-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)
-
-# ── FRI SDK (shared with iiwa_controller via symlink) ─────────────────────────
-set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI)
-
-file(GLOB_RECURSE FRI_SOURCES
-  "${FRI_SDK_DIR}/src/base/*.cpp"
-  "${FRI_SDK_DIR}/src/client_lbr/*.cpp"
-  "${FRI_SDK_DIR}/src/client_trafo/*.cpp"
-  "${FRI_SDK_DIR}/src/connection/*.cpp"
-  "${FRI_SDK_DIR}/src/nanopb-0.2.8/*.c"
-  "${FRI_SDK_DIR}/src/protobuf/*.c"
-  "${FRI_SDK_DIR}/src/protobuf/*.cpp"
-  "${FRI_SDK_DIR}/src/protobuf_gen/*.c"
-)
-
-add_library(fri_client_sdk_v2 STATIC ${FRI_SOURCES})
-
-target_include_directories(fri_client_sdk_v2 PUBLIC
-  ${FRI_SDK_DIR}/include
-  ${FRI_SDK_DIR}/src/nanopb-0.2.8
-  ${FRI_SDK_DIR}/src/protobuf
-  ${FRI_SDK_DIR}/src/protobuf_gen
-  ${FRI_SDK_DIR}/src/base
-  ${FRI_SDK_DIR}/src/client_lbr
-  ${FRI_SDK_DIR}/src/connection
-)
-
-target_compile_definitions(fri_client_sdk_v2 PUBLIC PB_FIELD_16BIT)
-target_compile_options(fri_client_sdk_v2 PRIVATE -fpermissive -w)
-set_target_properties(fri_client_sdk_v2 PROPERTIES POSITION_INDEPENDENT_CODE ON)
-target_link_libraries(fri_client_sdk_v2 PUBLIC pthread)
-
-# ── Plugin library ─────────────────────────────────────────────────────────────
-add_library(${PROJECT_NAME} SHARED
-  src/fri_client.cpp
-  src/system_interface.cpp
-)
-
-target_include_directories(${PROJECT_NAME} PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-)
-
-target_link_libraries(${PROJECT_NAME} PRIVATE
-  fri_client_sdk_v2
-  controller_interface::controller_interface
-  hardware_interface::hardware_interface
-  pluginlib::pluginlib
-  rclcpp::rclcpp
-  rclcpp_lifecycle::rclcpp_lifecycle
-  realtime_tools::realtime_tools
-)
-
-pluginlib_export_plugin_description_file(
-  hardware_interface
-  iiwa_hardware_interface_plugin.xml
-)
-
-# ── Install ────────────────────────────────────────────────────────────────────
-install(TARGETS ${PROJECT_NAME}
-  EXPORT export_${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-
-install(DIRECTORY include/
-  DESTINATION include
-)
-
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
-ament_export_targets(export_${PROJECT_NAME})
-ament_export_dependencies(
-  controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools)
-
-ament_package()

src/iiwa_controller_v2/external

@@ -1 +0,0 @@
-/home/daniel/dev/ros2_iiwa7/src/iiwa_controller/external

src/iiwa_controller_v2/iiwa_hardware_interface_plugin.xml

@@ -1,13 +0,0 @@
-<library path="iiwa_controller_v2">
-  <class
-    name="iiwa_controller_v2/SystemInterface"
-    type="iiwa_controller_v2::SystemInterface"
-    base_class_type="hardware_interface::SystemInterface">
-    <description>
-      ROS2 hardware interface for KUKA iiwa7 via FRI (v2).
-      Supports position and torque command modes.
-      Exposes extended state interfaces: external_torque, commanded_torque,
-      ipo_joint_position, sample_time, session_state, connection_quality, timestamps.
-    </description>
-  </class>
-</library>

src/iiwa_controller_v2/include/iiwa_controller_v2/command_guard.hpp

@@ -1,95 +0,0 @@
-#pragma once
-
-#include <array>
-#include <cmath>
-#include <limits>
-#include <string>
-
-#include "hardware_interface/hardware_info.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-#include "iiwa_controller_v2/fri_client.hpp"
-
-namespace iiwa_controller_v2
-{
-
-// Checks commands against joint limits parsed from the URDF before they reach the FRI client.
-// Limits are read from <command_interface name="position" min="..." max="..."/> and
-// <command_interface name="effort" max="..."/> in the robot description.
-struct CommandGuard
-{
-  struct JointLimits
-  {
-    std::string name;
-    double min_position{-std::numeric_limits<double>::infinity()};
-    double max_position{std::numeric_limits<double>::infinity()};
-    double max_torque{std::numeric_limits<double>::infinity()};
-  };
-
-  std::array<JointLimits, FRIClient::N_JOINTS> limits;
-
-  // Populate limits from URDF hardware info. Call once in on_init.
-  void configure(const hardware_interface::HardwareInfo & info)
-  {
-    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-      const auto & joint = info.joints[i];
-      limits[i].name = joint.name;
-
-      for (const auto & ci : joint.command_interfaces) {
-        if (ci.name == "position") {
-          if (!ci.min.empty()) {
-            limits[i].min_position = std::stod(ci.min);
-          }
-          if (!ci.max.empty()) {
-            limits[i].max_position = std::stod(ci.max);
-          }
-        } else if (ci.name == "effort") {
-          if (!ci.max.empty()) {
-            limits[i].max_torque = std::stod(ci.max);
-          }
-        }
-      }
-    }
-  }
-
-  // Returns false and logs the violation if any position command is out of range.
-  bool check_position(const std::array<double, FRIClient::N_JOINTS> & pos,
-                      const char * logger_name) const
-  {
-    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-      if (!std::isfinite(pos[i])) {
-        continue;  // NaN/inf filtering is already done in FRIClient
-      }
-      if (pos[i] < limits[i].min_position || pos[i] > limits[i].max_position) {
-        RCLCPP_ERROR(
-          rclcpp::get_logger(logger_name),
-          "Position command for '%s' = %.4f rad is outside limits [%.4f, %.4f]",
-          limits[i].name.c_str(), pos[i],
-          limits[i].min_position, limits[i].max_position);
-        return false;
-      }
-    }
-    return true;
-  }
-
-  // Returns false and logs the violation if any torque command exceeds the limit.
-  bool check_torque(const std::array<double, FRIClient::N_JOINTS> & tau,
-                    const char * logger_name) const
-  {
-    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-      if (!std::isfinite(tau[i])) {
-        continue;
-      }
-      if (std::abs(tau[i]) > limits[i].max_torque) {
-        RCLCPP_ERROR(
-          rclcpp::get_logger(logger_name),
-          "Torque command for '%s' = %.2f Nm exceeds limit %.2f Nm",
-          limits[i].name.c_str(), tau[i], limits[i].max_torque);
-        return false;
-      }
-    }
-    return true;
-  }
-};
-
-}  // namespace iiwa_controller_v2

src/iiwa_controller_v2/include/iiwa_controller_v2/fri_client.hpp

@@ -1,74 +0,0 @@
-#pragma once
-
-#include <array>
-#include <atomic>
-#include <mutex>
-
-#include "friClientApplication.h"
-#include "friLBRClient.h"
-#include "friUdpConnection.h"
-
-namespace iiwa_controller_v2
-{
-
-enum class CommandMode
-{
-  POSITION,
-  TORQUE
-};
-
-// Atomic snapshot of robot state, captured each FRI cycle and consumed by read().
-struct IIWAStateSnapshot
-{
-  std::array<double, 7> measured_pos{};    // open-loop: = filtered_pos_ in COMMANDING_ACTIVE
-  std::array<double, 7> ipo_pos{};         // interpolator position (valid only in COMMANDING)
-  std::array<double, 7> measured_tau{};    // measured joint torques [Nm]
-  std::array<double, 7> commanded_tau{};   // torques sent to robot in previous cycle [Nm]
-  std::array<double, 7> external_tau{};    // estimated external torques (gravity-compensated) [Nm]
-  double sample_time{0.005};
-  KUKA::FRI::EConnectionQuality connection_quality{KUKA::FRI::POOR};
-  KUKA::FRI::ESessionState session_state{KUKA::FRI::IDLE};
-  bool ipo_valid{false};
-  unsigned int time_stamp_sec{0};
-  unsigned int time_stamp_nano_sec{0};
-};
-
-class FRIClient : public KUKA::FRI::LBRClient
-{
-public:
-  static constexpr size_t N_JOINTS = 7;
-
-  explicit FRIClient(CommandMode mode = CommandMode::POSITION, double joint_position_tau = 0.04);
-  ~FRIClient() override = default;
-
-  // FRI SDK callbacks — called from friThreadFunc via ClientApplication::step()
-  void monitor() override;
-  void waitForCommand() override;
-  void command() override;
-  void onStateChange(KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
-
-  // Thread-safe API for the ros2_control loop
-  void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
-  void setTargetJointTorques(const std::array<double, N_JOINTS> & tau);
-  IIWAStateSnapshot getStateSnapshot() const;
-  KUKA::FRI::ESessionState getSessionState() const;
-
-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_{};
-  // Filtered position actually sent to the robot; initialised from IPO in waitForCommand().
-  std::array<double, N_JOINTS> filtered_pos_{};
-  IIWAStateSnapshot snapshot_{};
-
-  // Monitor state: getIpoJointPosition() is NOT available here.
-  void captureMonitoringData();
-  // COMMANDING states: IPO position is available and snapshot_.measured_pos = filtered_pos_.
-  void captureCommandingData();
-};
-
-}  // namespace iiwa_controller_v2

src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp

@@ -1,216 +0,0 @@
-#pragma once
-
-#include <array>
-#include <atomic>
-#include <limits>
-#include <memory>
-#include <string>
-#include <thread>
-#include <vector>
-
-#include "hardware_interface/hardware_info.hpp"
-#include "hardware_interface/system_interface.hpp"
-#include "hardware_interface/types/hardware_component_interface_params.hpp"
-#include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "rclcpp/clock.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/state.hpp"
-
-#include "iiwa_controller_v2/command_guard.hpp"
-#include "iiwa_controller_v2/fri_client.hpp"
-#include "iiwa_controller_v2/system_interface_type_values.hpp"
-
-namespace iiwa_controller_v2
-{
-
-class SystemInterface : public hardware_interface::SystemInterface
-{
-protected:
-  // ── Parameters ─────────────────────────────────────────────────────────────
-  struct Parameters
-  {
-    std::string robot_ip{"192.170.10.2"};
-    int         fri_port{30200};
-    bool        simulate{false};
-    std::string command_mode{"position"};
-    double      joint_position_tau{0.04};
-    bool        open_loop{true};   // JTC sees filtered_pos, not measured_pos
-    int         rt_prio{80};
-  };
-
-  // ── Command interface handles ───────────────────────────────────────────────
-  struct CommandInterfaceHandles
-  {
-    std::array<hardware_interface::CommandInterface::SharedPtr, FRIClient::N_JOINTS>
-      joint_position, torque;
-
-    void populate(const hardware_interface::SystemInterface & si,
-                  const hardware_interface::HardwareInfo & info)
-    {
-      for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-        const auto & jn = info.joints[i].name;
-        joint_position[i] = si.get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
-        torque[i]         = si.get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
-      }
-    }
-
-    void nan_interfaces() const
-    {
-      for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-        std::ignore = joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
-        std::ignore = torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
-      }
-    }
-
-    void pull(std::array<double, FRIClient::N_JOINTS> & pos_cmd,
-              std::array<double, FRIClient::N_JOINTS> & tau_cmd) const
-    {
-      for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-        pos_cmd[i] = joint_position[i]->get_optional().value_or(
-                       std::numeric_limits<double>::quiet_NaN());
-        tau_cmd[i] = torque[i]->get_optional().value_or(
-                       std::numeric_limits<double>::quiet_NaN());
-      }
-    }
-  };
-
-  // ── State interface handles ─────────────────────────────────────────────────
-  struct StateInterfaceHandles
-  {
-    // Standard per-joint
-    std::array<hardware_interface::StateInterface::SharedPtr, FRIClient::N_JOINTS>
-      position, velocity, effort;
-    // Extended per-joint (registered as unlisted)
-    std::array<hardware_interface::StateInterface::SharedPtr, FRIClient::N_JOINTS>
-      external_torque, commanded_torque, ipo_joint_position;
-    // Auxiliary (robot-level, registered as unlisted)
-    hardware_interface::StateInterface::SharedPtr
-      sample_time, session_state, connection_quality,
-      time_stamp_sec, time_stamp_nano_sec;
-
-    void populate(const hardware_interface::SystemInterface & si,
-                  const hardware_interface::HardwareInfo & info)
-    {
-      for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-        const auto & jn = info.joints[i].name;
-        position[i]          = si.get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
-        velocity[i]          = si.get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY);
-        effort[i]            = si.get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
-        external_torque[i]   = si.get_state_interface_handle(jn + "/" + HW_IF_EXTERNAL_TORQUE);
-        commanded_torque[i]  = si.get_state_interface_handle(jn + "/" + HW_IF_COMMANDED_TORQUE);
-        ipo_joint_position[i]= si.get_state_interface_handle(jn + "/" + HW_IF_IPO_JOINT_POSITION);
-      }
-      const std::string aux = std::string(HW_IF_AUXILIARY_PREFIX) + "/";
-      sample_time         = si.get_state_interface_handle(aux + HW_IF_SAMPLE_TIME);
-      session_state       = si.get_state_interface_handle(aux + HW_IF_SESSION_STATE);
-      connection_quality  = si.get_state_interface_handle(aux + HW_IF_CONNECTION_QUALITY);
-      time_stamp_sec      = si.get_state_interface_handle(aux + HW_IF_TIME_STAMP_SEC);
-      time_stamp_nano_sec = si.get_state_interface_handle(aux + HW_IF_TIME_STAMP_NANO_SEC);
-    }
-
-    void nan_interfaces() const
-    {
-      const double nan = std::numeric_limits<double>::quiet_NaN();
-      for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-        std::ignore = position[i]->set_value(nan);
-        std::ignore = velocity[i]->set_value(nan);
-        std::ignore = effort[i]->set_value(nan);
-        std::ignore = external_torque[i]->set_value(nan);
-        std::ignore = commanded_torque[i]->set_value(nan);
-        std::ignore = ipo_joint_position[i]->set_value(nan);
-      }
-      std::ignore = sample_time->set_value(nan);
-      std::ignore = session_state->set_value(nan);
-      std::ignore = connection_quality->set_value(nan);
-      std::ignore = time_stamp_sec->set_value(nan);
-      std::ignore = time_stamp_nano_sec->set_value(nan);
-    }
-
-    void push(const IIWAStateSnapshot & snap,
-              const std::array<double, FRIClient::N_JOINTS> & vel) const
-    {
-      for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-        std::ignore = position[i]->set_value(snap.measured_pos[i]);
-        std::ignore = velocity[i]->set_value(vel[i]);
-        std::ignore = effort[i]->set_value(snap.measured_tau[i]);
-        std::ignore = external_torque[i]->set_value(snap.external_tau[i]);
-        std::ignore = commanded_torque[i]->set_value(snap.commanded_tau[i]);
-        std::ignore = ipo_joint_position[i]->set_value(snap.ipo_pos[i]);
-      }
-      std::ignore = sample_time->set_value(snap.sample_time);
-      std::ignore = session_state->set_value(static_cast<double>(snap.session_state));
-      std::ignore = connection_quality->set_value(static_cast<double>(snap.connection_quality));
-      std::ignore = time_stamp_sec->set_value(static_cast<double>(snap.time_stamp_sec));
-      std::ignore = time_stamp_nano_sec->set_value(static_cast<double>(snap.time_stamp_nano_sec));
-    }
-  };
-
-public:
-  SystemInterface() = default;
-
-  // ── Lifecycle ───────────────────────────────────────────────────────────────
-  hardware_interface::CallbackReturn on_init(
-    const hardware_interface::HardwareComponentInterfaceParams & params) override;
-
-  std::vector<hardware_interface::InterfaceDescription>
-  export_unlisted_state_interface_descriptions() override;
-
-  hardware_interface::return_type prepare_command_mode_switch(
-    const std::vector<std::string> & start_interfaces,
-    const std::vector<std::string> & stop_interfaces) override;
-
-  hardware_interface::CallbackReturn on_configure(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  hardware_interface::CallbackReturn on_activate(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  hardware_interface::CallbackReturn on_deactivate(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  hardware_interface::CallbackReturn on_cleanup(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  hardware_interface::return_type read(
-    const rclcpp::Time & time, const rclcpp::Duration & period) override;
-
-  hardware_interface::return_type write(
-    const rclcpp::Time & time, const rclcpp::Duration & period) override;
-
-protected:
-  bool parse_parameters_();
-
-  // Returns true when the robot leaves COMMANDING_ACTIVE unexpectedly.
-  bool exit_commanding_active_(KUKA::FRI::ESessionState previous,
-                               KUKA::FRI::ESessionState current);
-
-  void friThreadFunc();
-
-  // Compute finite-difference velocity from FRI timestamps.
-  void compute_velocity_(const IIWAStateSnapshot & snap);
-
-  // ── Members ────────────────────────────────────────────────────────────────
-  Parameters parameters_;
-
-  std::unique_ptr<FRIClient>                     fri_client_;
-  std::unique_ptr<KUKA::FRI::UdpConnection>      connection_;
-  std::unique_ptr<KUKA::FRI::ClientApplication>  app_;
-
-  std::thread          fri_thread_;
-  std::atomic<bool>    fri_running_{false};
-  rclcpp::Clock        throttle_clock_{RCL_STEADY_TIME};
-
-  KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE};
-
-  std::array<double, FRIClient::N_JOINTS> velocity_{};
-  std::array<double, FRIClient::N_JOINTS> last_pos_{};
-  double last_ts_sec_{0.0};
-  double last_ts_nsec_{0.0};
-  bool   velocity_initialized_{false};
-
-  CommandGuard            command_guard_;
-  CommandInterfaceHandles command_if_handles_;
-  StateInterfaceHandles   state_if_handles_;
-};
-
-}  // namespace iiwa_controller_v2

src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface_type_values.hpp

@@ -1,21 +0,0 @@
-#pragma once
-
-namespace iiwa_controller_v2
-{
-
-// Per-joint state interface names (beyond standard position/velocity/effort)
-constexpr char HW_IF_EXTERNAL_TORQUE[]    = "external_torque";
-constexpr char HW_IF_COMMANDED_TORQUE[]   = "commanded_torque";
-constexpr char HW_IF_IPO_JOINT_POSITION[] = "ipo_joint_position";
-
-// Auxiliary sensor prefix (used for robot-level telemetry)
-constexpr char HW_IF_AUXILIARY_PREFIX[]   = "auxiliary";
-
-// Auxiliary sensor state interface names
-constexpr char HW_IF_SAMPLE_TIME[]          = "sample_time";
-constexpr char HW_IF_SESSION_STATE[]        = "session_state";
-constexpr char HW_IF_CONNECTION_QUALITY[]   = "connection_quality";
-constexpr char HW_IF_TIME_STAMP_SEC[]       = "time_stamp_sec";
-constexpr char HW_IF_TIME_STAMP_NANO_SEC[]  = "time_stamp_nano_sec";
-
-}  // namespace iiwa_controller_v2

src/iiwa_controller_v2/package.xml

@@ -1,29 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>iiwa_controller_v2</name>
-  <version>0.2.0</version>
-  <description>
-    Hardware interface v2 for KUKA iiwa7 via FRI (Fast Robot Interface).
-    Structured after lbr_fri_ros2_stack/lbr_ros2_control with lifecycle-aware
-    UDP socket management, extended state interfaces, and external-torque safety guard.
-  </description>
-  <maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
-  <license>Apache-2.0</license>
-
-  <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>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/iiwa_controller_v2/src/fri_client.cpp

@@ -1,154 +0,0 @@
-#include "iiwa_controller_v2/fri_client.hpp"
-
-#include <cmath>
-#include <cstring>
-
-#include "rclcpp/rclcpp.hpp"
-
-namespace iiwa_controller_v2
-{
-
-static const char * sessionStateName(KUKA::FRI::ESessionState s)
-{
-  switch (s) {
-    case KUKA::FRI::IDLE:             return "IDLE";
-    case KUKA::FRI::MONITORING_WAIT:  return "MONITORING_WAIT";
-    case KUKA::FRI::MONITORING_READY: return "MONITORING_READY";
-    case KUKA::FRI::COMMANDING_WAIT:  return "COMMANDING_WAIT";
-    case KUKA::FRI::COMMANDING_ACTIVE:return "COMMANDING_ACTIVE";
-    default:                          return "UNKNOWN";
-  }
-}
-
-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);
-}
-
-void FRIClient::captureMonitoringData()
-{
-  // getIpoJointPosition() throws in Monitor states — do NOT call it here.
-  std::memcpy(snapshot_.measured_pos.data(),
-              robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double));
-  std::memcpy(snapshot_.measured_tau.data(),
-              robotState().getMeasuredTorque(), N_JOINTS * sizeof(double));
-  std::memcpy(snapshot_.commanded_tau.data(),
-              robotState().getCommandedTorque(), N_JOINTS * sizeof(double));
-  std::memcpy(snapshot_.external_tau.data(),
-              robotState().getExternalTorque(), N_JOINTS * sizeof(double));
-  snapshot_.sample_time        = robotState().getSampleTime();
-  snapshot_.connection_quality = robotState().getConnectionQuality();
-  snapshot_.session_state      = robotState().getSessionState();
-  snapshot_.ipo_valid          = false;
-  snapshot_.time_stamp_sec     = robotState().getTimestampSec();
-  snapshot_.time_stamp_nano_sec= robotState().getTimestampNanoSec();
-}
-
-void FRIClient::captureCommandingData()
-{
-  captureMonitoringData();
-  std::memcpy(snapshot_.ipo_pos.data(),
-              robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
-  snapshot_.ipo_valid = true;
-  // Open-loop: expose the filtered position as "measured" so JTC sees no error.
-  snapshot_.measured_pos = filtered_pos_;
-}
-
-void FRIClient::monitor()
-{
-  std::lock_guard<std::mutex> lk(data_mutex_);
-  captureMonitoringData();
-}
-
-void FRIClient::waitForCommand()
-{
-  std::lock_guard<std::mutex> lk(data_mutex_);
-  captureCommandingData();
-
-  // Initialise both target and filter from IPO to avoid a step on the first command cycle.
-  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(filtered_pos_.data());
-
-  if (cmd_mode_ == CommandMode::TORQUE) {
-    target_tau_.fill(0.0);
-    robotCommand().setTorque(target_tau_.data());
-  }
-}
-
-void FRIClient::command()
-{
-  std::lock_guard<std::mutex> lk(data_mutex_);
-
-  // EMA filter applied BEFORE snapshot so measured_pos = what we actually sent this cycle.
-  const double dt    = robotState().getSampleTime();
-  const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0;
-  for (std::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) {
-    robotCommand().setTorque(target_tau_.data());
-  }
-
-  captureCommandingData();
-}
-
-void FRIClient::onStateChange(
-  KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState)
-{
-  session_state_.store(newState, std::memory_order_relaxed);
-
-  RCLCPP_INFO(
-    rclcpp::get_logger("iiwa_controller_v2"),
-    "FRI state change: %s → %s", sessionStateName(oldState), sessionStateName(newState));
-
-  // Safety: zero torques whenever we leave COMMANDING states.
-  if (newState == KUKA::FRI::IDLE ||
-      newState == KUKA::FRI::MONITORING_WAIT ||
-      newState == KUKA::FRI::MONITORING_READY)
-  {
-    std::lock_guard<std::mutex> lk(data_mutex_);
-    target_tau_.fill(0.0);
-    RCLCPP_WARN(
-      rclcpp::get_logger("iiwa_controller_v2"),
-      "FRI left commanding — torque targets reset to zero");
-  }
-}
-
-void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
-{
-  for (const auto v : q) {
-    if (!std::isfinite(v)) { return; }
-  }
-  std::lock_guard<std::mutex> lk(data_mutex_);
-  target_pos_ = q;
-}
-
-void FRIClient::setTargetJointTorques(const std::array<double, N_JOINTS> & tau)
-{
-  for (const auto v : tau) {
-    if (!std::isfinite(v)) { return; }
-  }
-  std::lock_guard<std::mutex> lk(data_mutex_);
-  target_tau_ = tau;
-}
-
-IIWAStateSnapshot FRIClient::getStateSnapshot() const
-{
-  std::lock_guard<std::mutex> lk(data_mutex_);
-  return snapshot_;
-}
-
-KUKA::FRI::ESessionState FRIClient::getSessionState() const
-{
-  return session_state_.load(std::memory_order_relaxed);
-}
-
-}  // namespace iiwa_controller_v2

src/iiwa_controller_v2/src/system_interface.cpp

@@ -1,404 +0,0 @@
-#include "iiwa_controller_v2/system_interface.hpp"
-
-#include <chrono>
-#include <cmath>
-#include <cstdint>
-#include <thread>
-
-#include "hardware_interface/types/hardware_component_interface_params.hpp"
-#include "pluginlib/class_list_macros.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-PLUGINLIB_EXPORT_CLASS(
-  iiwa_controller_v2::SystemInterface,
-  hardware_interface::SystemInterface)
-
-namespace iiwa_controller_v2
-{
-
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-
-static const char * LOG = "iiwa_controller_v2";
-
-// ── Helpers ────────────────────────────────────────────────────────────────────
-
-static std::string getParam(
-  const hardware_interface::HardwareInfo & info,
-  const std::string & name,
-  const std::string & default_val = "")
-{
-  auto it = info.hardware_parameters.find(name);
-  return (it != info.hardware_parameters.end()) ? it->second : default_val;
-}
-
-// ── on_init ────────────────────────────────────────────────────────────────────
-
-CallbackReturn SystemInterface::on_init(
-  const hardware_interface::HardwareComponentInterfaceParams & params)
-{
-  if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) {
-    return CallbackReturn::ERROR;
-  }
-
-  if (!parse_parameters_()) {
-    return CallbackReturn::ERROR;
-  }
-
-  if (info_.joints.size() != FRIClient::N_JOINTS) {
-    RCLCPP_FATAL(rclcpp::get_logger(LOG),
-      "Expected %zu joints, got %zu", FRIClient::N_JOINTS, info_.joints.size());
-    return CallbackReturn::ERROR;
-  }
-
-  RCLCPP_INFO(rclcpp::get_logger(LOG),
-    "on_init: ip=%s port=%d simulate=%s mode=%s tau=%.3f open_loop=%s",
-    parameters_.robot_ip.c_str(), parameters_.fri_port,
-    parameters_.simulate ? "true" : "false",
-    parameters_.command_mode.c_str(),
-    parameters_.joint_position_tau,
-    parameters_.open_loop ? "true" : "false");
-
-  command_guard_.configure(info_);
-
-  velocity_.fill(0.0);
-  last_pos_.fill(0.0);
-  return CallbackReturn::SUCCESS;
-}
-
-// ── export_unlisted_state_interface_descriptions ────────────────────────────────
-
-std::vector<hardware_interface::InterfaceDescription>
-SystemInterface::export_unlisted_state_interface_descriptions()
-{
-  std::vector<hardware_interface::InterfaceDescription> descs;
-
-  // Per-joint extended interfaces
-  const std::array<const char *, 3> per_joint_names = {
-    HW_IF_EXTERNAL_TORQUE,
-    HW_IF_COMMANDED_TORQUE,
-    HW_IF_IPO_JOINT_POSITION,
-  };
-  descs.reserve(FRIClient::N_JOINTS * per_joint_names.size() + 5);
-
-  for (const auto * if_name : per_joint_names) {
-    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-      hardware_interface::InterfaceInfo ifi;
-      ifi.name          = if_name;
-      ifi.data_type     = "double";
-      ifi.initial_value = "0.0";
-      descs.emplace_back(info_.joints[i].name, ifi);
-    }
-  }
-
-  // Auxiliary robot-level interfaces (use a virtual sensor component name)
-  const std::array<const char *, 5> aux_names = {
-    HW_IF_SAMPLE_TIME,
-    HW_IF_SESSION_STATE,
-    HW_IF_CONNECTION_QUALITY,
-    HW_IF_TIME_STAMP_SEC,
-    HW_IF_TIME_STAMP_NANO_SEC,
-  };
-  for (const auto * if_name : aux_names) {
-    hardware_interface::InterfaceInfo ifi;
-    ifi.name          = if_name;
-    ifi.data_type     = "double";
-    ifi.initial_value = "0.0";
-    descs.emplace_back(HW_IF_AUXILIARY_PREFIX, ifi);
-  }
-
-  return descs;
-}
-
-// ── prepare_command_mode_switch ─────────────────────────────────────────────────
-
-hardware_interface::return_type SystemInterface::prepare_command_mode_switch(
-  const std::vector<std::string> & /*start*/, const std::vector<std::string> & /*stop*/)
-{
-  // FRI does not support online command-mode switching.
-  return hardware_interface::return_type::OK;
-}
-
-// ── on_configure ───────────────────────────────────────────────────────────────
-
-CallbackReturn SystemInterface::on_configure(const rclcpp_lifecycle::State &)
-{
-  if (parameters_.simulate) {
-    RCLCPP_WARN(rclcpp::get_logger(LOG), "SIMULATION MODE — FRI disabled");
-    return CallbackReturn::SUCCESS;
-  }
-
-  const CommandMode cmd_mode =
-    (parameters_.command_mode == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
-
-  fri_client_ = std::make_unique<FRIClient>(cmd_mode, parameters_.joint_position_tau);
-  // 100 ms receive timeout: ensures friThreadFunc can exit cleanly after disconnect().
-  connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
-  app_        = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
-
-  if (!app_->connect(parameters_.fri_port, parameters_.robot_ip.c_str())) {
-    RCLCPP_FATAL(rclcpp::get_logger(LOG),
-      "Failed to open UDP socket on port %d (robot: %s)",
-      parameters_.fri_port, parameters_.robot_ip.c_str());
-    return CallbackReturn::ERROR;
-  }
-
-  RCLCPP_INFO(rclcpp::get_logger(LOG),
-    "UDP socket open on port %d. Start ServerFriRos2 on robot (%s)...",
-    parameters_.fri_port, parameters_.robot_ip.c_str());
-
-  return CallbackReturn::SUCCESS;
-}
-
-// ── on_activate ────────────────────────────────────────────────────────────────
-
-CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &)
-{
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "Activating...");
-
-  // Populate interface handles
-  command_if_handles_.populate(*this, info_);
-  state_if_handles_.populate(*this, info_);
-  command_if_handles_.nan_interfaces();
-  state_if_handles_.nan_interfaces();
-
-  if (parameters_.simulate) {
-    return CallbackReturn::SUCCESS;
-  }
-
-  // Start FRI thread
-  fri_running_.store(true, std::memory_order_relaxed);
-  fri_thread_ = std::thread(&SystemInterface::friThreadFunc, this);
-
-  // Wait up to 15 s for a FRI session to be established.
-  constexpr int kTimeoutMs = 15000;
-  constexpr int kPollMs    = 200;
-  for (int elapsed = 0;
-       fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs;
-       elapsed += kPollMs)
-  {
-    RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-      "Waiting for FRI session... (%d ms elapsed)", elapsed);
-    std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
-  }
-
-  if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      "FRI session not established after %d s. Check ServerFriRos2 on %s",
-      kTimeoutMs / 1000, parameters_.robot_ip.c_str());
-    // Don't fail — user may still start the robot-side app.
-  } else {
-    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI session established!");
-    const auto snap = fri_client_->getStateSnapshot();
-    last_pos_    = snap.measured_pos;
-    last_ts_sec_ = static_cast<double>(snap.time_stamp_sec);
-    last_ts_nsec_= static_cast<double>(snap.time_stamp_nano_sec);
-    velocity_.fill(0.0);
-    velocity_initialized_ = true;
-  }
-
-  previous_session_state_ = fri_client_->getSessionState();
-  return CallbackReturn::SUCCESS;
-}
-
-// ── on_deactivate ──────────────────────────────────────────────────────────────
-
-CallbackReturn SystemInterface::on_deactivate(const rclcpp_lifecycle::State &)
-{
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "Deactivating...");
-
-  if (!parameters_.simulate && fri_running_.load()) {
-    fri_running_.store(false, std::memory_order_relaxed);
-    // Disconnect FIRST so recvfrom() unblocks, then join the thread.
-    if (app_) { app_->disconnect(); }
-    if (fri_thread_.joinable()) { fri_thread_.join(); }
-    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI thread stopped");
-  }
-
-  // Release interface handles
-  for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-    command_if_handles_.joint_position[i] = nullptr;
-    command_if_handles_.torque[i]         = nullptr;
-    state_if_handles_.position[i]         = nullptr;
-    state_if_handles_.velocity[i]         = nullptr;
-    state_if_handles_.effort[i]           = nullptr;
-    state_if_handles_.external_torque[i]  = nullptr;
-    state_if_handles_.commanded_torque[i] = nullptr;
-    state_if_handles_.ipo_joint_position[i]= nullptr;
-  }
-  state_if_handles_.sample_time         = nullptr;
-  state_if_handles_.session_state       = nullptr;
-  state_if_handles_.connection_quality  = nullptr;
-  state_if_handles_.time_stamp_sec      = nullptr;
-  state_if_handles_.time_stamp_nano_sec = nullptr;
-
-  velocity_initialized_ = false;
-  return CallbackReturn::SUCCESS;
-}
-
-// ── on_cleanup ─────────────────────────────────────────────────────────────────
-
-CallbackReturn SystemInterface::on_cleanup(const rclcpp_lifecycle::State &)
-{
-  fri_client_.reset();
-  connection_.reset();
-  app_.reset();
-  return CallbackReturn::SUCCESS;
-}
-
-// ── friThreadFunc ──────────────────────────────────────────────────────────────
-
-void SystemInterface::friThreadFunc()
-{
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI thread started");
-  while (fri_running_.load(std::memory_order_relaxed)) {
-    if (!app_->step()) {
-      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-        "FRI step() returned false — connection may be lost");
-    }
-  }
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI thread stopped");
-}
-
-// ── read ───────────────────────────────────────────────────────────────────────
-
-hardware_interface::return_type SystemInterface::read(
-  const rclcpp::Time &, const rclcpp::Duration &)
-{
-  if (parameters_.simulate) {
-    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-      double pos = 0.0;
-      get_command(command_if_handles_.joint_position[i], pos, false);
-      std::ignore = state_if_handles_.position[i]->set_value(pos);
-      std::ignore = state_if_handles_.velocity[i]->set_value(0.0);
-      std::ignore = state_if_handles_.effort[i]->set_value(0.0);
-      std::ignore = state_if_handles_.external_torque[i]->set_value(0.0);
-      std::ignore = state_if_handles_.commanded_torque[i]->set_value(0.0);
-      std::ignore = state_if_handles_.ipo_joint_position[i]->set_value(pos);
-    }
-    return hardware_interface::return_type::OK;
-  }
-
-  const auto snap = fri_client_->getStateSnapshot();
-
-  // Detect unexpected exit from COMMANDING_ACTIVE
-  const auto current_state = static_cast<KUKA::FRI::ESessionState>(snap.session_state);
-  if (exit_commanding_active_(previous_session_state_, current_state)) {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      "Robot left COMMANDING_ACTIVE unexpectedly! Deactivate and re-activate the controller.");
-    return hardware_interface::return_type::ERROR;
-  }
-  previous_session_state_ = current_state;
-
-
-  compute_velocity_(snap);
-  state_if_handles_.push(snap, velocity_);
-
-  return hardware_interface::return_type::OK;
-}
-
-// ── write ──────────────────────────────────────────────────────────────────────
-
-hardware_interface::return_type SystemInterface::write(
-  const rclcpp::Time &, const rclcpp::Duration &)
-{
-  if (parameters_.simulate) {
-    return hardware_interface::return_type::OK;
-  }
-
-  if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) {
-    return hardware_interface::return_type::OK;
-  }
-
-  std::array<double, FRIClient::N_JOINTS> pos_cmd{}, tau_cmd{};
-  command_if_handles_.pull(pos_cmd, tau_cmd);
-
-  if (!command_guard_.check_position(pos_cmd, LOG)) {
-    return hardware_interface::return_type::ERROR;
-  }
-  if (!command_guard_.check_torque(tau_cmd, LOG)) {
-    return hardware_interface::return_type::ERROR;
-  }
-
-  fri_client_->setTargetJointPositions(pos_cmd);
-  fri_client_->setTargetJointTorques(tau_cmd);
-
-  return hardware_interface::return_type::OK;
-}
-
-// ── Protected helpers ──────────────────────────────────────────────────────────
-
-bool SystemInterface::parse_parameters_()
-{
-  const auto & info = info_;
-  try {
-    parameters_.robot_ip              = getParam(info, "robot_ip", "192.170.10.2");
-    parameters_.fri_port              = std::stoi(getParam(info, "fri_port", "30200"));
-    parameters_.simulate              = (getParam(info, "simulate", "false") == "true");
-    parameters_.command_mode          = getParam(info, "command_mode", "position");
-    parameters_.joint_position_tau    = std::stod(getParam(info, "joint_position_tau", "0.04"));
-    parameters_.open_loop             = (getParam(info, "open_loop", "true") == "true");
-    parameters_.rt_prio               = std::stoi(getParam(info, "rt_prio", "80"));
-
-    if (parameters_.fri_port < 30200 || parameters_.fri_port > 30209) {
-      RCLCPP_ERROR(rclcpp::get_logger(LOG),
-        "fri_port must be in [30200, 30209], got %d", parameters_.fri_port);
-      return false;
-    }
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Failed to parse hardware parameters: %s", e.what());
-    return false;
-  }
-  return true;
-}
-
-bool SystemInterface::exit_commanding_active_(
-  KUKA::FRI::ESessionState previous, KUKA::FRI::ESessionState current)
-{
-  return previous == KUKA::FRI::COMMANDING_ACTIVE && current != KUKA::FRI::COMMANDING_ACTIVE;
-}
-
-void SystemInterface::compute_velocity_(const IIWAStateSnapshot & snap)
-{
-  const double ts_sec  = static_cast<double>(snap.time_stamp_sec);
-  const double ts_nsec = static_cast<double>(snap.time_stamp_nano_sec);
-
-  if (!velocity_initialized_) {
-    last_pos_    = snap.measured_pos;
-    last_ts_sec_ = ts_sec;
-    last_ts_nsec_= ts_nsec;
-    velocity_.fill(0.0);
-    velocity_initialized_ = true;
-    return;
-  }
-
-  // No new FRI packet yet
-  if (ts_sec == last_ts_sec_ && ts_nsec == last_ts_nsec_) {
-    return;
-  }
-
-  // Use integer subtraction to avoid floating-point precision loss with large Unix timestamps
-  const double dt =
-    static_cast<double>(static_cast<int64_t>(snap.time_stamp_sec) -
-                        static_cast<int64_t>(static_cast<unsigned int>(last_ts_sec_))) +
-    (ts_nsec - last_ts_nsec_) * 1e-9;
-
-  // iiwa7 max joint velocity [rad/s], used to clamp impossible spikes
-  static constexpr std::array<double, FRIClient::N_JOINTS> kMaxVel =
-    {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
-  static constexpr double kVelDeadband = 1e-4;  // zero out near-stop residuals
-
-  if (dt > 0.0) {
-    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-      const double raw = (snap.measured_pos[i] - last_pos_[i]) / dt;
-      // Clamp to physical limit and apply zero deadband
-      const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]);
-      velocity_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
-    }
-  }
-
-  last_pos_    = snap.measured_pos;
-  last_ts_sec_ = ts_sec;
-  last_ts_nsec_= ts_nsec;
-}
-
-}  // namespace iiwa_controller_v2

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -8,9 +8,6 @@
     <xacro:arg name="fri_port" default="30200"/>
     <xacro:arg name="command_mode" default="position"/>
     <xacro:arg name="joint_position_tau" default="0.04"/>
-    <!-- iiwa_controller_v2: новые параметры. Убери эти строки при откате на v1 -->
-    <!-- <xacro:arg name="open_loop" default="true"/>
-    <xacro:arg name="rt_prio" default="80"/> -->
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -145,32 +142,6 @@
 
         <ros2_control name="iiwaFRIControl" type="system">
             <hardware>
-                <!-- ════════════════════════════════════════════════════════
-                     АКТИВНЫЙ ПЛАГИН: iiwa_controller_v2
-                     Чтобы вернуть v1 — раскомментируй блок v1 ниже
-                     и закомментируй блок v2.
-                     ════════════════════════════════════════════════════════ -->
-
-                <!-- ── v2 (активен) ──────────────────────────────────────── -->
-                <!-- <plugin>iiwa_controller_v2/SystemInterface</plugin> -->
-                <!-- <param name="robot_ip">$(arg robot_ip)</param> -->
-                <!-- <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> -->
-                <!-- новые параметры v2: -->
-                <!-- <param name="open_loop">$(arg open_loop)</param> -->
-                <!-- <param name="rt_prio">$(arg rt_prio)</param> -->
-
-                <!-- ── v1 (закомментировано) ─────────────────────────────
-                <plugin>iiwa_controller/IIWAHardwareInterface</plugin>
-                <param name="robot_ip">$(arg robot_ip)</param>
-                <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>
-                ─────────────────────────────────────────────────────────── -->
-            
                 <plugin>iiwa_controller/IIWAHardwareInterface</plugin>
                 <param name="robot_ip">$(arg robot_ip)</param>
                 <param name="fri_port">$(arg fri_port)</param>
@@ -185,8 +156,8 @@
                     <param name="max"> 2.97</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint1']}</param>
@@ -201,8 +172,8 @@
                     <param name="max"> 2.09</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint2']}</param>
@@ -217,8 +188,8 @@
                     <param name="max"> 2.97</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint3']}</param>
@@ -233,8 +204,8 @@
                     <param name="max"> 2.09</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint4']}</param>
@@ -249,8 +220,8 @@
                     <param name="max"> 2.97</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint5']}</param>
@@ -265,8 +236,8 @@
                     <param name="max"> 2.09</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint6']}</param>
@@ -281,8 +252,8 @@
                     <param name="max"> 3.05</param>
                 </command_interface>
                 <command_interface name="effort">
-                    <param name="min">-320</param>
+                    <param name="min">-200</param>
-                    <param name="max"> 320</param>
+                    <param name="max"> 200</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint7']}</param>

src/iiwa_description/urdf/joints.xacro

@@ -9,53 +9,60 @@
         <child link="base_link"/>
     </joint>
 
-    <!-- joint1 -->
+    <!-- joint1: ±170° hard, ±165° soft, k_velocity=10 -->
     <xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
-                        xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 -1"
+                        xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="500" 
+                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="1.71" damping="0.5"/>
+                        velocity="1.71" damping="10.0" friction="0.1"
+                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
 
 
-    <!-- joint2 -->
+    <!-- joint2: ±119° hard, ±114° soft -->
     <xacro:revolute_joint jname="joint2" parent="link1" child="link2"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="500" 
+                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="1.71" damping="0.5"/>
+                        velocity="1.71" damping="10.0" friction="0.1"
+                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
 
 
-    <!-- joint3 -->
+    <!-- joint3: ±170° hard, ±165° soft -->
     <xacro:revolute_joint jname="joint3" parent="link2" child="link3"
                         xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
-                        lower="-2.97" upper="2.97" effort="500" 
+                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="1.75" damping="0.5"/>
+                        velocity="1.75" damping="10.0" friction="0.1"
+                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
 
 
-    <!-- joint4 -->
+    <!-- joint4: ±119° hard, ±114° soft -->
     <xacro:revolute_joint jname="joint4" parent="link3" child="link4"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
-                        lower="-2.09" upper="2.09" effort="500" 
+                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="2.27" damping="0.5"/>
+                        velocity="2.27" damping="10.0" friction="0.1"
+                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
 
 
-    <!-- joint5 -->
+    <!-- joint5: ±170° hard, ±165° soft -->
     <xacro:revolute_joint jname="joint5" parent="link4" child="link5"
                         xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
-                        lower="-2.97" upper="2.97" effort="500" 
+                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="2.44" damping="0.5"/>
+                        velocity="2.44" damping="10.0" friction="0.1"
+                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
 
 
-    <!-- joint6 -->
+    <!-- joint6: ±119° hard, ±114° soft -->
     <xacro:revolute_joint jname="joint6" parent="link5" child="link6"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="500" 
+                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="3.14" damping="0.5"/>
+                        velocity="3.14" damping="10.0" friction="0.1"
+                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
 
 
-    <!-- joint7 -->
+    <!-- joint7: ±174° hard, ±169° soft -->
     <xacro:revolute_joint jname="joint7" parent="link6" child="link7"
                         xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-3.05" upper="3.05" effort="500" 
+                        lower="-3.05" upper="3.05" effort="200"
-                        velocity="3.14" damping="0.5"/>
+                        velocity="3.14" damping="10.0" friction="0.1"
+                        soft_lower="-2.9627" soft_upper="2.9627" k_velocity="10"/>
 
     <joint name="tools_joint" type="fixed">
         <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>

src/iiwa_description/urdf/macros.xacro

@@ -31,14 +31,15 @@
     </xacro:macro>
 
     <!-- Макрос для описания соединений -->
-    <xacro:macro name="revolute_joint" params="jname parent child xyz rpy axis lower upper effort velocity damping">
+    <xacro:macro name="revolute_joint" params="jname parent child xyz rpy axis lower upper effort velocity damping friction soft_lower soft_upper k_velocity">
         <joint name="${jname}" type="revolute">
             <origin xyz="${xyz}" rpy="${rpy}"/>
             <parent link="${parent}"/>
             <child link="${child}"/>
             <axis xyz="${axis}"/>
             <limit lower="${lower}" upper="${upper}" effort="${effort}" velocity="${velocity}"/>
-            <dynamics damping="${damping}"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <safety_controller soft_lower_limit="${soft_lower}" soft_upper_limit="${soft_upper}" k_velocity="${k_velocity}"/>
         </joint>
     </xacro:macro>
 

src/iiwa_description/urdf/params.xacro

@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
     
     <!-- Массы звеньев -->
-    <xacro:property name="mass_base" value="0.1"/>
+    <xacro:property name="mass_base" value="4.86"/>
     <xacro:property name="mass_link1" value="4"/>
     <xacro:property name="mass_link2" value="4"/>
     <xacro:property name="mass_link3" value="3"/>