flexivrobotics · munseng-flexiv · Jan 5, 2024 · Dec 12, 2023 · Dec 12, 2023 · Dec 12, 2023
diff --git 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:

diff --git 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]

diff --git 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:
 

diff --git a/flexiv_bringup/package.xml b/flexiv_bringup/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flexiv_bringup</name>
-  <version>0.8.0</version>
+  <version>0.9.0</version>
   <description>Package with launch files and run-time configurations for Flexiv robots with `ros_control`</description>
   <author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>

diff --git a/flexiv_controllers/package.xml b/flexiv_controllers/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flexiv_controllers</name>
-  <version>0.8.0</version>
+  <version>0.9.0</version>
   <description>Flexiv custom ROS controllers</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>

diff --git a/flexiv_description/package.xml b/flexiv_description/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flexiv_description</name>
-  <version>0.8.0</version>
+  <version>0.9.0</version>
   <description>URDF description for flexiv arms</description>
   <author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>

diff --git 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}
 )
 

diff --git 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<double> joint_position_command_;
     std::vector<double> joint_velocity_command_;
     std::vector<double> joint_effort_command_;
-    std::vector<double> internal_joint_position_command_;
 
     // Controller
     std::atomic<bool> position_controller_running_;

diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flexiv_hardware</name>
-  <version>0.8.0</version>
+  <version>0.9.0</version>
   <description>Hardware interfaces to Flexiv robots for ROS control</description>
   <author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>

diff --git a/flexiv_hardware/rdk b/flexiv_hardware/rdk
diff --git 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})
@@ -150,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();
 
@@ -248,7 +263,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 +295,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

diff --git a/flexiv_moveit_config/package.xml b/flexiv_moveit_config/package.xml
@@ -1,7 +1,7 @@
 <package>
 
   <name>flexiv_moveit_config</name>
-  <version>0.8.0</version>
+  <version>0.9.0</version>
   <description>
      An automatically generated package with all the configuration and launch files for using the rizon4 with the MoveIt Motion Planning Framework
   </description>

diff --git a/flexiv_msgs/package.xml b/flexiv_msgs/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flexiv_msgs</name>
-  <version>0.8.0</version>
+  <version>0.9.0</version>
   <description>Robot states messages definiton used in RDK</description>
   <author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
+31 −0		.github/ISSUE_TEMPLATE/bug_report.md
+20 −0		.github/ISSUE_TEMPLATE/feature_request.md
+16 −16		CMakeLists.txt
+5 −5		README.md
+1 −1		doc/Doxyfile.in
+7 −3		example/CMakeLists.txt
+0 −6		example/basics1_display_robot_states.cpp
+0 −6		example/basics3_primitive_execution.cpp
+6 −8		example/basics4_plan_execution.cpp
+0 −6		example/basics5_zero_force_torque_sensors.cpp
+0 −6		example/basics6_gripper_control.cpp
+2 −6		example/basics7_auto_recovery.cpp
+20 −14		example/intermediate1_realtime_joint_position_control.cpp
+20 −14		example/intermediate2_realtime_joint_torque_control.cpp
+20 −13		example/intermediate3_realtime_joint_floating.cpp
+64 −33		example/intermediate4_realtime_cartesian_pure_motion_control.cpp
+138 −120		example/intermediate5_realtime_cartesian_motion_force_control.cpp
+74 −61		example/intermediate6_robot_dynamics.cpp
+0 −6		example/intermediate7_teach_by_demonstration.cpp
+9 −12		example_py/basics1_display_robot_states.py
+6 −4		example_py/basics2_clear_fault.py
+16 −19		example_py/basics3_primitive_execution.py
+15 −15		example_py/basics4_plan_execution.py
+18 −17		example_py/basics5_zero_force_torque_sensors.py
+9 −12		example_py/basics6_gripper_control.py
+10 −14		example_py/basics7_auto_recovery.py
+25 −19		example_py/intermediate1_non_realtime_joint_position_control.py
+85 −52		example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py
+149 −119		example_py/intermediate3_non_realtime_cartesian_motion_force_control.py
+30 −19		example_py/intermediate4_teach_by_demonstration.py
+3 −0		include/flexiv/Data.hpp
+16 −13		include/flexiv/Gripper.hpp
+14 −6		include/flexiv/Log.hpp
+6 −21		include/flexiv/Mode.hpp
+37 −25		include/flexiv/Model.hpp
+400 −250		include/flexiv/Robot.hpp
+46 −33		include/flexiv/Scheduler.hpp
+29 −0		include/flexiv/Utility.hpp
+ −		lib/FlexivRdk.win_amd64.lib
+ −		lib/flexiv_rdk.win_amd64.lib
+ −		lib/libflexiv_rdk.aarch64-linux-gnu.a
+ −		lib/libflexiv_rdk.arm64-darwin.a
+ −		lib/libflexiv_rdk.x86_64-linux-gnu.a
+ −		lib_py/flexivrdk.cp310-win_amd64.pyd
+ −		lib_py/flexivrdk.cp38-win_amd64.pyd
+ −		lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so
+ −		lib_py/flexivrdk.cpython-310-darwin.so
+ −		lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so
+ −		lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so
+ −		lib_py/flexivrdk.cpython-38-darwin.so
+ −		lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so
+8 −4		test/CMakeLists.txt
+24 −22		test/test_dynamics_engine.cpp
+21 −16		test/test_dynamics_with_tool.cpp
+59 −93		test/test_endurance.cpp
+22 −18		test/test_loop_latency.cpp
+24 −7		test/test_scheduler.cpp
+31 −60		test/test_timeliness_monitor.cpp