diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 463c6a1..9db4571 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -172,6 +172,7 @@ def _runtime_setup(context, *args, **kwargs): "controller_timer": str(settings.digital_twin.webots.controller_timer), "fri_cycle_ms": str(settings.robot.fri_cycle_ms), "joint_position_tau": str(settings.robot.joint_position_tau), + "controller": settings.robot.active_controller, } controllers_launch = IncludeLaunchDescription( diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 8281b0a..93bb68f 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -20,10 +20,9 @@ def _setup_controllers(context, *args, **kwargs): controller_path = LaunchConfiguration("controller_path").perform(context) simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes") command_mode = LaunchConfiguration("command_mode").perform(context) + controller = LaunchConfiguration("controller").perform(context) # "jtc" | "forward" fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context)) joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context) - # JTC rate = FRI rate (1:1): каждый цикл JTC читает свежее состояние от FRI. - # При 2:1 нечётные JTC-циклы видят устаревший снапшот → чередование скорости 0/v → писк. update_rate = 1000 // fri_cycle_ms xacro_args = {"initial_positions_file": initial_positions_file} @@ -106,12 +105,21 @@ def _setup_controllers(context, *args, **kwargs): ], ) - jtc_args = ["iiwa_arm_controller", "--controller-manager", "/controller_manager"] - torque_args = ["iiwa_arm_torque_controller", "--controller-manager", "/controller_manager"] + cm = ["--controller-manager", "/controller_manager"] - if command_mode == "torque": + # JTC: активен если controller=jtc (и command_mode=position), иначе --inactive + jtc_args = ["iiwa_arm_controller"] + cm + if command_mode == "torque" or controller == "forward": jtc_args += ["--inactive"] - else: + + # ForwardCommandController: активен если controller=forward, иначе --inactive + forward_args = ["forward_position_controller"] + cm + if controller != "forward": + forward_args += ["--inactive"] + + # TorqueController: активен если command_mode=torque и controller=jtc + torque_args = ["iiwa_arm_torque_controller"] + cm + if not (command_mode == "torque" and controller == "jtc"): torque_args += ["--inactive"] jtc = Node( @@ -121,6 +129,13 @@ def _setup_controllers(context, *args, **kwargs): arguments=jtc_args, ) + forward_controller = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=forward_args, + ) + torque_controller = Node( package="controller_manager", executable="spawner", @@ -131,7 +146,7 @@ def _setup_controllers(context, *args, **kwargs): jtc_after_jsb = RegisterEventHandler( OnProcessExit( target_action=jsb, - on_exit=[jtc, torque_controller], + on_exit=[jtc, forward_controller, torque_controller], ) ) @@ -147,5 +162,6 @@ def generate_launch_description(): DeclareLaunchArgument("command_mode", default_value="position"), DeclareLaunchArgument("fri_cycle_ms", default_value="5"), DeclareLaunchArgument("joint_position_tau", default_value="0.04"), + DeclareLaunchArgument("controller", default_value="jtc"), OpaqueFunction(function=_setup_controllers), ]) diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index 1615bdd..abeecde 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/src/iiwa_config/config/moveit/iiwa_controller.yaml @@ -8,6 +8,19 @@ controller_manager: iiwa_arm_controller: type: "joint_trajectory_controller/JointTrajectoryController" + forward_position_controller: + type: "forward_command_controller/ForwardCommandController" + +joint_state_broadcaster: + ros__parameters: + interfaces: + - position + - velocity + - effort + - external_torque + - commanded_torque + - ipo_joint_position + iiwa_arm_controller: ros__parameters: joints: @@ -26,25 +39,16 @@ iiwa_arm_controller: - position - velocity - # Интерполяция от реально измеренной позиции, а не от желаемой. - # При true JTC стартует от последнего desired state, который может расходиться - # с measured_pos (= filtered_pos_ в open-loop режиме) → скачок команды → удар приводов. interpolate_from_desired_state: false - - # Разрешить неполные goals allow_partial_joints_goal: false - - # Полная остановка в конечной точке. - # При true и команде через топик JTC уходит в осцилляцию у цели. allow_nonzero_velocity_at_trajectory_end: false - state_publish_rate: 100.0 # Гц публикации /joint_states - action_monitor_rate: 20.0 # Гц мониторинга action goal + state_publish_rate: 100.0 + action_monitor_rate: 20.0 - # Допуски — насколько точно робот должен попасть в цель # constraints: - # stopped_velocity_tolerance: 0.01 # рад/с — считаем остановившимся - # goal_time: 1.0 # сек — доп. время на достижение цели + # stopped_velocity_tolerance: 0.01 + # goal_time: 1.0 # joint1: { trajectory: 0, goal: 0.01 } # joint2: { trajectory: 0, goal: 0.01 } # joint3: { trajectory: 0, goal: 0.01 } @@ -53,3 +57,14 @@ iiwa_arm_controller: # joint6: { trajectory: 0, goal: 0.01 } # joint7: { trajectory: 0, goal: 0.01 } +forward_position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + interface_name: position diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index d3e00c5..bbd36c0 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -5,6 +5,7 @@ robot: command_mode: "position" # torque, position fri_cycle_ms: 5 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц) joint_position_tau: 0.04 # постоянная времени фильтра позиций [с]: больше → плавнее, медленнее + active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro diff --git a/src/iiwa_controller_v2/CMakeLists.txt b/src/iiwa_controller_v2/CMakeLists.txt new file mode 100644 index 0000000..4c9aef8 --- /dev/null +++ b/src/iiwa_controller_v2/CMakeLists.txt @@ -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 + $ + $ +) + +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 new file mode 120000 index 0000000..0f37e91 --- /dev/null +++ b/src/iiwa_controller_v2/external @@ -0,0 +1 @@ +/home/daniel/dev/kuka_iiwa7_ros2/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 new file mode 100644 index 0000000..8972244 --- /dev/null +++ b/src/iiwa_controller_v2/iiwa_hardware_interface_plugin.xml @@ -0,0 +1,13 @@ + + + + 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 new file mode 100644 index 0000000..6c29d47 --- /dev/null +++ b/src/iiwa_controller_v2/include/iiwa_controller_v2/command_guard.hpp @@ -0,0 +1,95 @@ +#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 new file mode 100644 index 0000000..b6d212f --- /dev/null +++ b/src/iiwa_controller_v2/include/iiwa_controller_v2/fri_client.hpp @@ -0,0 +1,74 @@ +#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 new file mode 100644 index 0000000..bd78a0e --- /dev/null +++ b/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp @@ -0,0 +1,227 @@ +#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}; + bool external_torque_safety_check{true}; + double external_torque_limit{2.0}; // [Nm] per joint, safety threshold + }; + + // ── 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 ─────────────────────────────────────────────────────────────── + 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 + 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; + + // 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 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 new file mode 100644 index 0000000..c5857cd --- /dev/null +++ b/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface_type_values.hpp @@ -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 diff --git a/src/iiwa_controller_v2/package.xml b/src/iiwa_controller_v2/package.xml new file mode 100644 index 0000000..7ab285a --- /dev/null +++ b/src/iiwa_controller_v2/package.xml @@ -0,0 +1,29 @@ + + + + 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 new file mode 100644 index 0000000..2678c1a --- /dev/null +++ b/src/iiwa_controller_v2/src/fri_client.cpp @@ -0,0 +1,154 @@ +#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 new file mode 100644 index 0000000..f8382c0 --- /dev/null +++ b/src/iiwa_controller_v2/src/system_interface.cpp @@ -0,0 +1,410 @@ +#include "iiwa_controller_v2/system_interface.hpp" + +#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; + + // 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 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(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; + } + + 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 diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index e551db1..e5a69ad 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -8,6 +8,11 @@ + + + + + @@ -142,12 +147,33 @@ + + + + iiwa_controller_v2/SystemInterface + $(arg robot_ip) + $(arg fri_port) + false + $(arg command_mode) + $(arg joint_position_tau) + + $(arg open_loop) + $(arg rt_prio) + $(arg external_torque_safety_check) + $(arg external_torque_limit) + + diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index ffb0967..8fc3d98 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -17,6 +17,7 @@ class RobotCfg: description: str fri_cycle_ms: int joint_position_tau: float + active_controller: str # "jtc" | "forward" @dataclass(frozen=True) @@ -250,6 +251,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: description=resolve_path(str(require(robot_raw, "description")), settings_dir), fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)), joint_position_tau=float(robot_raw.get("joint_position_tau", 0.04)), + active_controller=str(robot_raw.get("active_controller", "jtc")), ) # digital_twin