diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..32d9d60 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,45 @@ +# To run checks over all the files in the repo manually: +# +# pre-commit run -a +# +# Or run checks automatically every time before commit: +# +# pre-commit install +# +# See https://pre-commit.com for more information +# See https://pre-commit.com/hooks.html for more hooks +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + args: [--allow-multiple-documents] + - id: check-added-large-files + args: [--maxkb=500] + - id: pretty-format-json + args: [--no-sort-keys, --autofix, --indent=4] + + - repo: https://github.com/myint/docformatter + rev: v1.7.5 + hooks: + - id: docformatter + args: [--in-place] + + - repo: https://github.com/psf/black + rev: 23.9.1 + hooks: + - id: black + language_version: python3 + args: [--line-length=88] + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format -i + language: system + files: \.(c|cc|cxx|cpp|cu|h|hh|hpp|hxx|java|js|m|proto)$ + args: ["-fallback-style=none"] diff --git a/README.md b/README.md index ac13bbf..276fb57 100644 --- a/README.md +++ b/README.md @@ -76,16 +76,17 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.0 source install/setup.bash ``` -**NOTE**: Remember to source the setup file and the workspace whenever a new terminal is opened: - -```bash -source /opt/ros/humble/setup.bash -source ~/flexiv_ros2_ws/install/setup.bash -``` +> [!NOTE] +> Remember to source the setup file and the workspace whenever a new terminal is opened: +> ```bash +> source /opt/ros/humble/setup.bash +> source ~/flexiv_ros2_ws/install/setup.bash +> ``` ## Usage -**NOTE**: the instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://rdk.flexiv.com/manual/ros2_packages.html) for more information. +> [!NOTE] +> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://rdk.flexiv.com/manual/ros2_packages.html) for more information. The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the robot server](https://rdk.flexiv.com/manual/getting_started.html#activate-rdk-server) and [establish connection](https://rdk.flexiv.com/manual/getting_started.html#establish-connection) between the workstation PC and the robot. @@ -111,13 +112,15 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa ros2 launch flexiv_bringup rizon.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip] rizon_type:=rizon4 ``` - **NOTE**: To test whether the connection between ROS and the robot is established, you could disable the starting of RViz first by setting the `start_rviz` launch argument to false. - Test with fake hardware (`ros2_control` capability): ```bash ros2 launch flexiv_bringup rizon.launch.py robot_ip:=dont-care local_ip:=dont-care use_fake_hardware:=true ``` +> [!TIP] +> To test whether the connection between ROS and the robot is established, you could disable the starting of RViz first by setting the `start_rviz` launch argument to false. + 2. Publish commands to controllers - To send the goal position to the controller by using the node from `flexiv_test_nodes`, start the following command in a new terminal: @@ -133,8 +136,6 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa ros2 launch flexiv_bringup rizon.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip] robot_controller:=joint_impedance_controller ``` - **NOTE**: The command starts the robot in the joint torque mode. In this mode, gravity and friction are compensated **only** for the robot **without** any attached objects (e.g. the gripper, camera). - Open a new terminal and run the launch file: ```bash @@ -143,7 +144,11 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa The robot should run a sine-sweep motion with joint impedance control. - **NOTE**: joint impedance control is not supported in fake/simulated hardware. +> [!NOTE] +> The command starts the robot in the joint torque mode. In this mode, gravity and friction are compensated **only** for the robot **without** any attached objects (e.g. the gripper, camera). + +> [!NOTE] +> Joint impedance control is not supported in fake/simulated hardware. ### Using MoveIt @@ -169,4 +174,15 @@ The robot driver (`rizon.launch.py`) publishes the following feedback states to - `/force_torque_sensor_broadcaster/wrench`: Force-torque (FT) sensor raw reading in flange frame: $^{flange}F_{raw}$ in force $[N]$ and moment $[Nm]$. The value is 0 if no FT sensor is installed. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] - `/tcp_pose_broadcaster/tcp_pose`: Measured TCP pose expressed in base frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)] -**NOTE**: The topic names of the broadcasters are specified in `flexiv_bringup/config/rizon_controllers.yaml` +> [!NOTE] +> The topic names of the broadcasters are specified in `flexiv_bringup/config/rizon_controllers.yaml` + +### GPIO + +All digital inputs on the robot control box can be accessed via the ROS topic `/gpio_controller/gpio_inputs`, which publishes the current state of all the 16 digital input ports *(True: port high, false: port low)*. + +The digital output ports on the control box can be set by publishing to the topic `/gpio_controller/gpio_outputs`. For example: + +```bash +ros2 topic pub /gpio_controller/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}" +``` diff --git a/flexiv_bringup/config/rizon_controllers.yaml b/flexiv_bringup/config/rizon_controllers.yaml index a97015d..9aeda6c 100644 --- a/flexiv_bringup/config/rizon_controllers.yaml +++ b/flexiv_bringup/config/rizon_controllers.yaml @@ -5,8 +5,8 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController - joint_impedance_controller: - type: flexiv_controllers/JointImpedanceController + rizon_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -14,18 +14,21 @@ controller_manager: force_torque_sensor_broadcaster: type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + joint_impedance_controller: + type: flexiv_controllers/JointImpedanceController + + gpio_controller: + type: flexiv_controllers/GPIOController + external_wrench_in_base_broadcaster: type: flexiv_controllers/ExternalTcpWrenchBroadcaster external_wrench_in_tcp_broadcaster: type: flexiv_controllers/ExternalTcpWrenchBroadcaster - + tcp_pose_state_broadcaster: type: flexiv_controllers/TcpPoseStateBroadcaster - rizon_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - force_torque_sensor_broadcaster: ros__parameters: sensor_name: force_torque_sensor @@ -40,7 +43,7 @@ external_wrench_in_tcp_broadcaster: external_wrench_in_base_broadcaster: ros__parameters: sensor_name: external_wrench_in_base - frame_id: base_link + frame_id: flange topic_name: external_wrench_in_base tcp_pose_state_broadcaster: @@ -91,7 +94,6 @@ rizon_arm_controller: state_publish_rate: 100.0 action_monitor_rate: 20.0 allow_partial_joints_goal: false - allow_integration_in_goal_trajectories: true constraints: stopped_velocity_tolerance: 0.01 goal_time: 0.0 diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py index 8cfb98a..b5e1212 100644 --- a/flexiv_bringup/launch/rizon.launch.py +++ b/flexiv_bringup/launch/rizon.launch.py @@ -2,7 +2,12 @@ from launch.actions import DeclareLaunchArgument, RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -118,7 +123,7 @@ def generate_launch_description(): ) robot_description = {"robot_description": robot_description_content} - # RViZ + # RViZ rviz_config_file = PathJoinSubstitution( [FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"] ) @@ -165,37 +170,64 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) - + # Run force torque sensor broadcaster force_torque_sensor_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["force_torque_sensor_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "force_torque_sensor_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) - + # Run external wrench in base broadcaster external_wrench_in_base_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["external_wrench_in_base_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "external_wrench_in_base_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) - + # Run external wrench in tcp broadcaster external_wrench_in_tcp_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["external_wrench_in_tcp_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "external_wrench_in_tcp_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) - + # Run tcp pose state broadcaster tcp_pose_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["tcp_pose_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "tcp_pose_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + # Run gpio controller + gpio_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["gpio_controller", "--controller-manager", "/controller_manager"], ) - + # Delay rviz start after `joint_state_broadcaster` delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( @@ -203,12 +235,14 @@ def generate_launch_description(): on_exit=[rviz_node], ) ) - + # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) ) ) @@ -220,6 +254,7 @@ def generate_launch_description(): external_wrench_in_base_broadcaster_spawner, external_wrench_in_tcp_broadcaster_spawner, tcp_pose_state_broadcaster_spawner, + gpio_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] diff --git a/flexiv_bringup/launch/rizon_moveit.launch.py b/flexiv_bringup/launch/rizon_moveit.launch.py index 12e12c9..344c2e7 100644 --- a/flexiv_bringup/launch/rizon_moveit.launch.py +++ b/flexiv_bringup/launch/rizon_moveit.launch.py @@ -5,7 +5,12 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -17,7 +22,9 @@ def load_yaml(package_name, file_path): try: with open(absolute_file_path, "r") as file: return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except ( + EnvironmentError + ): # parent of IOError, OSError *and* WindowsError where available return None @@ -47,7 +54,7 @@ def generate_launch_description(): description="IP address of the workstation PC (local).", ) ) - + declared_arguments.append( DeclareLaunchArgument( "start_rviz", @@ -72,7 +79,7 @@ def generate_launch_description(): Used only if 'use_fake_hardware' parameter is true.", ) ) - + declared_arguments.append( DeclareLaunchArgument( "warehouse_sqlite_path", @@ -81,6 +88,14 @@ def generate_launch_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + "start_servo", + default_value="false", + description="Start the MoveIt servo node?", + ) + ) + rizon_type = LaunchConfiguration("rizon_type") robot_ip = LaunchConfiguration("robot_ip") local_ip = LaunchConfiguration("local_ip") @@ -88,6 +103,7 @@ def generate_launch_description(): use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path") + start_servo = LaunchConfiguration("start_servo") # Get URDF via xacro flexiv_urdf_xacro = PathJoinSubstitution( @@ -125,7 +141,7 @@ def generate_launch_description(): flexiv_srdf_xacro = PathJoinSubstitution( [FindPackageShare("flexiv_moveit_config"), "srdf", "rizon.srdf.xacro"] ) - + robot_description_semantic_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -243,12 +259,12 @@ def generate_launch_description(): output="both", parameters=[robot_description], ) - + # Robot controllers robot_controllers = PathJoinSubstitution( [FindPackageShare("flexiv_bringup"), "config", "rizon_controllers.yaml"] ) - + # Run controller manager ros2_control_node = Node( package="controller_manager", @@ -256,19 +272,45 @@ def generate_launch_description(): parameters=[robot_description, robot_controllers], output="both", ) - + # Run robot controller robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["rizon_arm_controller", "--controller-manager", "/controller_manager"], + arguments=[ + "rizon_arm_controller", + "--controller-manager", + "/controller_manager", + ], ) - + # Run joint state broadcaster joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + # Servo node for realtime control + servo_yaml = load_yaml( + "flexiv_moveit_config", "config/rizon_moveit_servo_config.yaml" + ) + servo_params = {"moveit_servo": servo_yaml} + servo_node = Node( + package="moveit_servo", + condition=IfCondition(start_servo), + executable="servo_node_main", + parameters=[ + servo_params, + robot_description, + robot_description_semantic, + robot_description_kinematics, + ], + output="screen", ) nodes = [ @@ -278,6 +320,7 @@ def generate_launch_description(): joint_state_broadcaster_spawner, robot_controller_spawner, rviz_node, + servo_node, ] return LaunchDescription(declared_arguments + nodes) diff --git a/flexiv_bringup/package.xml b/flexiv_bringup/package.xml index a9b8594..4c51cb1 100644 --- a/flexiv_bringup/package.xml +++ b/flexiv_bringup/package.xml @@ -2,7 +2,7 @@ flexiv_bringup - 0.8.0 + 0.9.1 Package with launch files and run-time configurations for Flexiv robots with `ros2_control` Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_controllers/CMakeLists.txt b/flexiv_controllers/CMakeLists.txt index aaeacd2..a6d2090 100644 --- a/flexiv_controllers/CMakeLists.txt +++ b/flexiv_controllers/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(${PROJECT_NAME} src/joint_impedance_controller.cpp src/external_tcp_wrench_broadcaster.cpp src/tcp_pose_state_broadcaster.cpp + src/gpio_controller.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE include) diff --git a/flexiv_controllers/flexiv_controllers_plugins.xml b/flexiv_controllers/flexiv_controllers_plugins.xml index eb80f48..f9e44b1 100644 --- a/flexiv_controllers/flexiv_controllers_plugins.xml +++ b/flexiv_controllers/flexiv_controllers_plugins.xml @@ -17,4 +17,10 @@ The TCP pose state broadcaster publishes the measured TCP pose expressed in base frame. + + + The GPIO controller publishes the GPIO states. + + diff --git a/flexiv_controllers/include/flexiv_controllers/gpio_controller.hpp b/flexiv_controllers/include/flexiv_controllers/gpio_controller.hpp new file mode 100644 index 0000000..9b5259d --- /dev/null +++ b/flexiv_controllers/include/flexiv_controllers/gpio_controller.hpp @@ -0,0 +1,57 @@ +/** + * @file gpio_controller.hpp + * @brief GPIO controller as ROS 2 controller. Adapted from + * ros2_control_demos/example_10/gpio_controller + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#ifndef FLEXIV_CONTROLLERS__GPIO_CONTROLLER_HPP_ +#define FLEXIV_CONTROLLERS__GPIO_CONTROLLER_HPP_ + +#include +#include +#include + +#include +#include "flexiv_msgs/msg/gpio_states.hpp" + +namespace flexiv_controllers { +using CmdType = flexiv_msgs::msg::GPIOStates; + +class GPIOController : public controller_interface::ControllerInterface +{ +public: + GPIOController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update( + const rclcpp::Time& time, const rclcpp::Duration& period) 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; + +protected: + void initMsgs(); + + // internal commands + std::array digital_outputs_cmd_; + + // publisher + std::shared_ptr> gpio_inputs_publisher_; + CmdType gpio_inputs_msg_; + + // subscriber + rclcpp::Subscription::SharedPtr gpio_outputs_command_; +}; + +} /* namespace flexiv_controllers */ +#endif /* FLEXIV_CONTROLLERS__GPIO_CONTROLLER_HPP_ */ diff --git a/flexiv_controllers/package.xml b/flexiv_controllers/package.xml index 27a50e3..a3ff3ba 100644 --- a/flexiv_controllers/package.xml +++ b/flexiv_controllers/package.xml @@ -2,7 +2,7 @@ flexiv_controllers - 0.8.0 + 0.9.1 Flexiv custom ros2 controllers Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_controllers/src/gpio_controller.cpp b/flexiv_controllers/src/gpio_controller.cpp new file mode 100644 index 0000000..184a9ef --- /dev/null +++ b/flexiv_controllers/src/gpio_controller.cpp @@ -0,0 +1,131 @@ +/** + * @file gpio_controller.cpp + * @brief GPIO controller as ROS 2 controller. Adapted from + * ros2_control_demos/example_10/gpio_controller + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include "flexiv_controllers/gpio_controller.hpp" + +#include + +namespace flexiv_controllers { + +GPIOController::GPIOController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn GPIOController::on_init() +{ + try { + initMsgs(); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration GPIOController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (size_t i = 0; i < 16; ++i) { + config.names.emplace_back("gpio/digital_output_" + std::to_string(i)); + } + + return config; +} + +controller_interface::InterfaceConfiguration GPIOController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (size_t i = 0; i < 16; ++i) { + config.names.emplace_back("gpio/digital_input_" + std::to_string(i)); + } + + return config; +} + +controller_interface::return_type GPIOController::update( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +{ + // get inputs + for (size_t i = 0; i < 16; ++i) { + gpio_inputs_msg_.states[i].pin = i; + gpio_inputs_msg_.states[i].state = static_cast(state_interfaces_[i].get_value()); + } + gpio_inputs_publisher_->publish(gpio_inputs_msg_); + + // set outputs + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + command_interfaces_[i].set_value(digital_outputs_cmd_[i]); + } + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn GPIOController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) +{ + try { + // register publisher + gpio_inputs_publisher_ + = get_node()->create_publisher("~/gpio_inputs", rclcpp::SystemDefaultsQoS()); + + // register subscriber + gpio_outputs_command_ = get_node()->create_subscription( + "~/gpio_outputs", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { + for (size_t i = 0; i < msg->states.size(); ++i) { + if (msg->states[i].pin >= 16) { + RCLCPP_WARN(get_node()->get_logger(), + "Received command for pin %d, but only pins 0-15 are supported.", + msg->states[i].pin); + continue; + } else { + digital_outputs_cmd_[msg->states[i].pin] + = static_cast(msg->states[i].state); + } + } + }); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +void GPIOController::initMsgs() +{ + gpio_inputs_msg_.states.resize(digital_outputs_cmd_.size()); + digital_outputs_cmd_.fill(0.0); +} + +controller_interface::CallbackReturn GPIOController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GPIOController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) +{ + try { + // reset publisher + gpio_inputs_publisher_.reset(); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace flexiv_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + flexiv_controllers::GPIOController, controller_interface::ControllerInterface) diff --git a/flexiv_description/package.xml b/flexiv_description/package.xml index 7ed6b1f..e8dcfbf 100644 --- a/flexiv_description/package.xml +++ b/flexiv_description/package.xml @@ -2,7 +2,7 @@ flexiv_description - 0.8.0 + 0.9.1 URDF description for Flexiv robots Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_description/urdf/rizon10_joints_links.xacro b/flexiv_description/urdf/rizon10_joints_links.xacro index fdfd41a..2bdf2ad 100644 --- a/flexiv_description/urdf/rizon10_joints_links.xacro +++ b/flexiv_description/urdf/rizon10_joints_links.xacro @@ -86,7 +86,7 @@ - + @@ -107,7 +107,7 @@ - + @@ -128,7 +128,7 @@ - + @@ -149,7 +149,7 @@ - + @@ -170,7 +170,7 @@ - + @@ -191,7 +191,7 @@ - + @@ -212,7 +212,7 @@ - + @@ -233,7 +233,7 @@ - + diff --git a/flexiv_description/urdf/rizon4.ros2_control.xacro b/flexiv_description/urdf/rizon4.ros2_control.xacro index fa24f3a..f93ad98 100644 --- a/flexiv_description/urdf/rizon4.ros2_control.xacro +++ b/flexiv_description/urdf/rizon4.ros2_control.xacro @@ -21,6 +21,7 @@ flexiv_hardware/FlexivHardwareInterface ${robot_ip} ${local_ip} + ${prefix} @@ -179,6 +180,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flexiv_description/urdf/rizon4_joints_links.xacro b/flexiv_description/urdf/rizon4_joints_links.xacro index 4a45a04..a11882d 100644 --- a/flexiv_description/urdf/rizon4_joints_links.xacro +++ b/flexiv_description/urdf/rizon4_joints_links.xacro @@ -91,7 +91,7 @@ - + @@ -112,7 +112,7 @@ - + @@ -133,7 +133,7 @@ - + @@ -154,7 +154,7 @@ - + @@ -175,7 +175,7 @@ - + @@ -196,7 +196,7 @@ - + @@ -217,7 +217,7 @@ - + @@ -239,7 +239,7 @@ - + @@ -259,7 +259,7 @@ - + diff --git a/flexiv_hardware/CMakeLists.txt b/flexiv_hardware/CMakeLists.txt index 9d51615..1492c12 100644 --- a/flexiv_hardware/CMakeLists.txt +++ b/flexiv_hardware/CMakeLists.txt @@ -35,21 +35,21 @@ message("OS: ${CMAKE_SYSTEM_NAME}") message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libFlexivRdk.x86_64-linux-gnu.a") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a") elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libFlexivRdk.aarch64-linux-gnu.a") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a") else() message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libFlexivRdk.arm64-darwin.a") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libflexiv_rdk.arm64-darwin.a") else() message(FATAL_ERROR "Mac with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/FlexivRdk.win_amd64.lib") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/flexiv_rdk.win_amd64.lib") else() message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp index 2b6e36c..836e372 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp @@ -92,7 +92,6 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface std::vector hw_commands_joint_positions_; std::vector hw_commands_joint_velocities_; std::vector hw_commands_joint_efforts_; - std::vector internal_commands_joint_positions_; // Joint states std::vector hw_states_joint_positions_; @@ -112,6 +111,10 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface // Measured TCP pose expressed in base frame [x, y, z, qx, qy, qz, qw]. std::vector hw_states_tcp_pose_; + // GPIO commands and states + std::vector hw_commands_gpio_out_; + std::vector hw_states_gpio_in_; + static rclcpp::Logger getLogger(); // control modes @@ -121,11 +124,6 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface bool position_controller_running_; bool velocity_controller_running_; bool torque_controller_running_; - - // Store time between update loops - rclcpp::Clock clock_; - rclcpp::Time last_timestamp_; - rclcpp::Time current_timestamp_; }; } /* namespace flexiv_hardware */ diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml index f584cd4..d296d7d 100644 --- a/flexiv_hardware/package.xml +++ b/flexiv_hardware/package.xml @@ -2,7 +2,7 @@ flexiv_hardware - 0.8.0 + 0.9.1 Hardware interfaces to Flexiv robots for ROS 2 control Mun Seng Phoon Apache 2.0 diff --git a/flexiv_hardware/rdk b/flexiv_hardware/rdk index a9931cb..cfe44b9 160000 --- a/flexiv_hardware/rdk +++ b/flexiv_hardware/rdk @@ -1 +1 @@ -Subproject commit a9931cbd3cb99a74766e9ec3f22e3686ad3e09d9 +Subproject commit cfe44b94c092b5b385d7bfa9b063bcee0ba43959 diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index 1b197cc..fed6966 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -46,8 +46,8 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( hw_states_external_wrench_in_tcp_.resize( info_.sensors[2].state_interfaces.size(), std::numeric_limits::quiet_NaN()); hw_states_tcp_pose_.resize(7, std::numeric_limits::quiet_NaN()); - internal_commands_joint_positions_.resize( - info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_states_gpio_in_.resize(16, std::numeric_limits::quiet_NaN()); + hw_commands_gpio_out_.resize(16, std::numeric_limits::quiet_NaN()); stop_modes_ = {StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE}; @@ -144,8 +144,6 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - clock_ = rclcpp::Clock(); - RCLCPP_INFO(getLogger(), "Successfully connected to robot"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -188,6 +186,12 @@ std::vector FlexivHardwareInterface::export_ } } + const std::string prefix = info_.hardware_parameters.at("prefix"); + for (std::size_t i = 0; i < 16; i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + prefix + "gpio", "digital_input_" + std::to_string(i), &hw_states_gpio_in_[i])); + } + return state_interfaces; } @@ -206,6 +210,12 @@ FlexivHardwareInterface::export_command_interfaces() hardware_interface::HW_IF_EFFORT, &hw_commands_joint_efforts_[i])); } + const std::string prefix = info_.hardware_parameters.at("prefix"); + for (size_t i = 0; i < 16; i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + prefix + "gpio", "digital_output_" + std::to_string(i), &hw_commands_gpio_out_[i])); + } + return command_interfaces; } @@ -214,23 +224,38 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate( { RCLCPP_INFO(getLogger(), "Starting... please wait..."); - std::this_thread::sleep_for(std::chrono::seconds(2)); + std::this_thread::sleep_for(std::chrono::seconds(1)); try { + // Clear fault on robot server if any + if (robot_->isFault()) { + RCLCPP_WARN(getLogger(), "Fault occurred on robot server, trying to clear ..."); + // Try to clear the fault + robot_->clearFault(); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Check again + if (robot_->isFault()) { + RCLCPP_FATAL(getLogger(), "Fault cannot be cleared, exiting ..."); + return hardware_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO(getLogger(), "Fault on robot server is cleared"); + } + + // Enable the robot + RCLCPP_INFO(getLogger(), "Enabling robot ..."); robot_->enable(); + + // Wait for the robot to become operational + while (!robot_->isOperational(false)) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + RCLCPP_INFO(getLogger(), "Robot is now operational"); } catch (const flexiv::Exception& e) { RCLCPP_FATAL(getLogger(), "Could not enable robot."); RCLCPP_FATAL(getLogger(), e.what()); return hardware_interface::CallbackReturn::ERROR; } - // Wait for the robot to become operational - while (!robot_->isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - RCLCPP_INFO(getLogger(), "Robot is now operational"); - - last_timestamp_ = clock_.now(); RCLCPP_INFO(getLogger(), "System successfully started!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -241,7 +266,7 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate( { RCLCPP_INFO(getLogger(), "Stopping... please wait..."); - std::this_thread::sleep_for(std::chrono::seconds(2)); + std::this_thread::sleep_for(std::chrono::seconds(1)); robot_->stop(); robot_->disconnect(); @@ -255,13 +280,12 @@ hardware_interface::return_type FlexivHardwareInterface::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { flexiv::RobotStates robot_states; - if (robot_->isOperational() && robot_->getMode() != flexiv::Mode::IDLE) { + if (robot_->isOperational(false) && robot_->getMode() != flexiv::Mode::IDLE) { robot_->getRobotStates(robot_states); hw_states_joint_positions_ = robot_states.q; hw_states_joint_velocities_ = robot_states.dtheta; hw_states_joint_efforts_ = robot_states.tau; - internal_commands_joint_positions_ = hw_states_joint_positions_; hw_states_force_torque_sensor_ = robot_states.ftSensorRaw; hw_states_external_wrench_in_base_ = robot_states.extWrenchInBase; @@ -275,6 +299,12 @@ hardware_interface::return_type FlexivHardwareInterface::read( hw_states_tcp_pose_[4] = robot_states.tcpPose[5]; hw_states_tcp_pose_[5] = robot_states.tcpPose[6]; hw_states_tcp_pose_[6] = robot_states.tcpPose[3]; + + // Read GPIO input states + auto gpio_in = robot_->readDigitalInput(); + for (size_t i = 0; i < hw_states_gpio_in_.size(); i++) { + hw_states_gpio_in_[i] = static_cast(gpio_in[i]); + } } return hardware_interface::return_type::OK; @@ -283,10 +313,6 @@ hardware_interface::return_type FlexivHardwareInterface::read( hardware_interface::return_type FlexivHardwareInterface::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - current_timestamp_ = clock_.now(); - rclcpp::Duration duration = current_timestamp_ - last_timestamp_; - last_timestamp_ = current_timestamp_; - std::vector targetAcceleration(n_joints, 0); std::vector targetVelocity(n_joints, 0); std::fill(targetAcceleration.begin(), targetAcceleration.end(), 0.0); @@ -296,12 +322,15 @@ hardware_interface::return_type FlexivHardwareInterface::write( bool isNanVel = false; bool isNanEff = false; for (std::size_t i = 0; i < n_joints; i++) { - if (hw_commands_joint_positions_[i] != hw_commands_joint_positions_[i]) + if (hw_commands_joint_positions_[i] != hw_commands_joint_positions_[i]) { isNanPos = true; - if (hw_commands_joint_velocities_[i] != hw_commands_joint_velocities_[i]) + } + if (hw_commands_joint_velocities_[i] != hw_commands_joint_velocities_[i]) { isNanVel = true; - if (hw_commands_joint_efforts_[i] != hw_commands_joint_efforts_[i]) + } + if (hw_commands_joint_efforts_[i] != hw_commands_joint_efforts_[i]) { isNanEff = true; + } } if (position_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION @@ -310,17 +339,26 @@ hardware_interface::return_type FlexivHardwareInterface::write( hw_commands_joint_positions_, targetVelocity, targetAcceleration); } else if (velocity_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION && !isNanVel) { - for (std::size_t i = 0; i < n_joints; i++) { - internal_commands_joint_positions_[i] - += hw_commands_joint_velocities_[i] * duration.seconds(); - } robot_->streamJointPosition( - internal_commands_joint_positions_, hw_commands_joint_velocities_, targetAcceleration); + hw_states_joint_positions_, hw_commands_joint_velocities_, targetAcceleration); } else if (torque_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_TORQUE && !isNanEff) { robot_->streamJointTorque(hw_commands_joint_efforts_, true, true); } + // Write digital output + std::vector ports_indices; + std::vector ports_values; + for (size_t i = 0; i < hw_commands_gpio_out_.size(); i++) { + if (hw_commands_gpio_out_[i] != hw_commands_gpio_out_[i]) { + continue; + } + ports_indices.push_back(i); + ports_values.push_back(static_cast(hw_commands_gpio_out_[i])); + } + + robot_->writeDigitalOutput(ports_indices, ports_values); + return hardware_interface::return_type::OK; } diff --git a/flexiv_moveit_config/config/rizon_moveit_servo_config.yaml b/flexiv_moveit_config/config/rizon_moveit_servo_config.yaml new file mode 100644 index 0000000..41dc4d7 --- /dev/null +++ b/flexiv_moveit_config/config/rizon_moveit_servo_config.yaml @@ -0,0 +1,71 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + +## MoveIt properties +move_group_name: rizon_arm # Often 'manipulator' or 'arm' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: flange # The name of the end effector link, used to return the EE pose +robot_link_command_frame: flange # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /rizon_arm_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/flexiv_moveit_config/package.xml b/flexiv_moveit_config/package.xml index d118bc9..360e608 100644 --- a/flexiv_moveit_config/package.xml +++ b/flexiv_moveit_config/package.xml @@ -2,7 +2,7 @@ flexiv_moveit_config - 0.8.0 + 0.9.1 MoveIt2 configuration and launch files for Flexiv Rizon robots Mun Seng Phoon Apache 2.0 @@ -18,6 +18,8 @@ moveit_kinematics moveit_planners_ompl moveit_ros_visualization + moveit_servo + moveit_simple_controller_manager rviz2 flexiv_description urdf diff --git a/flexiv_moveit_config/srdf/rizon_macro.srdf.xacro b/flexiv_moveit_config/srdf/rizon_macro.srdf.xacro index 84417f7..20b4a7a 100644 --- a/flexiv_moveit_config/srdf/rizon_macro.srdf.xacro +++ b/flexiv_moveit_config/srdf/rizon_macro.srdf.xacro @@ -15,8 +15,6 @@ - - @@ -38,5 +36,6 @@ + diff --git a/flexiv_msgs/CMakeLists.txt b/flexiv_msgs/CMakeLists.txt index b284188..58022c6 100644 --- a/flexiv_msgs/CMakeLists.txt +++ b/flexiv_msgs/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(geometry_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) set(msg_files + msg/Digital.msg + msg/GPIOStates.msg msg/JointPosVel.msg msg/Mode.msg msg/RobotStates.msg diff --git a/flexiv_msgs/msg/Digital.msg b/flexiv_msgs/msg/Digital.msg new file mode 100644 index 0000000..6183945 --- /dev/null +++ b/flexiv_msgs/msg/Digital.msg @@ -0,0 +1,2 @@ +uint8 pin +bool state diff --git a/flexiv_msgs/msg/GPIOStates.msg b/flexiv_msgs/msg/GPIOStates.msg new file mode 100644 index 0000000..f3cbe41 --- /dev/null +++ b/flexiv_msgs/msg/GPIOStates.msg @@ -0,0 +1 @@ +Digital[] states diff --git a/flexiv_msgs/package.xml b/flexiv_msgs/package.xml index 49a0470..27a2614 100644 --- a/flexiv_msgs/package.xml +++ b/flexiv_msgs/package.xml @@ -2,7 +2,7 @@ flexiv_msgs - 0.8.0 + 0.9.1 Robot states messages definiton used in RDK Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_test_nodes/package.xml b/flexiv_test_nodes/package.xml index d76155b..02e1ac0 100644 --- a/flexiv_test_nodes/package.xml +++ b/flexiv_test_nodes/package.xml @@ -2,7 +2,7 @@ flexiv_test_nodes - 0.8.0 + 0.9.1 Demo nodes for testing flexiv_ros2 Mun Seng Phoon Apache-2.0