Add iiwa_controller_v2: Implement hardware interface for KUKA iiwa7 via FRI
- Introduced iiwa_hardware_interface_plugin.xml to define the hardware interface. - Created command_guard.hpp to enforce joint limits for position and torque commands. - Developed fri_client.hpp and fri_client.cpp to manage FRI communication and state snapshots. - Implemented system_interface.hpp and system_interface.cpp for lifecycle management and command handling. - Added system_interface_type_values.hpp for extended state interface names. - Defined package.xml for ROS2 integration with necessary dependencies.
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,93 @@
|
||||
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()
|
||||
Symlink
+1
@@ -0,0 +1 @@
|
||||
/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_controller/external
|
||||
@@ -0,0 +1,13 @@
|
||||
<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>
|
||||
@@ -0,0 +1,95 @@
|
||||
#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
|
||||
@@ -0,0 +1,74 @@
|
||||
#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
|
||||
@@ -0,0 +1,227 @@
|
||||
#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};
|
||||
bool external_torque_safety_check{true};
|
||||
double external_torque_limit{2.0}; // [Nm] per joint, safety threshold
|
||||
};
|
||||
|
||||
// ── 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 ───────────────────────────────────────────────────────────────
|
||||
controller_interface::CallbackReturn on_init(
|
||||
const hardware_interface::HardwareComponentInterfaceParams & params) override;
|
||||
|
||||
// Per-joint: external_torque, commanded_torque, ipo_joint_position
|
||||
// Auxiliary: sample_time, session_state, connection_quality, time_stamp_*
|
||||
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;
|
||||
|
||||
// on_configure: opens the UDP socket (FRI does not need the robot yet)
|
||||
controller_interface::CallbackReturn on_configure(
|
||||
const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
// on_activate: starts the FRI thread, waits for COMMANDING_WAIT
|
||||
controller_interface::CallbackReturn on_activate(
|
||||
const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
// on_deactivate: stops the FRI thread
|
||||
controller_interface::CallbackReturn on_deactivate(
|
||||
const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
// on_cleanup: closes the UDP socket
|
||||
controller_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);
|
||||
|
||||
// Safety check: abort if any external torque exceeds the configured limit.
|
||||
bool external_torque_safe_(const IIWAStateSnapshot & snap) const;
|
||||
|
||||
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
|
||||
@@ -0,0 +1,21 @@
|
||||
#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
|
||||
@@ -0,0 +1,29 @@
|
||||
<?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>
|
||||
@@ -0,0 +1,154 @@
|
||||
#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
|
||||
@@ -0,0 +1,410 @@
|
||||
#include "iiwa_controller_v2/system_interface.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#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;
|
||||
|
||||
// External torque safety check
|
||||
if (parameters_.external_torque_safety_check && !external_torque_safe_(snap)) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(LOG),
|
||||
"External torque exceeded safety limit (%.1f Nm). Stopping.", parameters_.external_torque_limit);
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
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"));
|
||||
parameters_.external_torque_safety_check =
|
||||
(getParam(info, "external_torque_safety_check", "true") == "true");
|
||||
parameters_.external_torque_limit =
|
||||
std::stod(getParam(info, "external_torque_limit", "2.0"));
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
bool SystemInterface::external_torque_safe_(const IIWAStateSnapshot & snap) const
|
||||
{
|
||||
for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
|
||||
if (std::abs(snap.external_tau[i]) > parameters_.external_torque_limit) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
const double dt = (ts_sec + ts_nsec * 1e-9) - (last_ts_sec_ + last_ts_nsec_ * 1e-9);
|
||||
if (dt > 0.0) {
|
||||
for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
|
||||
velocity_[i] = (snap.measured_pos[i] - last_pos_[i]) / dt;
|
||||
}
|
||||
}
|
||||
|
||||
last_pos_ = snap.measured_pos;
|
||||
last_ts_sec_ = ts_sec;
|
||||
last_ts_nsec_= ts_nsec;
|
||||
}
|
||||
|
||||
} // namespace iiwa_controller_v2
|
||||
Reference in New Issue
Block a user