diff --git a/flexiv_bringup/config/rizon_cartesian_controllers.yaml b/flexiv_bringup/config/rizon_cartesian_controllers.yaml new file mode 100644 index 0000000..7af5a43 --- /dev/null +++ b/flexiv_bringup/config/rizon_cartesian_controllers.yaml @@ -0,0 +1,20 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + cartesian_motion_controller: + type: cartesian_motion_controller/CartesianMotionController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + flexiv_robot_states_broadcaster: + type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster + +cartesian_motion_controller: + ros__parameters: + tcp_name: $(var robot_sn)_tcp + +flexiv_robot_states_broadcaster: + ros__parameters: + robot_sn: $(var robot_sn) diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py index 52b227d..b400e0f 100644 --- a/flexiv_bringup/launch/rizon.launch.py +++ b/flexiv_bringup/launch/rizon.launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): DeclareLaunchArgument( rdk_control_mode_param_name, default_value="joint_position", - description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance", + description="RDK control mode for joint controllers. For Cartesian mode, use rizon_cartesian.launch.py", choices=["joint_position", "joint_impedance"], ) ) diff --git a/flexiv_bringup/launch/rizon_cartesian.launch.py b/flexiv_bringup/launch/rizon_cartesian.launch.py new file mode 100644 index 0000000..0ecfaef --- /dev/null +++ b/flexiv_bringup/launch/rizon_cartesian.launch.py @@ -0,0 +1,241 @@ +""" +Launch file for Flexiv Rizon robot with Cartesian motion-force control. +This launch file sets up the robot in RT_CARTESIAN_MOTION_FORCE mode for +real-time Cartesian pure motion control. +""" + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + RegisterEventHandler, +) +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile, ParameterValue +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) + + +def generate_launch_description(): + rizon_type_param_name = "rizon_type" + robot_sn_param_name = "robot_sn" + start_rviz_param_name = "start_rviz" + load_mounted_ft_sensor_param_name = "load_mounted_ft_sensor" + use_fake_hardware_param_name = "use_fake_hardware" + fake_sensor_commands_param_name = "fake_sensor_commands" + + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + rizon_type_param_name, + description="Type of the Flexiv Rizon robot.", + default_value="Rizon4", + choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"], + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + robot_sn_param_name, + description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + start_rviz_param_name, + default_value="true", + description="Start RViz automatically with the launch file", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + load_mounted_ft_sensor_param_name, + default_value="false", + description="Flag to load the mounted force torque sensor.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + use_fake_hardware_param_name, + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + fake_sensor_commands_param_name, + default_value="false", + description="Enable fake command interfaces for sensors.", + ) + ) + + # Initialize Arguments + rizon_type = LaunchConfiguration(rizon_type_param_name) + robot_sn = LaunchConfiguration(robot_sn_param_name) + start_rviz = LaunchConfiguration(start_rviz_param_name) + load_mounted_ft_sensor = LaunchConfiguration(load_mounted_ft_sensor_param_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name) + + # Fixed to cartesian_motion_force mode + rdk_control_mode = "cartesian_motion_force" + + # Get URDF via xacro + flexiv_urdf_xacro = PathJoinSubstitution( + [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] + ) + + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + flexiv_urdf_xacro, + " ", + "robot_sn:=", + robot_sn, + " ", + "rizon_type:=", + rizon_type, + " ", + "rdk_control_mode:=", + rdk_control_mode, + " ", + "load_gripper:=false", + " ", + "load_mounted_ft_sensor:=", + load_mounted_ft_sensor, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "fake_sensor_commands:=", + fake_sensor_commands, + ] + ), + value_type=str, + ) + + robot_description = {"robot_description": robot_description_content} + + # RViZ + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(start_rviz), + ) + + # Robot controllers for Cartesian mode + robot_controllers = PathJoinSubstitution( + [FindPackageShare("flexiv_bringup"), "config", "rizon_cartesian_controllers.yaml"] + ) + + # Controller Manager + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + robot_description, + ParameterFile(robot_controllers, allow_substs=True), + {"robot_sn": robot_sn}, + {"rdk_control_mode": rdk_control_mode}, + ], + remappings=[("joint_states", "flexiv_arm/joint_states")], + output="both", + ) + + # Joint state publisher + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[ + { + "source_list": ["flexiv_arm/joint_states"], + "rate": 30, + } + ], + ) + + # Robot state publisher + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # Run joint state broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + # Run Flexiv robot states broadcaster + flexiv_robot_states_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["flexiv_robot_states_broadcaster"], + parameters=[{"robot_sn": robot_sn}], + condition=UnlessCondition(use_fake_hardware), + ) + + # Run Cartesian motion controller + cartesian_motion_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["cartesian_motion_controller", "--controller-manager", "/controller_manager"], + ) + + # Delay start of cartesian_motion_controller after joint_state_broadcaster + delay_cartesian_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[cartesian_motion_controller_spawner], + ) + ) + + # Delay rviz start after cartesian_motion_controller_spawner + delay_rviz_after_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=cartesian_motion_controller_spawner, + on_exit=[rviz_node], + ) + ) + + nodes = [ + ros2_control_node, + joint_state_publisher_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + flexiv_robot_states_broadcaster_spawner, + delay_cartesian_controller_spawner, + delay_rviz_after_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/flexiv_controllers/cartesian_motion_controller/CMakeLists.txt b/flexiv_controllers/cartesian_motion_controller/CMakeLists.txt new file mode 100644 index 0000000..cbf78ab --- /dev/null +++ b/flexiv_controllers/cartesian_motion_controller/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.8) +project(cartesian_motion_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(flexiv_msgs REQUIRED) +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(cartesian_motion_controller_parameters + src/cartesian_motion_controller_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/cartesian_motion_controller.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +target_link_libraries(${PROJECT_NAME} + cartesian_motion_controller_parameters +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_lifecycle + controller_interface + hardware_interface + pluginlib + realtime_tools + geometry_msgs + std_msgs + flexiv_msgs +) + +pluginlib_export_plugin_description_file(controller_interface cartesian_motion_controller.xml) + +install( + TARGETS ${PROJECT_NAME} cartesian_motion_controller_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + rclcpp_lifecycle + controller_interface + hardware_interface + pluginlib + realtime_tools + geometry_msgs + std_msgs + flexiv_msgs +) + +ament_package() diff --git a/flexiv_controllers/cartesian_motion_controller/cartesian_motion_controller.xml b/flexiv_controllers/cartesian_motion_controller/cartesian_motion_controller.xml new file mode 100644 index 0000000..c1a3184 --- /dev/null +++ b/flexiv_controllers/cartesian_motion_controller/cartesian_motion_controller.xml @@ -0,0 +1,10 @@ + + + + Controller for Flexiv robot real-time Cartesian motion-force control. + Subscribes to CartesianMotionForceCommand messages and sends them to the hardware interface. + + + diff --git a/flexiv_controllers/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.hpp b/flexiv_controllers/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.hpp new file mode 100644 index 0000000..f4b2507 --- /dev/null +++ b/flexiv_controllers/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.hpp @@ -0,0 +1,84 @@ +/** + * @file cartesian_motion_controller.hpp + * @brief ROS2 controller for Flexiv robot Cartesian motion-force control. + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + */ + +#ifndef CARTESIAN_MOTION_CONTROLLER__CARTESIAN_MOTION_CONTROLLER_HPP_ +#define CARTESIAN_MOTION_CONTROLLER__CARTESIAN_MOTION_CONTROLLER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace cartesian_motion_controller { + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class CartesianMotionController : public controller_interface::ControllerInterface +{ +public: + CartesianMotionController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CallbackReturn on_init() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::return_type update( + const rclcpp::Time& time, const rclcpp::Duration& period) override; + +private: + std::shared_ptr param_listener_; + Params params_; + + std::vector> + cartesian_pose_command_interfaces_; + std::vector> + cartesian_wrench_command_interfaces_; + std::vector> + cartesian_pose_state_interfaces_; + + rclcpp::Subscription::SharedPtr command_sub_; + realtime_tools::RealtimeBuffer> + rt_command_ptr_; + + using StatePublisher = realtime_tools::RealtimePublisher; + std::shared_ptr> state_publisher_; + std::unique_ptr realtime_state_publisher_; + + static constexpr size_t kCartPoseSize = 7; + static constexpr size_t kCartDoF = 6; + + const std::vector cartesian_pose_interface_names_ = { + "cartesian_pose_x", "cartesian_pose_y", "cartesian_pose_z", + "cartesian_pose_qw", "cartesian_pose_qx", "cartesian_pose_qy", "cartesian_pose_qz"}; + + const std::vector cartesian_wrench_interface_names_ = { + "cartesian_wrench_fx", "cartesian_wrench_fy", "cartesian_wrench_fz", + "cartesian_wrench_mx", "cartesian_wrench_my", "cartesian_wrench_mz"}; + + void commandCallback( + const std::shared_ptr msg); +}; + +} // namespace cartesian_motion_controller + +#endif // CARTESIAN_MOTION_CONTROLLER__CARTESIAN_MOTION_CONTROLLER_HPP_ diff --git a/flexiv_controllers/cartesian_motion_controller/package.xml b/flexiv_controllers/cartesian_motion_controller/package.xml new file mode 100644 index 0000000..70cb02d --- /dev/null +++ b/flexiv_controllers/cartesian_motion_controller/package.xml @@ -0,0 +1,29 @@ + + + + cartesian_motion_controller + 1.8.0 + ROS2 controller for Flexiv robot Cartesian motion-force control + Flexiv + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_lifecycle + controller_interface + hardware_interface + pluginlib + realtime_tools + geometry_msgs + std_msgs + flexiv_msgs + generate_parameter_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/flexiv_controllers/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/flexiv_controllers/cartesian_motion_controller/src/cartesian_motion_controller.cpp new file mode 100644 index 0000000..dbecf9c --- /dev/null +++ b/flexiv_controllers/cartesian_motion_controller/src/cartesian_motion_controller.cpp @@ -0,0 +1,273 @@ +/** + * @file cartesian_motion_controller.cpp + * @brief ROS2 controller for Flexiv robot Cartesian motion-force control + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + */ + +#include "cartesian_motion_controller/cartesian_motion_controller.hpp" + +#include +#include + +namespace cartesian_motion_controller { + +CartesianMotionController::CartesianMotionController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::InterfaceConfiguration +CartesianMotionController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tcp_name = params_.tcp_name; + + for (const auto& interface_name : cartesian_pose_interface_names_) { + config.names.push_back(tcp_name + "/" + interface_name); + } + for (const auto& interface_name : cartesian_wrench_interface_names_) { + config.names.push_back(tcp_name + "/" + interface_name); + } + + return config; +} + +controller_interface::InterfaceConfiguration +CartesianMotionController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tcp_name = params_.tcp_name; + + for (const auto& interface_name : cartesian_pose_interface_names_) { + config.names.push_back(tcp_name + "/" + interface_name); + } + + return config; +} + +CallbackReturn CartesianMotionController::on_init() +{ + try { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_node()->get_logger(), + "Exception thrown during init: %s", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianMotionController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (params_.tcp_name.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'tcp_name' parameter is empty"); + return CallbackReturn::ERROR; + } + + command_sub_ = get_node()->create_subscription( + "~/command", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianMotionController::commandCallback, this, std::placeholders::_1)); + + state_publisher_ = get_node()->create_publisher( + "~/tcp_pose", rclcpp::SystemDefaultsQoS()); + realtime_state_publisher_ = std::make_unique(state_publisher_); + + RCLCPP_INFO(get_node()->get_logger(), "Cartesian motion controller configured"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianMotionController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) +{ + cartesian_pose_command_interfaces_.clear(); + cartesian_wrench_command_interfaces_.clear(); + cartesian_pose_state_interfaces_.clear(); + + const std::string tcp_name = params_.tcp_name; + + for (const auto& interface_name : cartesian_pose_interface_names_) { + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](const auto& interface) { + return interface.get_prefix_name() == tcp_name + && interface.get_interface_name() == interface_name; + }); + if (it != command_interfaces_.end()) { + cartesian_pose_command_interfaces_.emplace_back(*it); + } else { + RCLCPP_ERROR(get_node()->get_logger(), + "Could not find command interface: %s/%s", tcp_name.c_str(), interface_name.c_str()); + return CallbackReturn::ERROR; + } + } + + for (const auto& interface_name : cartesian_wrench_interface_names_) { + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](const auto& interface) { + return interface.get_prefix_name() == tcp_name + && interface.get_interface_name() == interface_name; + }); + if (it != command_interfaces_.end()) { + cartesian_wrench_command_interfaces_.emplace_back(*it); + } else { + RCLCPP_ERROR(get_node()->get_logger(), + "Could not find command interface: %s/%s", tcp_name.c_str(), interface_name.c_str()); + return CallbackReturn::ERROR; + } + } + + for (const auto& interface_name : cartesian_pose_interface_names_) { + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](const auto& interface) { + return interface.get_prefix_name() == tcp_name + && interface.get_interface_name() == interface_name; + }); + if (it != state_interfaces_.end()) { + cartesian_pose_state_interfaces_.emplace_back(*it); + } else { + RCLCPP_ERROR(get_node()->get_logger(), + "Could not find state interface: %s/%s", tcp_name.c_str(), interface_name.c_str()); + return CallbackReturn::ERROR; + } + } + + rt_command_ptr_.reset(); + + RCLCPP_INFO(get_node()->get_logger(), "Cartesian motion controller activated"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianMotionController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) +{ + cartesian_pose_command_interfaces_.clear(); + cartesian_wrench_command_interfaces_.clear(); + cartesian_pose_state_interfaces_.clear(); + + RCLCPP_INFO(get_node()->get_logger(), "Cartesian motion controller deactivated"); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type CartesianMotionController::update( + const rclcpp::Time& time, const rclcpp::Duration& /*period*/) +{ + auto command_ptr = rt_command_ptr_.readFromRT(); + + if (command_ptr && *command_ptr) { + const auto& cmd = *command_ptr; + + // Validate quaternion orientation + double qw = cmd->target_pose.orientation.w; + double qx = cmd->target_pose.orientation.x; + double qy = cmd->target_pose.orientation.y; + double qz = cmd->target_pose.orientation.z; + double quat_norm = std::sqrt(qw * qw + qx * qx + qy * qy + qz * qz); + + if (std::abs(quat_norm - 1.0) > 0.01) { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Invalid quaternion (norm = %.4f, expected 1.0). Normalizing.", quat_norm); + + // Normalize quaternion + if (quat_norm > 1e-6) { + qw /= quat_norm; + qx /= quat_norm; + qy /= quat_norm; + qz /= quat_norm; + } else { + RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Quaternion norm is near zero. Using identity orientation."); + qw = 1.0; + qx = qy = qz = 0.0; + } + } + + // Validate position (check for NaN and Inf) + if (!std::isfinite(cmd->target_pose.position.x) || + !std::isfinite(cmd->target_pose.position.y) || + !std::isfinite(cmd->target_pose.position.z)) { + RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Invalid position command (NaN or Inf detected). Skipping command."); + return controller_interface::return_type::OK; + } + + // Write validated pose commands + if (cartesian_pose_command_interfaces_.size() == kCartPoseSize) { + (void)cartesian_pose_command_interfaces_[0].get().set_value(cmd->target_pose.position.x); + (void)cartesian_pose_command_interfaces_[1].get().set_value(cmd->target_pose.position.y); + (void)cartesian_pose_command_interfaces_[2].get().set_value(cmd->target_pose.position.z); + (void)cartesian_pose_command_interfaces_[3].get().set_value(qw); + (void)cartesian_pose_command_interfaces_[4].get().set_value(qx); + (void)cartesian_pose_command_interfaces_[5].get().set_value(qy); + (void)cartesian_pose_command_interfaces_[6].get().set_value(qz); + } + + // Validate and write wrench commands + if (cartesian_wrench_command_interfaces_.size() == kCartDoF) { + // Check for valid wrench values + bool wrench_valid = std::isfinite(cmd->target_wrench.force.x) && + std::isfinite(cmd->target_wrench.force.y) && + std::isfinite(cmd->target_wrench.force.z) && + std::isfinite(cmd->target_wrench.torque.x) && + std::isfinite(cmd->target_wrench.torque.y) && + std::isfinite(cmd->target_wrench.torque.z); + + if (wrench_valid) { + (void)cartesian_wrench_command_interfaces_[0].get().set_value(cmd->target_wrench.force.x); + (void)cartesian_wrench_command_interfaces_[1].get().set_value(cmd->target_wrench.force.y); + (void)cartesian_wrench_command_interfaces_[2].get().set_value(cmd->target_wrench.force.z); + (void)cartesian_wrench_command_interfaces_[3].get().set_value(cmd->target_wrench.torque.x); + (void)cartesian_wrench_command_interfaces_[4].get().set_value(cmd->target_wrench.torque.y); + (void)cartesian_wrench_command_interfaces_[5].get().set_value(cmd->target_wrench.torque.z); + } else { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Invalid wrench command (NaN or Inf detected). Using zero wrench."); + // Write zeros for safety + for (size_t i = 0; i < kCartDoF; ++i) { + (void)cartesian_wrench_command_interfaces_[i].get().set_value(0.0); + } + } + } + } + + if (realtime_state_publisher_ && realtime_state_publisher_->trylock()) { + auto& msg = realtime_state_publisher_->msg_; + msg.header.stamp = time; + msg.header.frame_id = "world"; + + if (cartesian_pose_state_interfaces_.size() == kCartPoseSize) { + msg.pose.position.x = cartesian_pose_state_interfaces_[0].get().get_optional().value_or(0.0); + msg.pose.position.y = cartesian_pose_state_interfaces_[1].get().get_optional().value_or(0.0); + msg.pose.position.z = cartesian_pose_state_interfaces_[2].get().get_optional().value_or(0.0); + msg.pose.orientation.w = cartesian_pose_state_interfaces_[3].get().get_optional().value_or(1.0); + msg.pose.orientation.x = cartesian_pose_state_interfaces_[4].get().get_optional().value_or(0.0); + msg.pose.orientation.y = cartesian_pose_state_interfaces_[5].get().get_optional().value_or(0.0); + msg.pose.orientation.z = cartesian_pose_state_interfaces_[6].get().get_optional().value_or(0.0); + } + + realtime_state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +void CartesianMotionController::commandCallback( + const std::shared_ptr msg) +{ + rt_command_ptr_.writeFromNonRT(msg); +} + +} // namespace cartesian_motion_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_motion_controller::CartesianMotionController, + controller_interface::ControllerInterface) diff --git a/flexiv_controllers/cartesian_motion_controller/src/cartesian_motion_controller_parameters.yaml b/flexiv_controllers/cartesian_motion_controller/src/cartesian_motion_controller_parameters.yaml new file mode 100644 index 0000000..2aca7c2 --- /dev/null +++ b/flexiv_controllers/cartesian_motion_controller/src/cartesian_motion_controller_parameters.yaml @@ -0,0 +1,7 @@ +cartesian_motion_controller: + tcp_name: + type: string + default_value: "" + description: "Name of the TCP interface (e.g., 'tcp' or with prefix 'rizon4s_123456_tcp')" + validation: + not_empty<>: null diff --git a/flexiv_description/urdf/rizon10.ros2_control.xacro b/flexiv_description/urdf/rizon10.ros2_control.xacro index e3e161f..77ee78a 100644 --- a/flexiv_description/urdf/rizon10.ros2_control.xacro +++ b/flexiv_description/urdf/rizon10.ros2_control.xacro @@ -4,6 +4,7 @@ @@ -19,6 +20,7 @@ flexiv_hardware/FlexivHardwareInterface ${robot_sn} ${prefix} + ${rdk_control_mode} @@ -183,6 +185,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flexiv_description/urdf/rizon4.ros2_control.xacro b/flexiv_description/urdf/rizon4.ros2_control.xacro index c7295bb..243df1b 100644 --- a/flexiv_description/urdf/rizon4.ros2_control.xacro +++ b/flexiv_description/urdf/rizon4.ros2_control.xacro @@ -185,6 +185,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flexiv_hardware/CMakeLists.txt b/flexiv_hardware/CMakeLists.txt index bc326ae..e05ec50 100644 --- a/flexiv_hardware/CMakeLists.txt +++ b/flexiv_hardware/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(flexiv_rdk REQUIRED) +find_package(flexiv_msgs REQUIRED) add_library( ${PROJECT_NAME} @@ -43,6 +44,7 @@ ament_target_dependencies( pluginlib rclcpp rclcpp_lifecycle + flexiv_msgs ) pluginlib_export_plugin_description_file(hardware_interface flexiv_hardware_interface_plugin.xml) @@ -77,6 +79,7 @@ ament_export_dependencies( pluginlib rclcpp rclcpp_lifecycle + flexiv_msgs ) ament_package() diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp index c7aadc3..f59f6e2 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp @@ -9,11 +9,14 @@ #ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ #define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ +#include #include #include +#include #include // ROS +#include #include #include #include @@ -21,6 +24,13 @@ #include #include +// Flexiv msgs +#include "flexiv_msgs/srv/set_cartesian_impedance.hpp" +#include "flexiv_msgs/srv/set_null_space_posture.hpp" +#include "flexiv_msgs/srv/set_max_contact_wrench.hpp" +#include "flexiv_msgs/srv/set_force_control_frame.hpp" +#include "flexiv_msgs/srv/set_force_control_axis.hpp" + // ros2_control hardware_interface #include #include @@ -41,9 +51,16 @@ enum StoppingInterface NONE, STOP_POSITION, STOP_VELOCITY, - STOP_EFFORT + STOP_EFFORT, + STOP_CARTESIAN }; +/** Cartesian pose array size: [x, y, z, qw, qx, qy, qz] */ +constexpr size_t kCartPoseSize = 7; + +/** Cartesian space degrees of freedom: [Fx, Fy, Fz, Mx, My, Mz] */ +constexpr size_t kCartDoF = 6; + class FlexivHardwareInterface : public hardware_interface::SystemInterface { public: @@ -93,6 +110,14 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface std::vector hw_states_joint_velocities_; std::vector hw_states_joint_efforts_; + // Cartesian commands and states + std::array hw_commands_cartesian_pose_; + std::array hw_commands_cartesian_wrench_; + std::array hw_commands_cartesian_velocity_; + std::array hw_commands_cartesian_acceleration_; + std::array hw_states_cartesian_pose_; + std::array hw_states_cartesian_velocity_; + // Robot States flexiv::rdk::RobotStates hw_flexiv_robot_states_; flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_ = &hw_flexiv_robot_states_; @@ -106,13 +131,44 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface static rclcpp::Logger getLogger(); - // control modes + // Controller mode tracking bool controllers_initialized_; std::vector stop_modes_; std::vector start_modes_; bool position_controller_running_; bool velocity_controller_running_; bool torque_controller_running_; + bool cartesian_motion_controller_running_; + bool cartesian_mode_active_; + std::array init_tcp_pose_; + + bool isCartesianCommandValid() const; + + // Cartesian configuration services (run in a dedicated thread) + rclcpp::Node::SharedPtr service_node_; + std::thread service_thread_; + std::atomic service_thread_running_{false}; + rclcpp::Service::SharedPtr set_cartesian_impedance_srv_; + rclcpp::Service::SharedPtr set_null_space_posture_srv_; + rclcpp::Service::SharedPtr set_max_contact_wrench_srv_; + rclcpp::Service::SharedPtr set_force_control_frame_srv_; + rclcpp::Service::SharedPtr set_force_control_axis_srv_; + + void setCartesianImpedanceCallback( + const std::shared_ptr request, + std::shared_ptr response); + void setNullSpacePostureCallback( + const std::shared_ptr request, + std::shared_ptr response); + void setMaxContactWrenchCallback( + const std::shared_ptr request, + std::shared_ptr response); + void setForceControlFrameCallback( + const std::shared_ptr request, + std::shared_ptr response); + void setForceControlAxisCallback( + const std::shared_ptr request, + std::shared_ptr response); }; } /* namespace flexiv_hardware */ diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml index 31f342d..4db1666 100644 --- a/flexiv_hardware/package.xml +++ b/flexiv_hardware/package.xml @@ -14,6 +14,7 @@ rclcpp_lifecycle hardware_interface pluginlib + flexiv_msgs ament_lint_auto ament_lint_common diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index 34a3e79..e94cdb3 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -8,6 +8,8 @@ #include #include +#include +#include #include #include @@ -22,6 +24,21 @@ namespace { constexpr double kMaxJointVelocity = 2.0; constexpr double kMaxJointAcceleration = 3.0; +// Custom command interface names for Cartesian control +const std::string HW_IF_CARTESIAN_POSE_X = "cartesian_pose_x"; +const std::string HW_IF_CARTESIAN_POSE_Y = "cartesian_pose_y"; +const std::string HW_IF_CARTESIAN_POSE_Z = "cartesian_pose_z"; +const std::string HW_IF_CARTESIAN_POSE_QW = "cartesian_pose_qw"; +const std::string HW_IF_CARTESIAN_POSE_QX = "cartesian_pose_qx"; +const std::string HW_IF_CARTESIAN_POSE_QY = "cartesian_pose_qy"; +const std::string HW_IF_CARTESIAN_POSE_QZ = "cartesian_pose_qz"; +const std::string HW_IF_CARTESIAN_WRENCH_FX = "cartesian_wrench_fx"; +const std::string HW_IF_CARTESIAN_WRENCH_FY = "cartesian_wrench_fy"; +const std::string HW_IF_CARTESIAN_WRENCH_FZ = "cartesian_wrench_fz"; +const std::string HW_IF_CARTESIAN_WRENCH_MX = "cartesian_wrench_mx"; +const std::string HW_IF_CARTESIAN_WRENCH_MY = "cartesian_wrench_my"; +const std::string HW_IF_CARTESIAN_WRENCH_MZ = "cartesian_wrench_mz"; + } namespace flexiv_hardware { @@ -54,8 +71,18 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( position_controller_running_ = false; velocity_controller_running_ = false; torque_controller_running_ = false; + cartesian_motion_controller_running_ = false; + cartesian_mode_active_ = false; controllers_initialized_ = false; + hw_commands_cartesian_pose_.fill(std::numeric_limits::quiet_NaN()); + hw_commands_cartesian_wrench_.fill(0.0); + hw_commands_cartesian_velocity_.fill(0.0); + hw_commands_cartesian_acceleration_.fill(0.0); + hw_states_cartesian_pose_.fill(0.0); + hw_states_cartesian_velocity_.fill(0.0); + init_tcp_pose_.fill(0.0); + if (info_.joints.size() != kJointDoF) { RCLCPP_FATAL(getLogger(), "Got %ld joints. Expected %ld.", info_.joints.size(), kJointDoF); return hardware_interface::CallbackReturn::ERROR; @@ -131,10 +158,12 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( rdk_control_mode_ = flexiv::rdk::Mode::NRT_JOINT_POSITION; } else if (rdk_control_mode_str == "joint_impedance") { rdk_control_mode_ = flexiv::rdk::Mode::NRT_JOINT_IMPEDANCE; + } else if (rdk_control_mode_str == "cartesian_motion_force") { + rdk_control_mode_ = flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE; } else { RCLCPP_FATAL(getLogger(), "Parameter 'rdk_control_mode' has invalid value '%s'. Options: joint_position, " - "joint_impedance", + "joint_impedance, cartesian_motion_force", rdk_control_mode_str.c_str()); return hardware_interface::CallbackReturn::ERROR; } @@ -185,6 +214,22 @@ std::vector FlexivHardwareInterface::export_ prefix + "gpio", "digital_input_" + std::to_string(i), &hw_states_gpio_in_[i])); } + // Cartesian TCP pose states + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_X, &hw_states_cartesian_pose_[0])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_Y, &hw_states_cartesian_pose_[1])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_Z, &hw_states_cartesian_pose_[2])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QW, &hw_states_cartesian_pose_[3])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QX, &hw_states_cartesian_pose_[4])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QY, &hw_states_cartesian_pose_[5])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QZ, &hw_states_cartesian_pose_[6])); + return state_interfaces; } @@ -209,6 +254,36 @@ FlexivHardwareInterface::export_command_interfaces() prefix + "gpio", "digital_output_" + std::to_string(i), &hw_commands_gpio_out_[i])); } + // Cartesian TCP pose commands + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_X, &hw_commands_cartesian_pose_[0])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_Y, &hw_commands_cartesian_pose_[1])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_Z, &hw_commands_cartesian_pose_[2])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QW, &hw_commands_cartesian_pose_[3])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QX, &hw_commands_cartesian_pose_[4])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QY, &hw_commands_cartesian_pose_[5])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_POSE_QZ, &hw_commands_cartesian_pose_[6])); + + // Cartesian TCP wrench commands + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_WRENCH_FX, &hw_commands_cartesian_wrench_[0])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_WRENCH_FY, &hw_commands_cartesian_wrench_[1])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_WRENCH_FZ, &hw_commands_cartesian_wrench_[2])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_WRENCH_MX, &hw_commands_cartesian_wrench_[3])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_WRENCH_MY, &hw_commands_cartesian_wrench_[4])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "tcp", HW_IF_CARTESIAN_WRENCH_MZ, &hw_commands_cartesian_wrench_[5])); + return command_interfaces; } @@ -255,6 +330,86 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate( RCLCPP_INFO(getLogger(), "System successfully started!"); + // Switch to Cartesian mode early (before RT loop) to avoid blocking 1kHz control + if (rdk_control_mode_ == flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + // Zero force-torque sensor + RCLCPP_WARN(getLogger(), + "Zeroing force/torque sensor. Make sure nothing is in contact with the robot."); + try { + robot_->SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); + robot_->ExecutePrimitive("ZeroFTSensor", std::map{}); + + // Wait for primitive to finish + while (!std::get(robot_->primitive_states()["terminated"])) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + RCLCPP_INFO(getLogger(), "Sensor zeroing complete"); + } catch (const std::exception& e) { + RCLCPP_FATAL(getLogger(), "Failed to zero force/torque sensor"); + RCLCPP_FATAL(getLogger(), e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + + init_tcp_pose_ = robot_->states().tcp_pose; + robot_->SwitchMode(flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE); + robot_->SetForceControlAxis( + std::array{false, false, false, false, false, false}); + cartesian_mode_active_ = true; + RCLCPP_INFO(getLogger(), "Switched to RT_CARTESIAN_MOTION_FORCE mode"); + RCLCPP_INFO(getLogger(), "Initial TCP pose: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]", + init_tcp_pose_[0], init_tcp_pose_[1], init_tcp_pose_[2], + init_tcp_pose_[3], init_tcp_pose_[4], init_tcp_pose_[5], init_tcp_pose_[6]); + + // Create Cartesian configuration services on a dedicated node/thread + const std::string prefix = info_.hardware_parameters.at("prefix"); + std::string sanitized_prefix = prefix; + std::replace(sanitized_prefix.begin(), sanitized_prefix.end(), '-', '_'); + const std::string node_name = sanitized_prefix.empty() + ? "flexiv_hardware_services" + : sanitized_prefix + "flexiv_hardware_services"; + service_node_ = rclcpp::Node::make_shared(node_name); + + set_cartesian_impedance_srv_ = service_node_->create_service( + "~/set_cartesian_impedance", + std::bind(&FlexivHardwareInterface::setCartesianImpedanceCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_null_space_posture_srv_ = service_node_->create_service( + "~/set_null_space_posture", + std::bind(&FlexivHardwareInterface::setNullSpacePostureCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_max_contact_wrench_srv_ = service_node_->create_service( + "~/set_max_contact_wrench", + std::bind(&FlexivHardwareInterface::setMaxContactWrenchCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_force_control_frame_srv_ = service_node_->create_service( + "~/set_force_control_frame", + std::bind(&FlexivHardwareInterface::setForceControlFrameCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_force_control_axis_srv_ = service_node_->create_service( + "~/set_force_control_axis", + std::bind(&FlexivHardwareInterface::setForceControlAxisCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + service_thread_running_ = true; + service_thread_ = std::thread([this]() { + while (service_thread_running_) { + rclcpp::spin_some(service_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + }); + + RCLCPP_INFO(getLogger(), "Cartesian configuration services available:"); + RCLCPP_INFO(getLogger(), " - %s/set_cartesian_impedance", node_name.c_str()); + RCLCPP_INFO(getLogger(), " - %s/set_null_space_posture", node_name.c_str()); + RCLCPP_INFO(getLogger(), " - %s/set_max_contact_wrench", node_name.c_str()); + RCLCPP_INFO(getLogger(), " - %s/set_force_control_frame", node_name.c_str()); + RCLCPP_INFO(getLogger(), " - %s/set_force_control_axis", node_name.c_str()); + } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -263,6 +418,19 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate( { RCLCPP_INFO(getLogger(), "Stopping... please wait..."); + service_thread_running_ = false; + if (service_thread_.joinable()) { + service_thread_.join(); + } + set_cartesian_impedance_srv_.reset(); + set_null_space_posture_srv_.reset(); + set_max_contact_wrench_srv_.reset(); + set_force_control_frame_srv_.reset(); + set_force_control_axis_srv_.reset(); + service_node_.reset(); + + cartesian_mode_active_ = false; + robot_->Stop(); RCLCPP_INFO(getLogger(), "System successfully stopped!"); @@ -277,14 +445,22 @@ hardware_interface::return_type FlexivHardwareInterface::read( hw_flexiv_robot_states_ = robot_->states(); - // Read joint states for (size_t i = 0; i < info_.joints.size(); i++) { hw_states_joint_positions_[i] = robot_->states().q[i]; hw_states_joint_velocities_[i] = robot_->states().dtheta[i]; hw_states_joint_efforts_[i] = robot_->states().tau[i]; } - // Read GPIO input states + const auto& tcp_pose = robot_->states().tcp_pose; + for (size_t i = 0; i < kCartPoseSize; i++) { + hw_states_cartesian_pose_[i] = tcp_pose[i]; + } + + const auto& tcp_vel = robot_->states().tcp_vel; + for (size_t i = 0; i < kCartDoF; i++) { + hw_states_cartesian_velocity_[i] = tcp_vel[i]; + } + auto gpio_in = robot_->digital_inputs(); for (size_t i = 0; i < hw_states_gpio_in_.size(); i++) { hw_states_gpio_in_[i] = static_cast(gpio_in[i]); @@ -294,10 +470,19 @@ hardware_interface::return_type FlexivHardwareInterface::read( return hardware_interface::return_type::OK; } +bool FlexivHardwareInterface::isCartesianCommandValid() const +{ + for (size_t i = 0; i < kCartPoseSize; i++) { + if (std::isnan(hw_commands_cartesian_pose_[i])) { + return false; + } + } + return true; +} + hardware_interface::return_type FlexivHardwareInterface::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - // Initialize target vectors to hold position std::vector target_pos(robot_->info().DoF); std::vector target_vel(robot_->info().DoF); @@ -323,7 +508,7 @@ hardware_interface::return_type FlexivHardwareInterface::write( target_pos = hw_commands_joint_positions_; robot_->SendJointPosition(target_pos, target_vel, max_vel, max_acc); } else if (velocity_controller_running_ && robot_->mode() == rdk_control_mode_ && !isNanVel) { - target_pos = hw_commands_joint_positions_; + target_pos = hw_states_joint_positions_; target_vel = hw_commands_joint_velocities_; robot_->SendJointPosition(target_pos, target_vel, max_vel, max_acc); } else if (torque_controller_running_ && robot_->mode() == flexiv::rdk::Mode::RT_JOINT_TORQUE @@ -331,6 +516,16 @@ hardware_interface::return_type FlexivHardwareInterface::write( std::vector target_torque(robot_->info().DoF); target_torque = hw_commands_joint_efforts_; robot_->StreamJointTorque(target_torque, true, true); + } else if (cartesian_mode_active_ + && robot_->mode() == flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + if (cartesian_motion_controller_running_ && isCartesianCommandValid()) { + robot_->StreamCartesianMotionForce( + hw_commands_cartesian_pose_, hw_commands_cartesian_wrench_, + hw_commands_cartesian_velocity_, hw_commands_cartesian_acceleration_); + } else { + std::array zero_wrench = {}; + robot_->StreamCartesianMotionForce(init_tcp_pose_, zero_wrench); + } } // Write digital output @@ -369,7 +564,8 @@ hardware_interface::return_type FlexivHardwareInterface::prepare_command_mode_sw start_modes_.clear(); stop_modes_.clear(); - // Starting interfaces + const std::string prefix = info_.hardware_parameters.at("prefix"); + for (const auto& key : start_interfaces) { for (std::size_t i = 0; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { @@ -382,13 +578,42 @@ hardware_interface::return_type FlexivHardwareInterface::prepare_command_mode_sw start_modes_.push_back(hardware_interface::HW_IF_EFFORT); } } + if (key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_X + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_Y + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_Z + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QW + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QX + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QY + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QZ) { + start_modes_.push_back("cartesian_pose"); + } + if (key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_FX + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_FY + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_FZ + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_MX + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_MY + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_MZ) { + start_modes_.push_back("cartesian_wrench"); + } } - // All joints must be given new command mode at the same time - if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) { + + size_t cartesian_pose_count = std::count(start_modes_.begin(), start_modes_.end(), "cartesian_pose"); + size_t cartesian_wrench_count = std::count(start_modes_.begin(), start_modes_.end(), "cartesian_wrench"); + size_t cartesian_count = cartesian_pose_count + cartesian_wrench_count; + if (cartesian_pose_count > 0 && cartesian_pose_count != kCartPoseSize) { + RCLCPP_ERROR(getLogger(), "All Cartesian pose interfaces must be claimed together"); return hardware_interface::return_type::ERROR; } - // All joints must have the same command mode - if (start_modes_.size() != 0 + if (cartesian_wrench_count > 0 && cartesian_wrench_count != kCartDoF) { + RCLCPP_ERROR(getLogger(), "All Cartesian wrench interfaces must be claimed together"); + return hardware_interface::return_type::ERROR; + } + + size_t joint_start_count = start_modes_.size() - cartesian_count; + if (joint_start_count != 0 && joint_start_count != info_.joints.size()) { + return hardware_interface::return_type::ERROR; + } + if (joint_start_count != 0 && cartesian_count == 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { return hardware_interface::return_type::ERROR; } @@ -406,12 +631,21 @@ hardware_interface::return_type FlexivHardwareInterface::prepare_command_mode_sw stop_modes_.push_back(StoppingInterface::STOP_EFFORT); } } - } - // stop all interfaces at the same time - if (stop_modes_.size() != 0 - && (stop_modes_.size() != info_.joints.size() - || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) { - return hardware_interface::return_type::ERROR; + if (key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_X + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_Y + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_Z + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QW + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QX + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QY + || key == prefix + "tcp/" + HW_IF_CARTESIAN_POSE_QZ + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_FX + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_FY + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_FZ + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_MX + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_MY + || key == prefix + "tcp/" + HW_IF_CARTESIAN_WRENCH_MZ) { + stop_modes_.push_back(StoppingInterface::STOP_CARTESIAN); + } } controllers_initialized_ = true; @@ -438,6 +672,11 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw != stop_modes_.end()) { torque_controller_running_ = false; robot_->Stop(); + } else if (stop_modes_.size() != 0 + && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_CARTESIAN) + != stop_modes_.end()) { + cartesian_motion_controller_running_ = false; + robot_->Stop(); } if (start_modes_.size() != 0 @@ -445,6 +684,7 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw != start_modes_.end()) { velocity_controller_running_ = false; torque_controller_running_ = false; + cartesian_motion_controller_running_ = false; // Hold joints before user commands arrives std::fill(hw_commands_joint_positions_.begin(), hw_commands_joint_positions_.end(), @@ -460,6 +700,7 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw != start_modes_.end()) { position_controller_running_ = false; torque_controller_running_ = false; + cartesian_motion_controller_running_ = false; // Hold joints before user commands arrives std::fill(hw_commands_joint_velocities_.begin(), hw_commands_joint_velocities_.end(), @@ -475,6 +716,7 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw != start_modes_.end()) { position_controller_running_ = false; velocity_controller_running_ = false; + cartesian_motion_controller_running_ = false; // Hold joints when starting joint torque controller before user // commands arrives @@ -485,6 +727,25 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw robot_->SwitchMode(flexiv::rdk::Mode::RT_JOINT_TORQUE); torque_controller_running_ = true; + } else if (start_modes_.size() != 0 + && std::find(start_modes_.begin(), start_modes_.end(), "cartesian_pose") + != start_modes_.end()) { + position_controller_running_ = false; + velocity_controller_running_ = false; + torque_controller_running_ = false; + + init_tcp_pose_ = robot_->states().tcp_pose; + hw_commands_cartesian_pose_.fill(std::numeric_limits::quiet_NaN()); + hw_commands_cartesian_wrench_.fill(0.0); + hw_commands_cartesian_velocity_.fill(0.0); + hw_commands_cartesian_acceleration_.fill(0.0); + + RCLCPP_INFO(getLogger(), "Cartesian motion controller activated"); + RCLCPP_INFO(getLogger(), "Initial TCP pose: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]", + init_tcp_pose_[0], init_tcp_pose_[1], init_tcp_pose_[2], + init_tcp_pose_[3], init_tcp_pose_[4], init_tcp_pose_[5], init_tcp_pose_[6]); + + cartesian_motion_controller_running_ = true; } start_modes_.clear(); @@ -493,6 +754,159 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw return hardware_interface::return_type::OK; } +void FlexivHardwareInterface::setCartesianImpedanceCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + try { + if (robot_->mode() != flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + response->success = false; + response->message = "Robot is not in Cartesian motion-force control mode"; + return; + } + + std::array stiffness; + std::array damping_ratio; + for (size_t i = 0; i < kCartDoF; i++) { + stiffness[i] = request->stiffness[i]; + damping_ratio[i] = request->damping_ratio[i]; + } + + robot_->SetCartesianImpedance(stiffness, damping_ratio); + + response->success = true; + response->message = "Cartesian impedance set successfully"; + RCLCPP_INFO(getLogger(), "SetCartesianImpedance: stiffness=[%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]", + stiffness[0], stiffness[1], stiffness[2], stiffness[3], stiffness[4], stiffness[5]); + } catch (const std::exception& e) { + response->success = false; + response->message = std::string("Failed to set Cartesian impedance: ") + e.what(); + RCLCPP_ERROR(getLogger(), "%s", response->message.c_str()); + } +} + +void FlexivHardwareInterface::setNullSpacePostureCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + try { + if (robot_->mode() != flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + response->success = false; + response->message = "Robot is not in Cartesian motion-force control mode"; + return; + } + + std::vector ref_positions(request->ref_positions.begin(), + request->ref_positions.end()); + + robot_->SetNullSpacePosture(ref_positions); + + response->success = true; + response->message = "Null space posture set successfully"; + RCLCPP_INFO(getLogger(), "SetNullSpacePosture: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + ref_positions[0], ref_positions[1], ref_positions[2], ref_positions[3], + ref_positions[4], ref_positions[5], ref_positions[6]); + } catch (const std::exception& e) { + response->success = false; + response->message = std::string("Failed to set null space posture: ") + e.what(); + RCLCPP_ERROR(getLogger(), "%s", response->message.c_str()); + } +} + +void FlexivHardwareInterface::setMaxContactWrenchCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + try { + if (robot_->mode() != flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + response->success = false; + response->message = "Robot is not in Cartesian motion-force control mode"; + return; + } + + std::array max_wrench; + for (size_t i = 0; i < kCartDoF; i++) { + max_wrench[i] = request->max_wrench[i]; + } + + robot_->SetMaxContactWrench(max_wrench); + + response->success = true; + response->message = "Max contact wrench set successfully"; + RCLCPP_INFO(getLogger(), "SetMaxContactWrench: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]", + max_wrench[0], max_wrench[1], max_wrench[2], max_wrench[3], max_wrench[4], max_wrench[5]); + } catch (const std::exception& e) { + response->success = false; + response->message = std::string("Failed to set max contact wrench: ") + e.what(); + RCLCPP_ERROR(getLogger(), "%s", response->message.c_str()); + } +} + +void FlexivHardwareInterface::setForceControlFrameCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + try { + if (robot_->mode() != flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + response->success = false; + response->message = "Robot is not in Cartesian motion-force control mode"; + return; + } + + flexiv::rdk::CoordType frame; + if (request->frame == "world" || request->frame == "WORLD") { + frame = flexiv::rdk::CoordType::WORLD; + } else if (request->frame == "tcp" || request->frame == "TCP") { + frame = flexiv::rdk::CoordType::TCP; + } else { + response->success = false; + response->message = "Invalid frame. Use 'world' or 'tcp'"; + return; + } + + robot_->SetForceControlFrame(frame); + + response->success = true; + response->message = "Force control frame set to: " + request->frame; + RCLCPP_INFO(getLogger(), "SetForceControlFrame: %s", request->frame.c_str()); + } catch (const std::exception& e) { + response->success = false; + response->message = std::string("Failed to set force control frame: ") + e.what(); + RCLCPP_ERROR(getLogger(), "%s", response->message.c_str()); + } +} + +void FlexivHardwareInterface::setForceControlAxisCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + try { + if (robot_->mode() != flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE) { + response->success = false; + response->message = "Robot is not in Cartesian motion-force control mode"; + return; + } + + std::array force_axes; + for (size_t i = 0; i < kCartDoF; i++) { + force_axes[i] = request->enable_force_control[i]; + } + + robot_->SetForceControlAxis(force_axes); + + response->success = true; + response->message = "Force control axes set successfully"; + RCLCPP_INFO(getLogger(), "SetForceControlAxis: [%s, %s, %s, %s, %s, %s]", + force_axes[0] ? "force" : "motion", force_axes[1] ? "force" : "motion", + force_axes[2] ? "force" : "motion", force_axes[3] ? "force" : "motion", + force_axes[4] ? "force" : "motion", force_axes[5] ? "force" : "motion"); + } catch (const std::exception& e) { + response->success = false; + response->message = std::string("Failed to set force control axes: ") + e.what(); + RCLCPP_ERROR(getLogger(), "%s", response->message.c_str()); + } +} + } /* namespace flexiv_hardware */ #include "pluginlib/class_list_macros.hpp" diff --git a/flexiv_msgs/CMakeLists.txt b/flexiv_msgs/CMakeLists.txt index 8e0206b..42d09fc 100644 --- a/flexiv_msgs/CMakeLists.txt +++ b/flexiv_msgs/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) set(msg_files + msg/CartesianMotionForceCommand.msg msg/Digital.msg msg/GPIOStates.msg msg/JointPosVel.msg @@ -15,8 +16,13 @@ set(msg_files msg/RobotStates.msg ) -# set(srv_files -# ) +set(srv_files + srv/SetCartesianImpedance.srv + srv/SetNullSpacePosture.srv + srv/SetMaxContactWrench.srv + srv/SetForceControlFrame.srv + srv/SetForceControlAxis.srv +) set(action_files action/Grasp.action @@ -25,7 +31,7 @@ set(action_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} - # ${srv_files} + ${srv_files} ${action_files} DEPENDENCIES std_msgs diff --git a/flexiv_msgs/msg/CartesianMotionForceCommand.msg b/flexiv_msgs/msg/CartesianMotionForceCommand.msg new file mode 100644 index 0000000..681884c --- /dev/null +++ b/flexiv_msgs/msg/CartesianMotionForceCommand.msg @@ -0,0 +1,19 @@ +# Cartesian motion-force command for Flexiv robots +# Used with RT_CARTESIAN_MOTION_FORCE mode (real-time Cartesian motion-force control) + +std_msgs/Header header + +# Target TCP pose in world frame [x, y, z, qw, qx, qy, qz] +geometry_msgs/Pose target_pose + +# Target TCP wrench (force and moment) in force control reference frame +# [fx, fy, fz, mx, my, mz] Unit: [N]:[Nm] +geometry_msgs/Wrench target_wrench + +# Target TCP velocity (linear and angular) in world frame +# [vx, vy, vz, wx, wy, wz] Unit: [m/s]:[rad/s] +geometry_msgs/Twist target_velocity + +# Target TCP acceleration (linear and angular) in world frame +# [ax, ay, az, alpha_x, alpha_y, alpha_z] Unit: [m/s^2]:[rad/s^2] +geometry_msgs/Accel target_acceleration diff --git a/flexiv_msgs/srv/SetCartesianImpedance.srv b/flexiv_msgs/srv/SetCartesianImpedance.srv new file mode 100644 index 0000000..94c2f7c --- /dev/null +++ b/flexiv_msgs/srv/SetCartesianImpedance.srv @@ -0,0 +1,9 @@ +# Set Cartesian impedance properties +# stiffness: [kx, ky, kz, kRx, kRy, kRz] in [N/m]:[Nm/rad] +# damping_ratio: [zx, zy, zz, zRx, zRy, zRz], valid range [0.3, 0.8] + +float64[6] stiffness +float64[6] damping_ratio +--- +bool success +string message diff --git a/flexiv_msgs/srv/SetForceControlAxis.srv b/flexiv_msgs/srv/SetForceControlAxis.srv new file mode 100644 index 0000000..871f21c --- /dev/null +++ b/flexiv_msgs/srv/SetForceControlAxis.srv @@ -0,0 +1,6 @@ +# Set which Cartesian axes are force-controlled (true) or motion-controlled (false) +# Order: [Fx, Fy, Fz, Mx, My, Mz] +bool[6] enable_force_control +--- +bool success +string message diff --git a/flexiv_msgs/srv/SetForceControlFrame.srv b/flexiv_msgs/srv/SetForceControlFrame.srv new file mode 100644 index 0000000..1da7c2d --- /dev/null +++ b/flexiv_msgs/srv/SetForceControlFrame.srv @@ -0,0 +1,6 @@ +# Set the reference frame for force control +# Options: "world" or "tcp" +string frame +--- +bool success +string message diff --git a/flexiv_msgs/srv/SetMaxContactWrench.srv b/flexiv_msgs/srv/SetMaxContactWrench.srv new file mode 100644 index 0000000..79043b2 --- /dev/null +++ b/flexiv_msgs/srv/SetMaxContactWrench.srv @@ -0,0 +1,8 @@ +# Set maximum contact wrench for motion control +# max_wrench: [fx, fy, fz, mx, my, mz] in [N]:[Nm] +# Set to infinity to disable regulation + +float64[6] max_wrench +--- +bool success +string message diff --git a/flexiv_msgs/srv/SetNullSpacePosture.srv b/flexiv_msgs/srv/SetNullSpacePosture.srv new file mode 100644 index 0000000..82e953f --- /dev/null +++ b/flexiv_msgs/srv/SetNullSpacePosture.srv @@ -0,0 +1,7 @@ +# Set reference joint positions for null-space posture control +# ref_positions: [rad] for each joint + +float64[7] ref_positions +--- +bool success +string message