grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Add joint_position_tau parameter for smoother joint control and update related configurations

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-14 11:40:04 +10:00
parent 302eb3d8d7
commit c3d1de7b01
12 changed files with 74 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_controller/CMakeLists.txt
+14 -2
View File
@@ -8,10 +8,13 @@ set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(ament_cmake REQUIRED)
find_package(controller_interface REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(std_msgs REQUIRED)
# FRI SDK
set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI)
@@ -45,10 +48,11 @@ target_compile_options(fri_client_sdk PRIVATE -fpermissive -w)
set_target_properties(fri_client_sdk PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_link_libraries(fri_client_sdk PUBLIC pthread)
# Плагин hardware interface
# Плагины hardware interface + controller
add_library(${PROJECT_NAME} SHARED
src/FRIClient.cpp
src/IIWAHardwareInterface.cpp
src/IIWAJointPositionController.cpp
)
target_include_directories(${PROJECT_NAME} PUBLIC
@@ -58,10 +62,13 @@ target_include_directories(${PROJECT_NAME} PUBLIC
target_link_libraries(${PROJECT_NAME} PRIVATE
fri_client_sdk
controller_interface::controller_interface
hardware_interface::hardware_interface
pluginlib::pluginlib
rclcpp::rclcpp
rclcpp_lifecycle::rclcpp_lifecycle
realtime_tools::realtime_tools
${std_msgs_TARGETS}
)
pluginlib_export_plugin_description_file(
@@ -69,6 +76,11 @@ pluginlib_export_plugin_description_file(
iiwa_hardware_interface_plugin.xml
)
pluginlib_export_plugin_description_file(
controller_interface
iiwa_controller_plugin.xml
)
# Установка — только библиотека и заголовки, без config/launch/urdf
install(TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
@@ -85,6 +97,6 @@ ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_targets(export_${PROJECT_NAME})
ament_export_dependencies(
hardware_interface pluginlib rclcpp rclcpp_lifecycle)
controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs)
ament_package()
src/iiwa_controller/include/iiwa_controller/FRIClient.h
+8 -1
View File
@@ -35,7 +35,10 @@ class FRIClient : public KUKA::FRI::LBRClient
public:
static constexpr size_t N_JOINTS = 7;
explicit FRIClient(CommandMode mode = CommandMode::POSITION);
// joint_position_tau — постоянная времени экспоненциального фильтра позиций [с].
// Аналог joint_position_tau из lbr_fri_ros2_stack (по умолчанию 0.04 с = 40 мс).
// Сглаживает скачки команд перед отправкой роботу → убирает писк и стук суставов.
explicit FRIClient(CommandMode mode = CommandMode::POSITION, double joint_position_tau = 0.04);
~FRIClient() override = default;
// Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
@@ -54,11 +57,15 @@ public:
private:
CommandMode cmd_mode_;
double joint_position_tau_;
std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
mutable std::mutex data_mutex_;
std::array<double, N_JOINTS> target_pos_{};
std::array<double, N_JOINTS> target_tau_{};
// Сглаженная позиция, которую реально отправляем роботу.
// Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
std::array<double, N_JOINTS> filtered_pos_{};
IIWAStateSnapshot snapshot_{};
// Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна)
src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp
+1
View File
@@ -56,6 +56,7 @@ private:
int fri_port_{30200};
bool simulate_{false};
std::string cmd_mode_str_{"position"};
double joint_position_tau_{0.04}; // постоянная времени фильтра позиций [с]
// Объекты FRI SDK
std::unique_ptr<FRIClient> fri_client_;
src/iiwa_controller/package.xml
+3
View File
@@ -9,10 +9,13 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>realtime_tools</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
src/iiwa_controller/src/FRIClient.cpp
+17 -4
View File
@@ -20,10 +20,12 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
}
}
FRIClient::FRIClient(CommandMode mode) : cmd_mode_(mode)
FRIClient::FRIClient(CommandMode mode, double joint_position_tau)
: cmd_mode_(mode), joint_position_tau_(joint_position_tau)
{
target_pos_.fill(0.0);
target_tau_.fill(0.0);
filtered_pos_.fill(0.0);
}
// Вызывается только в Monitor-состояниях.
@@ -73,10 +75,12 @@ void FRIClient::waitForCommand()
std::lock_guard<std::mutex> lock(data_mutex_);
captureCommandingData();
// Инициализируем цель IPO-позицией, иначе до первого write() будем посылать нули.
// Инициализируем цель и фильтр IPO-позицией.
// Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE.
std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
robotCommand().setJointPosition(target_pos_.data());
robotCommand().setJointPosition(filtered_pos_.data());
if (cmd_mode_ == CommandMode::TORQUE) {
// Пока контроллер не синхронизирован, момент держим на нуле
@@ -91,7 +95,16 @@ void FRIClient::command()
std::lock_guard<std::mutex> lock(data_mutex_);
captureCommandingData();
robotCommand().setJointPosition(target_pos_.data());
// Экспоненциальный фильтр первого порядка: alpha = dt / (tau + dt).
// Сглаживает скачки команд от контроллера — устраняет писк и стук суставов.
// При tau=0.04 с и dt=0.005 с: alpha≈0.11 (11% новой команды за цикл).
const double dt = snapshot_.sample_time;
const double alpha = dt / (joint_position_tau_ + dt);
for (size_t i = 0; i < N_JOINTS; ++i) {
filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
}
robotCommand().setJointPosition(filtered_pos_.data());
if (cmd_mode_ == CommandMode::TORQUE) {
// В режиме TORQUE позиция работает как feedforward удержания, момент добавляется поверх.
src/iiwa_controller/src/IIWAHardwareInterface.cpp
+5 -3
View File
@@ -41,13 +41,15 @@ CallbackReturn IIWAHardwareInterface::on_init(
fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
simulate_ = (getParam(info, "simulate", "false") == "true");
cmd_mode_str_ = getParam(info, "command_mode", "position");
joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04"));
RCLCPP_INFO(
rclcpp::get_logger("IIWAHardwareInterface"),
"on_init: ip=%s port=%d simulate=%s mode=%s",
"on_init: ip=%s port=%d simulate=%s mode=%s tau=%.3f",
robot_ip_.c_str(), fri_port_,
simulate_ ? "true" : "false",
cmd_mode_str_.c_str());
cmd_mode_str_.c_str(),
joint_position_tau_);
if (info.joints.size() != N_JOINTS) {
RCLCPP_FATAL(
@@ -114,7 +116,7 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
return CallbackReturn::SUCCESS;
}
fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_));
fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_), joint_position_tau_);
// 100 мс таймаут: если закрытие сокета не разблокирует recvfrom() мгновенно,
// поток всё равно выйдет через одну итерацию.
connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);