From 26d3d7dcfdde046a8efe9d01688795b1efe63d1e Mon Sep 17 00:00:00 2001 From: munseng Date: Tue, 12 Dec 2023 14:40:12 +0800 Subject: [PATCH 1/8] Update RDK to v0.9 --- flexiv_hardware/rdk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flexiv_hardware/rdk b/flexiv_hardware/rdk index a9931cb..00cd9c7 160000 --- a/flexiv_hardware/rdk +++ b/flexiv_hardware/rdk @@ -1 +1 @@ -Subproject commit a9931cbd3cb99a74766e9ec3f22e3686ad3e09d9 +Subproject commit 00cd9c702d4797b8079c5d22de5b3c6e0fca5cba From d09ad5490fbb048c9f525dbdb6e08a364b39a70d Mon Sep 17 00:00:00 2001 From: munseng Date: Tue, 12 Dec 2023 14:54:18 +0800 Subject: [PATCH 2/8] Update flake8 in pre-commit --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index bf81426..195c105 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -38,8 +38,8 @@ repos: language_version: python3 args: [--line-length=88] -- repo: https://gitlab.com/pycqa/flake8 - rev: 3.7.9 +- repo: https://github.com/pycqa/flake8 + rev: 6.0.0 hooks: - id: flake8 args: [--max-line-length=88, --ignore=E402 W503 E203 E501 W504] From bdc408c3bdb81500a6146c36ad09fb7abddadf0d Mon Sep 17 00:00:00 2001 From: munseng Date: Tue, 12 Dec 2023 14:54:49 +0800 Subject: [PATCH 3/8] Update RDK static lib path --- flexiv_hardware/CMakeLists.txt | 35 ++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/flexiv_hardware/CMakeLists.txt b/flexiv_hardware/CMakeLists.txt index 0b3ab4d..a8157c0 100644 --- a/flexiv_hardware/CMakeLists.txt +++ b/flexiv_hardware/CMakeLists.txt @@ -4,8 +4,6 @@ project(flexiv_hardware) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) -option(BUILD_FOR_ARM64 "Link to RDK library for arm64 processor, otherwise link to x64" OFF) - find_package(catkin REQUIRED COMPONENTS controller_manager geometry_msgs @@ -35,11 +33,29 @@ catkin_package( flexiv_hardware_interface ) -add_library(FlexivRdk SHARED IMPORTED) -if (${BUILD_FOR_ARM64}) - set_target_properties(FlexivRdk PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libFlexivRdk.arm64-darwin.a) -else() - set_target_properties(FlexivRdk PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libFlexivRdk.x86_64-linux-gnu.a) +# Set static library +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/libflexiv_rdk.x86_64-linux-gnu.a") + elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") + 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/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/flexiv_rdk.win_amd64.lib") + else() + message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") + endif() endif() add_library(flexiv_hardware_interface @@ -54,11 +70,10 @@ target_include_directories( ) target_link_libraries(flexiv_hardware_interface PRIVATE - FlexivRdk + ${RDK_STATIC_LIBRARY} ${catkin_LIBRARIES} ) - add_executable(flexiv_hardware_interface_node src/flexiv_hardware_interface.cpp src/flexiv_hardware_interface_node.cpp @@ -72,7 +87,7 @@ target_include_directories( ) target_link_libraries(flexiv_hardware_interface_node PRIVATE - FlexivRdk + ${RDK_STATIC_LIBRARY} ${catkin_LIBRARIES} ) From de97020eab37aacdc057d298b82c854326c851c3 Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 4 Jan 2024 16:41:10 +0800 Subject: [PATCH 4/8] Remove develop branch --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index dab18a8..a536688 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,9 +2,9 @@ name: Industrial CI on: push: - branches: [ main, develop ] + branches: [ main ] pull_request: - branches: [ main, develop ] + branches: [ main ] jobs: industrial_ci: From b9fbf0651e33522c7e991e0235c949f71f825156 Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 4 Jan 2024 16:49:14 +0800 Subject: [PATCH 5/8] Remove internal joint position command in velocity control --- .../include/flexiv_hardware/flexiv_hardware_interface.h | 1 - flexiv_hardware/src/flexiv_hardware_interface.cpp | 8 +------- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h index d1524d7..82cbef5 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h @@ -271,7 +271,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW std::vector joint_position_command_; std::vector joint_velocity_command_; std::vector joint_effort_command_; - std::vector internal_joint_position_command_; // Controller std::atomic position_controller_running_; diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index f72f91f..c4c78d9 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -21,7 +21,6 @@ FlexivHardwareInterface::FlexivHardwareInterface() , joint_position_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) , joint_velocity_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) , joint_effort_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) -, internal_joint_position_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) , ext_wrench_in_tcp_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) , ext_wrench_in_base_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) , ft_sensor_state_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) @@ -248,7 +247,6 @@ void FlexivHardwareInterface::read( joint_position_state_ = robot_states.q; joint_velocity_state_ = robot_states.dtheta; joint_effort_state_ = robot_states.tau; - internal_joint_position_command_ = joint_position_state_; ext_wrench_in_base_ = robot_states.extWrenchInBase; ext_wrench_in_tcp_ = robot_states.extWrenchInTcp; @@ -281,11 +279,7 @@ void FlexivHardwareInterface::write( } else if (velocity_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION && !(vectorHasNan(joint_velocity_command_))) { - for (std::size_t i = 0; i < num_joints_; i++) { - internal_joint_position_command_[i] - += joint_velocity_command_[i] * period.toSec(); - } - robot_->streamJointPosition(internal_joint_position_command_, + robot_->streamJointPosition(joint_position_state_, joint_velocity_command_, target_acceleration); } else if (effort_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_TORQUE From 7752c79aacfba83faa760b4a6e43e90263900fd3 Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 4 Jan 2024 16:58:30 +0800 Subject: [PATCH 6/8] Add fault clearing before enable robot --- .../src/flexiv_hardware_interface.cpp | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index c4c78d9..247210c 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -149,20 +149,36 @@ bool FlexivHardwareInterface::initRobot() return false; } - // Enable the robot, make sure the E-stop is released before enabling try { + // Clear fault on robot server is any + if (robot_->isFault()) { + ROS_WARN("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()) { + ROS_ERROR("Fault cannot be cleared, exiting ..."); + return false; + } + ROS_INFO("Fault on robot server is cleared"); + } + + // Enable the robot + ROS_INFO("Enabling robot ..."); robot_->enable(); + + // Wait for the robot to become operational + while (!robot_->isOperational()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + ROS_INFO("Robot is now operational."); + } catch (const flexiv::Exception& e) { ROS_ERROR("Could not enable robot: %s", e.what()); return false; } - // Wait for the robot to become operational - while (!robot_->isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ROS_INFO("Robot is now operational."); - // get current position and set to initial position setInitPosition(); From fb4dac5f01d0d70c7f75991b621479c5c807566e Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 4 Jan 2024 17:01:01 +0800 Subject: [PATCH 7/8] Update README with alerts --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 759bfd1..6a8bbaa 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,9 @@ RDK APIs are wrapped into ROS packages in `flexiv_ros`. Key functionalities like real-time joint torque and position control, `ros_control` and MoveIt! integrations are implemented. +> [!IMPORTANT] +> For new features and bug fixes, please migrate to ROS 2 and use the [Flexiv ROS 2 Humble](https://github.com/flexivrobotics/flexiv_ros2). + ## References [Flexiv RDK main webpage](https://rdk.flexiv.com/) contains important information like RDK user manual and network setup. @@ -63,7 +66,8 @@ To start the robot driver run the following command in a terminal: $ roslaunch flexiv_bringup rizon_control.launch robot_ip:=[robot_ip] local_ip:=[local_ip] ``` -*Note:* Rizon 4 is the default robot type in the above launch command. If you want to control the Rizon 4s or Rizon 10, add the launch argument e.g. `rizon_type:=rizon4s`. +> [!NOTE] +> Rizon 4 is the default robot type in the above launch command. If you want to control the Rizon 4s or Rizon 10, add the launch argument e.g. `rizon_type:=rizon4s`. In another terminal, start the following launch file to run the rqt: From 69b3128da338b013e1eac7b63e401a249e76475d Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 4 Jan 2024 17:04:29 +0800 Subject: [PATCH 8/8] Bump packages version to 0.9 --- flexiv_bringup/package.xml | 2 +- flexiv_controllers/package.xml | 2 +- flexiv_description/package.xml | 2 +- flexiv_hardware/package.xml | 2 +- flexiv_moveit_config/package.xml | 2 +- flexiv_msgs/package.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/flexiv_bringup/package.xml b/flexiv_bringup/package.xml index 9a0c34a..87732a8 100644 --- a/flexiv_bringup/package.xml +++ b/flexiv_bringup/package.xml @@ -1,7 +1,7 @@ flexiv_bringup - 0.8.0 + 0.9.0 Package with launch files and run-time configurations for Flexiv robots with `ros_control` Mun Seng Phoon Mun Seng Phoon diff --git a/flexiv_controllers/package.xml b/flexiv_controllers/package.xml index 0c9d8a7..73ee2f6 100644 --- a/flexiv_controllers/package.xml +++ b/flexiv_controllers/package.xml @@ -1,7 +1,7 @@ flexiv_controllers - 0.8.0 + 0.9.0 Flexiv custom ROS controllers Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_description/package.xml b/flexiv_description/package.xml index 142121e..dd29561 100644 --- a/flexiv_description/package.xml +++ b/flexiv_description/package.xml @@ -1,7 +1,7 @@ flexiv_description - 0.8.0 + 0.9.0 URDF description for flexiv arms Mun Seng Phoon Mun Seng Phoon diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml index f132968..2da4bd8 100644 --- a/flexiv_hardware/package.xml +++ b/flexiv_hardware/package.xml @@ -1,7 +1,7 @@ flexiv_hardware - 0.8.0 + 0.9.0 Hardware interfaces to Flexiv robots for ROS control Mun Seng Phoon Mun Seng Phoon diff --git a/flexiv_moveit_config/package.xml b/flexiv_moveit_config/package.xml index da8a5b2..efc1963 100644 --- a/flexiv_moveit_config/package.xml +++ b/flexiv_moveit_config/package.xml @@ -1,7 +1,7 @@ flexiv_moveit_config - 0.8.0 + 0.9.0 An automatically generated package with all the configuration and launch files for using the rizon4 with the MoveIt Motion Planning Framework diff --git a/flexiv_msgs/package.xml b/flexiv_msgs/package.xml index ba26064..58b2e43 100644 --- a/flexiv_msgs/package.xml +++ b/flexiv_msgs/package.xml @@ -1,7 +1,7 @@ flexiv_msgs - 0.8.0 + 0.9.0 Robot states messages definiton used in RDK Mun Seng Phoon Mun Seng Phoon