Enhance IIWA Robot Configuration and Launch Files
- Updated iiwa7 URDF Xacro to include command and state interfaces for position and effort with defined limits for all joints. - Modified setting_loader.py to include new fields for command_mode and description in the RobotCfg dataclass and settings loading process. - Created iiwa.launch.py to manage the launch of the IIWA robot, integrating MoveIt configurations and RViz support. - Added iiwa_controllers.launch.py to set up the controller manager and spawner for the IIWA robot. - Introduced iiwa_hardware_interface_plugin.xml to define the hardware interface for the KUKA IIWA 7 robot. - Added iiwa7_fri.urdf.xacro to support FRI (Fast Robot Interface) with appropriate command and state interfaces for each joint.
This commit is contained in:
Даниил Грабарь
@@ -1,79 +1,156 @@
|
||||
// ============================================================
|
||||
// FRIClient.cpp
|
||||
//
|
||||
// Ключевые решения:
|
||||
// 1. В waitForCommand() мы «инициализируем» target_pos_ текущей
|
||||
// позицией робота, чтобы при переходе в COMMANDING_ACTIVE
|
||||
// не было рывка.
|
||||
// 2. В command() данные читаются/пишутся под мьютексом —
|
||||
// ros2_control::write() работает в другом потоке.
|
||||
// 3. Момент в режиме TORQUE суммируется с gravity compensation
|
||||
// робота (setJointPosition — feedforward, addJointTorque — delta).
|
||||
// ============================================================
|
||||
#include "iiwa_controller/FRIClient.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using namespace KUKA::FRI;
|
||||
#include <cstring> // std::memcpy
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
inline const char* to_string(KUKA::FRI::ESessionState s)
|
||||
namespace iiwa_controller
|
||||
{
|
||||
using namespace KUKA::FRI;
|
||||
switch (s)
|
||||
{
|
||||
case IDLE: return "IDLE";
|
||||
case MONITORING_WAIT: return "MONITORING_WAIT";
|
||||
case MONITORING_READY: return "MONITORING_READY";
|
||||
case COMMANDING_WAIT: return "COMMANDING_WAIT";
|
||||
case COMMANDING_ACTIVE: return "COMMANDING_ACTIVE";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
FRIClient::FRIClient() {
|
||||
targetJointPositions_.fill(0.0);
|
||||
measuredJointPositions_.fill(0.0);
|
||||
measuredTorque_.fill(0.0);
|
||||
};
|
||||
// Вспомогательная функция
|
||||
static const char* friStateName(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";
|
||||
}
|
||||
}
|
||||
|
||||
void FRIClient::monitor()
|
||||
{
|
||||
std::memcpy(measuredJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
// Конструктор
|
||||
FRIClient::FRIClient(CommandMode mode): cmd_mode_(mode) {
|
||||
target_pos_.fill(0.0);
|
||||
target_tau_.fill(0.0);
|
||||
measured_pos_.fill(0.0);
|
||||
measured_tau_.fill(0.0);
|
||||
}
|
||||
|
||||
std::memcpy(measuredTorque_.data(),
|
||||
robotState().getMeasuredTorque(),
|
||||
7 * sizeof(double));
|
||||
}
|
||||
// Вспомогательный приватный метод: обновить measured_pos_ и _tau_
|
||||
// !!!вызывать только под data_mutex_!!!
|
||||
void FRIClient::updateMeasuredState() {
|
||||
// getMeasuredJointPosition() возвращает указатель на массив double[7]
|
||||
const double* pos_ptr = robotState().getMeasuredJointPosition();
|
||||
const double* tau_ptr = robotState().getMeasuredTorque();
|
||||
std::memcpy(measured_pos_.data(), pos_ptr, N_JOINTS * sizeof(double));
|
||||
std::memcpy(measured_tau_.data(), tau_ptr, N_JOINTS * sizeof(double));
|
||||
}
|
||||
|
||||
void FRIClient::setTargetJointPositions(const std::array<double, 7> target_pos) {
|
||||
targetJointPositions_ = target_pos;
|
||||
}
|
||||
// monitor() — MONITORING_WAIT / MONITORING_READY
|
||||
// Только читаем состояние, команды не отправляем
|
||||
void FRIClient::monitor() {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
updateMeasuredState();
|
||||
}
|
||||
|
||||
std::array<double, 7> FRIClient::getMeasuredJointPositions() const {
|
||||
return measuredJointPositions_;
|
||||
}
|
||||
// waitForCommand() — COMMANDING_WAIT
|
||||
// FRI требует, чтобы в этом состоянии мы всё равно отправляли
|
||||
// команду. Отправляем «эхо» текущей позиции — робот не двигается.
|
||||
// Заодно инициализируем target_pos_ измеренной позицией, чтобы
|
||||
// при входе в COMMANDING_ACTIVE не было скачка.
|
||||
void FRIClient::waitForCommand() {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
updateMeasuredState();
|
||||
|
||||
std::array<double, 7> FRIClient::getMeasuredTorque() const {
|
||||
return measuredTorque_;
|
||||
}
|
||||
// Инициализируем целевую позицию текущей —
|
||||
// ros2_control перезапишет её в следующем цикле write()
|
||||
target_pos_ = measured_pos_;
|
||||
|
||||
void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) {
|
||||
RCLCPP_INFO_STREAM(
|
||||
// Отправляем эхо позиции
|
||||
robotCommand().setJointPosition(target_pos_.data());
|
||||
}
|
||||
|
||||
// command() — COMMANDING_ACTIVE
|
||||
// Основной цикл управления. Вызывается каждые send_period мс.
|
||||
void FRIClient::command() {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
updateMeasuredState();
|
||||
|
||||
if (cmd_mode_ == CommandMode::POSITION) {
|
||||
// Режим управления позицией
|
||||
// Просто отправляем целевую позицию, записанную из write()
|
||||
robotCommand().setJointPosition(target_pos_.data());
|
||||
}
|
||||
// CommandMode::TORQUE
|
||||
else {
|
||||
// Режим управления моментом
|
||||
// FRI требует одновременно задавать позицию
|
||||
// и дополнительный момент.
|
||||
// target_pos_ используется как feedforward (без движения),
|
||||
// target_tau_ желаемый дополнительный момент поверх
|
||||
// внутреннего регулятора KUKA.
|
||||
robotCommand().setJointPosition(target_pos_.data());
|
||||
robotCommand().setTorque(target_tau_.data());
|
||||
}
|
||||
}
|
||||
|
||||
// onStateChange() — уведомление о смене состояния FRI
|
||||
void FRIClient::onStateChange(KUKA::FRI::ESessionState oldState,
|
||||
KUKA::FRI::ESessionState newState) {
|
||||
session_state_.store(newState, std::memory_order_relaxed);
|
||||
|
||||
RCLCPP_INFO(
|
||||
rclcpp::get_logger("FRIClient"),
|
||||
"[FRI Client] FRI state: " << to_string(oldState) << " --> " << to_string(newState));
|
||||
}
|
||||
"[FRI] Состояние: %s → %s",
|
||||
friStateName(oldState),
|
||||
friStateName(newState));
|
||||
|
||||
// При потере сессии очищаем целевые команды для безопасности
|
||||
if (newState == KUKA::FRI::IDLE ||
|
||||
newState == KUKA::FRI::MONITORING_WAIT) {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
target_tau_.fill(0.0);
|
||||
// target_pos_ оставляем — при переподключении нужно знать
|
||||
// последнюю «безопасную» позицию
|
||||
RCLCPP_WARN(rclcpp::get_logger("FRIClient"),
|
||||
"[FRI] Команды сброшены (сессия неактивна)");
|
||||
}
|
||||
}
|
||||
|
||||
void FRIClient::waitForCommand()
|
||||
{
|
||||
std::memcpy(targetJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
|
||||
std::memcpy(measuredJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
// Thread-safe setters/getters (вызываются из ros2_control)
|
||||
void FRIClient::setTargetJointPositions(
|
||||
const std::array<double, N_JOINTS>& q) {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
target_pos_ = q;
|
||||
}
|
||||
|
||||
std::memcpy(measuredTorque_.data(),
|
||||
robotState().getMeasuredTorque(),
|
||||
7 * sizeof(double));
|
||||
void FRIClient::setTargetJointTorques(
|
||||
const std::array<double, N_JOINTS>& tau) {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
target_tau_ = tau;
|
||||
}
|
||||
|
||||
robotCommand().setJointPosition(targetJointPositions_.data());
|
||||
}
|
||||
std::array<double, FRIClient::N_JOINTS>
|
||||
FRIClient::getMeasuredJointPositions() const {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
return measured_pos_;
|
||||
}
|
||||
|
||||
void FRIClient::command() {
|
||||
std::memcpy(measuredJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
std::array<double, FRIClient::N_JOINTS>
|
||||
FRIClient::getMeasuredTorque() const {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
return measured_tau_;
|
||||
}
|
||||
|
||||
robotCommand().setJointPosition(targetJointPositions_.data());
|
||||
}
|
||||
bool FRIClient::isCommandingActive() const {
|
||||
return session_state_.load(std::memory_order_relaxed) ==
|
||||
KUKA::FRI::COMMANDING_ACTIVE;
|
||||
}
|
||||
|
||||
KUKA::FRI::ESessionState FRIClient::getSessionState() const {
|
||||
return session_state_.load(std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user