From ace4d02b8a3a074120fdc6405e3feab4d0d0e014 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Mon, 18 May 2026 11:10:11 +1000 Subject: [PATCH] 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. --- .../config/moveit/iiwa_controller.yaml | 6 +- src/iiwa_config/config/setting.yaml | 2 +- .../iiwa_controller/IIWAHardwareInterface.hpp | 24 +- .../src/IIWAHardwareInterface.cpp | 286 ++++++++----- src/iiwa_controller_v2/CMakeLists.txt | 93 ---- src/iiwa_controller_v2/external | 1 - .../iiwa_hardware_interface_plugin.xml | 13 - .../iiwa_controller_v2/command_guard.hpp | 95 ---- .../include/iiwa_controller_v2/fri_client.hpp | 74 ---- .../iiwa_controller_v2/system_interface.hpp | 216 ---------- .../system_interface_type_values.hpp | 21 - src/iiwa_controller_v2/package.xml | 29 -- src/iiwa_controller_v2/src/fri_client.cpp | 154 ------- .../src/system_interface.cpp | 404 ------------------ src/iiwa_description/urdf/iiwa7.urdf.xacro | 57 +-- src/iiwa_description/urdf/joints.xacro | 53 ++- src/iiwa_description/urdf/macros.xacro | 5 +- src/iiwa_description/urdf/params.xacro | 2 +- 18 files changed, 240 insertions(+), 1295 deletions(-) delete mode 100644 src/iiwa_controller_v2/CMakeLists.txt delete mode 120000 src/iiwa_controller_v2/external delete mode 100644 src/iiwa_controller_v2/iiwa_hardware_interface_plugin.xml delete mode 100644 src/iiwa_controller_v2/include/iiwa_controller_v2/command_guard.hpp delete mode 100644 src/iiwa_controller_v2/include/iiwa_controller_v2/fri_client.hpp delete mode 100644 src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp delete mode 100644 src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface_type_values.hpp delete mode 100644 src/iiwa_controller_v2/package.xml delete mode 100644 src/iiwa_controller_v2/src/fri_client.cpp delete mode 100644 src/iiwa_controller_v2/src/system_interface.cpp diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index 21903c8..d0bb476 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/src/iiwa_config/config/moveit/iiwa_controller.yaml @@ -18,8 +18,10 @@ joint_state_broadcaster: - velocity - effort - external_torque - - commanded_torque - - ipo_joint_position + # commanded_torque и ipo_joint_position доступны только при активном v2 + # при переключении на v2 — раскомментировать: + # - commanded_torque + # - ipo_joint_position iiwa_arm_controller: ros__parameters: diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 4c210e1..7ab15ec 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -4,7 +4,7 @@ robot: port: 30200 command_mode: "position" # torque, position fri_cycle_ms: 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 diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 0e80ca5..4a6a05d 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -34,13 +35,16 @@ public: CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override; - // external_torque не объявлен в URDF, поэтому регистрируем его здесь как unlisted. - // Стандартные интерфейсы (position, velocity, effort) базовый класс берёт из URDF сам. + // external_torque не объявлен в URDF — регистрируем вручную как unlisted. std::vector 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 fri_client_; std::unique_ptr connection_; std::unique_ptr 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 fri_running_{false}; void friThreadFunc(); @@ -81,13 +83,17 @@ private: std::array h_cmd_pos_; std::array h_cmd_eff_; - // Предыдущие позиции и скорость (обновляются только при свежем FRI-пакете) + // Вычисление скорости конечными разностями std::array prev_pos_{}; - std::array vel_filtered_{}; + std::array 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; diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 97d76d6..5245f0e 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #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"); - fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); - simulate_ = (getParam(info, "simulate", "false") == "true"); - cmd_mode_str_ = getParam(info, "command_mode", "position"); + robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); + fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); + simulate_ = (getParam(info, "simulate", "false") == "true"); + cmd_mode_str_ = getParam(info, "command_mode", "position"); joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04")); - RCLCPP_INFO( - rclcpp::get_logger("IIWAHardwareInterface"), + RCLCPP_INFO(rclcpp::get_logger(LOG), "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::get_logger("IIWAHardwareInterface"), + RCLCPP_FATAL(rclcpp::get_logger(LOG), "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. -// Стандартные интерфейсы (position, velocity, effort) базовый класс берёт из URDF сам. +// ── export_unlisted_state_interface_descriptions ──────────────────────────────── + std::vector 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.data_type = "double"; + if_info.name = "external_torque"; + 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(parseCommandMode(cmd_mode_str_), joint_position_tau_); + // 100 мс таймаут recvfrom — поток корректно завершится после disconnect(). + connection_ = std::make_unique(100); + app_ = std::make_unique(*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_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_ext_[i] = get_state_interface_handle(jn + "/external_torque"); - + 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_eff_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT); + 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]) { - RCLCPP_FATAL( - rclcpp::get_logger("IIWAHardwareInterface"), + if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || + !h_cmd_pos_[i] || !h_cmd_eff_[i]) + { + RCLCPP_FATAL(rclcpp::get_logger(LOG), "Не удалось получить хэндл интерфейса для сустава '%s'. " "Проверьте объявление / в 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(parseCommandMode(cmd_mode_str_), joint_position_tau_); - // 100 мс таймаут: если закрытие сокета не разблокирует recvfrom() мгновенно, - // поток всё равно выйдет через одну итерацию. - connection_ = std::make_unique(100); - app_ = std::make_unique(*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; - int elapsed = 0; - while (fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs) { + 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, + "Ожидание FRI-сессии... (%d мс)", elapsed); std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs)); - elapsed += kPollMs; } if (fri_client_->getSessionState() == KUKA::FRI::IDLE) { - RCLCPP_ERROR( - rclcpp::get_logger("IIWAHardwareInterface"), + RCLCPP_ERROR(rclcpp::get_logger(LOG), "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; - vel_filtered_.fill(0.0); + 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; } + previous_session_state_ = fri_client_->getSessionState(); return CallbackReturn::SUCCESS; } -// Отдельный поток для FRI: step() блокируется в recvfrom() пока не придёт UDP-пакет, -// потом вызывает нужный callback и отправляет ответ роботу. -// 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 поток завершён"); -} +// ── on_deactivate ────────────────────────────────────────────────────────────── +// Останавливает FRI-поток. FRI-объекты остаются — их очищает on_cleanup(). 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-потоке. - // Только после этого ждём завершения потока. Если сделать наоборот, - // join() зависнет навсегда потому что поток заблокирован в recvfrom(). - if (app_) { - app_->disconnect(); - } - if (fri_thread_.joinable()) { - fri_thread_.join(); - } - RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI отключён"); + // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке. + // Только потом join(), иначе он зависнет навсегда. + if (app_) { app_->disconnect(); } + if (fri_thread_.joinable()) { fri_thread_.join(); } + RCLCPP_INFO(rclcpp::get_logger(LOG), "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-потока через мьютекс. -// Период JTC остаётся стабильным: при update_rate=400 и fri_cycle_ms=5 соотношение 2:1, -// идентичное рабочей конфигурации fri_cycle_ms=10 + update_rate=200. +// ── on_cleanup ───────────────────────────────────────────────────────────────── +// Освобождает FRI-объекты. Вызывается после on_deactivate(). + +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( + static_cast(snap.time_stamp_sec) - + static_cast(last_ts_sec_)) + + (static_cast(snap.time_stamp_nano_sec) - + static_cast(last_ts_nsec_)) * 1e-9; + + static constexpr std::array 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-пакете. - // measured_pos в Commanding = filtered_pos_ (open-loop), поэтому скорость — это - // производная сглаженной команды: гладкий сигнал без шума датчика и без лага. - const bool fresh = (snap.time_stamp_sec != last_ts_sec_ || - snap.time_stamp_nano_sec != last_ts_nsec_); - if (fresh) { - const double dt = - (static_cast(snap.time_stamp_sec) - static_cast(last_ts_sec_)) + - (static_cast(snap.time_stamp_nano_sec) - static_cast(last_ts_nsec_)) * 1e-9; - - // iiwa7 physical velocity limits [rad/s], used to clamp impossible spikes - static constexpr std::array 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; + // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE. + const auto current_state = fri_client_->getSessionState(); + if (previous_session_state_ == KUKA::FRI::COMMANDING_ACTIVE && + current_state != KUKA::FRI::COMMANDING_ACTIVE) + { + RCLCPP_ERROR(rclcpp::get_logger(LOG), + "Робот вышел из COMMANDING_ACTIVE! Деактивируйте и повторно активируйте контроллер."); + return hardware_interface::return_type::ERROR; } + 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 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 diff --git a/src/iiwa_controller_v2/CMakeLists.txt b/src/iiwa_controller_v2/CMakeLists.txt deleted file mode 100644 index 4c9aef8..0000000 --- a/src/iiwa_controller_v2/CMakeLists.txt +++ /dev/null @@ -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 - $ - $ -) - -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() diff --git a/src/iiwa_controller_v2/external b/src/iiwa_controller_v2/external deleted file mode 120000 index d642ea3..0000000 --- a/src/iiwa_controller_v2/external +++ /dev/null @@ -1 +0,0 @@ -/home/daniel/dev/ros2_iiwa7/src/iiwa_controller/external \ No newline at end of file diff --git a/src/iiwa_controller_v2/iiwa_hardware_interface_plugin.xml b/src/iiwa_controller_v2/iiwa_hardware_interface_plugin.xml deleted file mode 100644 index 8972244..0000000 --- a/src/iiwa_controller_v2/iiwa_hardware_interface_plugin.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - 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. - - - diff --git a/src/iiwa_controller_v2/include/iiwa_controller_v2/command_guard.hpp b/src/iiwa_controller_v2/include/iiwa_controller_v2/command_guard.hpp deleted file mode 100644 index 6c29d47..0000000 --- a/src/iiwa_controller_v2/include/iiwa_controller_v2/command_guard.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#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 and -// in the robot description. -struct CommandGuard -{ - struct JointLimits - { - std::string name; - double min_position{-std::numeric_limits::infinity()}; - double max_position{std::numeric_limits::infinity()}; - double max_torque{std::numeric_limits::infinity()}; - }; - - std::array 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 & 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 & 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 diff --git a/src/iiwa_controller_v2/include/iiwa_controller_v2/fri_client.hpp b/src/iiwa_controller_v2/include/iiwa_controller_v2/fri_client.hpp deleted file mode 100644 index b6d212f..0000000 --- a/src/iiwa_controller_v2/include/iiwa_controller_v2/fri_client.hpp +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include -#include -#include - -#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 measured_pos{}; // open-loop: = filtered_pos_ in COMMANDING_ACTIVE - std::array ipo_pos{}; // interpolator position (valid only in COMMANDING) - std::array measured_tau{}; // measured joint torques [Nm] - std::array commanded_tau{}; // torques sent to robot in previous cycle [Nm] - std::array 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 & q); - void setTargetJointTorques(const std::array & tau); - IIWAStateSnapshot getStateSnapshot() const; - KUKA::FRI::ESessionState getSessionState() const; - -private: - CommandMode cmd_mode_; - double joint_position_tau_; - std::atomic session_state_{KUKA::FRI::IDLE}; - - mutable std::mutex data_mutex_; - std::array target_pos_{}; - std::array target_tau_{}; - // Filtered position actually sent to the robot; initialised from IPO in waitForCommand(). - std::array 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 diff --git a/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp b/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp deleted file mode 100644 index eca7b01..0000000 --- a/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp +++ /dev/null @@ -1,216 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#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 - 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::quiet_NaN()); - std::ignore = torque[i]->set_value(std::numeric_limits::quiet_NaN()); - } - } - - void pull(std::array & pos_cmd, - std::array & 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::quiet_NaN()); - tau_cmd[i] = torque[i]->get_optional().value_or( - std::numeric_limits::quiet_NaN()); - } - } - }; - - // ── State interface handles ───────────────────────────────────────────────── - struct StateInterfaceHandles - { - // Standard per-joint - std::array - position, velocity, effort; - // Extended per-joint (registered as unlisted) - std::array - 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::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 & 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(snap.session_state)); - std::ignore = connection_quality->set_value(static_cast(snap.connection_quality)); - std::ignore = time_stamp_sec->set_value(static_cast(snap.time_stamp_sec)); - std::ignore = time_stamp_nano_sec->set_value(static_cast(snap.time_stamp_nano_sec)); - } - }; - -public: - SystemInterface() = default; - - // ── Lifecycle ─────────────────────────────────────────────────────────────── - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareComponentInterfaceParams & params) override; - - std::vector - export_unlisted_state_interface_descriptions() override; - - hardware_interface::return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & 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 fri_client_; - std::unique_ptr connection_; - std::unique_ptr app_; - - std::thread fri_thread_; - std::atomic fri_running_{false}; - rclcpp::Clock throttle_clock_{RCL_STEADY_TIME}; - - KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE}; - - std::array velocity_{}; - std::array 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 diff --git a/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface_type_values.hpp b/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface_type_values.hpp deleted file mode 100644 index c5857cd..0000000 --- a/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface_type_values.hpp +++ /dev/null @@ -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 diff --git a/src/iiwa_controller_v2/package.xml b/src/iiwa_controller_v2/package.xml deleted file mode 100644 index 7ab285a..0000000 --- a/src/iiwa_controller_v2/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - iiwa_controller_v2 - 0.2.0 - - 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. - - daniel - Apache-2.0 - - ament_cmake - - controller_interface - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/iiwa_controller_v2/src/fri_client.cpp b/src/iiwa_controller_v2/src/fri_client.cpp deleted file mode 100644 index 2678c1a..0000000 --- a/src/iiwa_controller_v2/src/fri_client.cpp +++ /dev/null @@ -1,154 +0,0 @@ -#include "iiwa_controller_v2/fri_client.hpp" - -#include -#include - -#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 lk(data_mutex_); - captureMonitoringData(); -} - -void FRIClient::waitForCommand() -{ - std::lock_guard 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 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 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 & q) -{ - for (const auto v : q) { - if (!std::isfinite(v)) { return; } - } - std::lock_guard lk(data_mutex_); - target_pos_ = q; -} - -void FRIClient::setTargetJointTorques(const std::array & tau) -{ - for (const auto v : tau) { - if (!std::isfinite(v)) { return; } - } - std::lock_guard lk(data_mutex_); - target_tau_ = tau; -} - -IIWAStateSnapshot FRIClient::getStateSnapshot() const -{ - std::lock_guard lk(data_mutex_); - return snapshot_; -} - -KUKA::FRI::ESessionState FRIClient::getSessionState() const -{ - return session_state_.load(std::memory_order_relaxed); -} - -} // namespace iiwa_controller_v2 diff --git a/src/iiwa_controller_v2/src/system_interface.cpp b/src/iiwa_controller_v2/src/system_interface.cpp deleted file mode 100644 index daf9716..0000000 --- a/src/iiwa_controller_v2/src/system_interface.cpp +++ /dev/null @@ -1,404 +0,0 @@ -#include "iiwa_controller_v2/system_interface.hpp" - -#include -#include -#include -#include - -#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 -SystemInterface::export_unlisted_state_interface_descriptions() -{ - std::vector descs; - - // Per-joint extended interfaces - const std::array 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 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 & /*start*/, const std::vector & /*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(cmd_mode, parameters_.joint_position_tau); - // 100 ms receive timeout: ensures friThreadFunc can exit cleanly after disconnect(). - connection_ = std::make_unique(100); - app_ = std::make_unique(*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(snap.time_stamp_sec); - last_ts_nsec_= static_cast(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(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 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(snap.time_stamp_sec); - const double ts_nsec = static_cast(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(static_cast(snap.time_stamp_sec) - - static_cast(static_cast(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 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 diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 7edf3aa..0a58b53 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -8,9 +8,6 @@ - - @@ -145,32 +142,6 @@ - - - - - - - - - - - - - - - iiwa_controller/IIWAHardwareInterface $(arg robot_ip) $(arg fri_port) @@ -185,8 +156,8 @@ 2.97 - -320 - 320 + -200 + 200 ${initial_positions['joint1']} @@ -201,8 +172,8 @@ 2.09 - -320 - 320 + -200 + 200 ${initial_positions['joint2']} @@ -217,8 +188,8 @@ 2.97 - -320 - 320 + -200 + 200 ${initial_positions['joint3']} @@ -233,8 +204,8 @@ 2.09 - -320 - 320 + -200 + 200 ${initial_positions['joint4']} @@ -249,8 +220,8 @@ 2.97 - -320 - 320 + -200 + 200 ${initial_positions['joint5']} @@ -265,8 +236,8 @@ 2.09 - -320 - 320 + -200 + 200 ${initial_positions['joint6']} @@ -281,8 +252,8 @@ 3.05 - -320 - 320 + -200 + 200 ${initial_positions['joint7']} diff --git a/src/iiwa_description/urdf/joints.xacro b/src/iiwa_description/urdf/joints.xacro index 978b2f1..0359163 100644 --- a/src/iiwa_description/urdf/joints.xacro +++ b/src/iiwa_description/urdf/joints.xacro @@ -9,53 +9,60 @@ - + + xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 1" + lower="-2.97" upper="2.97" effort="200" + velocity="1.71" damping="10.0" friction="0.1" + soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/> - + + lower="-2.09" upper="2.09" effort="200" + velocity="1.71" damping="10.0" friction="0.1" + soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/> - + + lower="-2.97" upper="2.97" effort="200" + velocity="1.75" damping="10.0" friction="0.1" + soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/> - + + lower="-2.09" upper="2.09" effort="200" + velocity="2.27" damping="10.0" friction="0.1" + soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/> - + + lower="-2.97" upper="2.97" effort="200" + velocity="2.44" damping="10.0" friction="0.1" + soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/> - + + lower="-2.09" upper="2.09" effort="200" + velocity="3.14" damping="10.0" friction="0.1" + soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/> - + + lower="-3.05" upper="3.05" effort="200" + velocity="3.14" damping="10.0" friction="0.1" + soft_lower="-2.9627" soft_upper="2.9627" k_velocity="10"/> @@ -63,4 +70,4 @@ - \ No newline at end of file + diff --git a/src/iiwa_description/urdf/macros.xacro b/src/iiwa_description/urdf/macros.xacro index 6b7cbb6..d9ab979 100644 --- a/src/iiwa_description/urdf/macros.xacro +++ b/src/iiwa_description/urdf/macros.xacro @@ -31,14 +31,15 @@ - + - + + diff --git a/src/iiwa_description/urdf/params.xacro b/src/iiwa_description/urdf/params.xacro index 483b3ce..15449ab 100644 --- a/src/iiwa_description/urdf/params.xacro +++ b/src/iiwa_description/urdf/params.xacro @@ -2,7 +2,7 @@ - +