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

refactor: remove command_mode from configuration and related files

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-25 12:30:59 +10:00
parent e60402c8f2
commit b560fcf229
13 changed files with 35 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_controller/iiwa_hardware_interface_plugin.xml
+1 -1
View File
@@ -5,7 +5,7 @@
base_class_type="hardware_interface::SystemInterface">
<description>
ROS2 hardware interface для KUKA iiwa 7 через FRI (Fast Robot Interface).
Поддерживает режимы управления: position, torque.
Поддерживает управление в режиме position через FRI.
</description>
</class>
</library>
src/iiwa_controller/include/iiwa_controller/FRIClient.h
+1 -9
View File
@@ -11,11 +11,6 @@
namespace iiwa_controller
{
enum class CommandMode
{
POSITION,
TORQUE
};
// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
// и так же за один lock читается из read() в потоке управления.
@@ -40,7 +35,7 @@ public:
// 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);
explicit FRIClient(double joint_position_tau = 0.04);
~FRIClient() override = default;
// Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
@@ -52,19 +47,16 @@ public:
// Потокобезопасное API для ros2_control, вызывается из read() и write()
void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
void setTargetJointTorques(const std::array<double, N_JOINTS> & tau);
IIWAStateSnapshot getStateSnapshot() const;
bool isCommandingActive() 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_{};
// Сглаженная позиция, которую реально отправляем роботу.
// Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
std::array<double, N_JOINTS> filtered_pos_{};
src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp
-4
View File
@@ -59,7 +59,6 @@ private:
std::string robot_ip_;
int fri_port_{30200};
bool simulate_{false};
std::string cmd_mode_str_{"position"};
double joint_position_tau_{0.04};
// EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей.
// joint_velocity_tau = 0 отключает фильтр (raw finite difference).
@@ -82,9 +81,7 @@ private:
std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_eff_;
std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ext_;
// Хэндлы командных интерфейсов
std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
// Вычисление скорости: конечные разности + EMA-фильтр
std::array<double, N_JOINTS> prev_pos_{};
@@ -100,7 +97,6 @@ private:
rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};
CommandMode parseCommandMode(const std::string & mode_str) const;
};
} // namespace iiwa_controller
src/iiwa_controller/src/FRIClient.cpp
+2 -29
View File
@@ -20,11 +20,10 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
}
}
FRIClient::FRIClient(CommandMode mode, double joint_position_tau)
: cmd_mode_(mode), joint_position_tau_(joint_position_tau)
FRIClient::FRIClient(double joint_position_tau)
: joint_position_tau_(joint_position_tau)
{
target_pos_.fill(0.0);
target_tau_.fill(0.0);
filtered_pos_.fill(0.0);
}
@@ -86,12 +85,6 @@ void FRIClient::waitForCommand()
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());
}
}
// Вызывается в COMMANDING_ACTIVE, основной цикл управления
@@ -110,10 +103,6 @@ void FRIClient::command()
robotCommand().setJointPosition(filtered_pos_.data());
if (cmd_mode_ == CommandMode::TORQUE) {
robotCommand().setTorque(target_tau_.data());
}
// Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
captureCommandingData();
}
@@ -131,11 +120,6 @@ void FRIClient::onStateChange(
newState == KUKA::FRI::MONITORING_WAIT ||
newState == KUKA::FRI::MONITORING_READY)
{
std::lock_guard<std::mutex> lock(data_mutex_);
target_tau_.fill(0.0);
RCLCPP_WARN(
rclcpp::get_logger("FRIClient"),
"FRI сессия неактивна, моменты обнулены");
}
}
@@ -152,17 +136,6 @@ void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
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> lock(data_mutex_);
target_tau_ = tau;
}
IIWAStateSnapshot FRIClient::getStateSnapshot() const
{
std::lock_guard<std::mutex> lock(data_mutex_);
src/iiwa_controller/src/IIWAHardwareInterface.cpp
+5 -18
View File
@@ -46,15 +46,13 @@ CallbackReturn IIWAHardwareInterface::on_init(
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"));
joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
RCLCPP_INFO(rclcpp::get_logger(LOG),
"on_init: ip=%s port=%d simulate=%s mode=%s pos_tau=%.3f vel_tau=%.3f",
"on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f",
robot_ip_.c_str(), fri_port_,
simulate_ ? "true" : "false",
cmd_mode_str_.c_str(),
joint_position_tau_,
joint_velocity_tau_);
@@ -99,7 +97,7 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State
return CallbackReturn::SUCCESS;
}
fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_), joint_position_tau_);
fri_client_ = std::make_unique<FRIClient>(joint_position_tau_);
// 100 мс таймаут recvfrom — поток корректно завершится после disconnect().
connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
@@ -132,10 +130,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
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])
if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i])
{
RCLCPP_FATAL(rclcpp::get_logger(LOG),
"Не удалось получить хэндл интерфейса для сустава '%s'. "
@@ -199,7 +195,7 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
for (size_t i = 0; i < N_JOINTS; ++i) {
h_pos_[i] = h_vel_[i] = h_eff_[i] = h_ext_[i] = nullptr;
h_cmd_pos_[i] = h_cmd_eff_[i] = nullptr;
h_cmd_pos_[i] = nullptr;
}
velocity_initialized_ = false;
@@ -342,23 +338,14 @@ hardware_interface::return_type IIWAHardwareInterface::write(
return hardware_interface::return_type::OK;
}
std::array<double, N_JOINTS> pos_cmd{}, tau_cmd{};
std::array<double, N_JOINTS> pos_cmd{};
for (size_t i = 0; i < N_JOINTS; ++i) {
get_command(h_cmd_pos_[i], pos_cmd[i], false);
get_command(h_cmd_eff_[i], tau_cmd[i], false);
}
fri_client_->setTargetJointPositions(pos_cmd);
fri_client_->setTargetJointTorques(tau_cmd);
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