├── .clang-format ├── .github └── workflows │ ├── foxy-binary-build.yml │ └── humble-binary-build.yml ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── flexiv_bringup ├── CMakeLists.txt ├── README.md ├── config │ ├── joint_trajectory_position_publisher.yaml │ ├── rizon_controllers.yaml │ ├── sine_sweep_impedance_config.yaml │ └── sine_sweep_position_config.yaml ├── launch │ ├── rizon.launch.py │ ├── rizon_moveit.launch.py │ ├── sine_sweep_impedance.launch.py │ ├── sine_sweep_position.launch.py │ └── test_joint_trajectory_controller.launch.py └── package.xml ├── flexiv_controllers ├── flexiv_robot_states_broadcaster │ ├── CMakeLists.txt │ ├── flexiv_robot_states_broadcaster.xml │ ├── include │ │ └── flexiv_robot_states_broadcaster │ │ │ ├── flexiv_robot_states.hpp │ │ │ └── flexiv_robot_states_broadcaster.hpp │ ├── package.xml │ └── src │ │ ├── flexiv_robot_states_broadcaster.cpp │ │ └── flexiv_robot_states_broadcaster_parameters.yaml ├── gpio_controller │ ├── CMakeLists.txt │ ├── README.md │ ├── gpio_controller.xml │ ├── include │ │ └── gpio_controller │ │ │ └── gpio_controller.hpp │ ├── package.xml │ └── src │ │ ├── gpio_controller.cpp │ │ └── gpio_controller_parameters.yaml └── joint_impedance_controller │ ├── CMakeLists.txt │ ├── include │ └── joint_impedance_controller │ │ └── joint_impedance_controller.hpp │ ├── joint_impedance_controller.xml │ ├── package.xml │ └── src │ └── joint_impedance_controller.cpp ├── flexiv_description ├── CMakeLists.txt ├── launch │ └── view_rizon.launch.py ├── meshes │ ├── grav │ │ ├── collision │ │ │ ├── base.stl │ │ │ ├── finger_tip.stl │ │ │ ├── inner_bar.stl │ │ │ ├── outer_bar.stl │ │ │ └── static_assembly.stl │ │ └── visual │ │ │ ├── base.stl │ │ │ ├── finger_mount.stl │ │ │ ├── finger_tip.mtl │ │ │ ├── finger_tip.obj │ │ │ ├── inner_bar.stl │ │ │ ├── outer_bar.stl │ │ │ └── static_assembly.stl │ ├── rizon10 │ │ ├── collision │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ └── visual │ │ │ ├── link0.obj │ │ │ ├── link1.obj │ │ │ ├── link2.obj │ │ │ ├── link3.obj │ │ │ ├── link4.obj │ │ │ ├── link5.obj │ │ │ ├── link6.obj │ │ │ ├── link7.obj │ │ │ └── ring.obj │ ├── rizon10s │ │ ├── collision │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ └── visual │ │ │ ├── link0.obj │ │ │ ├── link1.obj │ │ │ ├── link2.obj │ │ │ ├── link3.obj │ │ │ ├── link4.obj │ │ │ ├── link5.obj │ │ │ ├── link6.obj │ │ │ ├── link7.obj │ │ │ └── ring.obj │ ├── rizon4 │ │ ├── collision │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ └── visual │ │ │ ├── link0.obj │ │ │ ├── link1.obj │ │ │ ├── link2.obj │ │ │ ├── link3.obj │ │ │ ├── link4.obj │ │ │ ├── link5.obj │ │ │ ├── link6.obj │ │ │ ├── link7.obj │ │ │ └── ring.obj │ └── rizon4s │ │ ├── collision │ │ ├── link0.stl │ │ ├── link1.stl │ │ ├── link2.stl │ │ ├── link3.stl │ │ ├── link4.stl │ │ ├── link5.stl │ │ ├── link6.stl │ │ └── link7.stl │ │ └── visual │ │ ├── link0.obj │ │ ├── link1.obj │ │ ├── link2.obj │ │ ├── link3.obj │ │ ├── link4.obj │ │ ├── link5.obj │ │ ├── link6.obj │ │ ├── link7.obj │ │ └── ring.obj ├── package.xml ├── rviz │ └── view_rizon.rviz └── urdf │ ├── flexiv_arm.materials.xacro │ ├── grav_macro.xacro │ ├── rizon.urdf.xacro │ ├── rizon10.ros2_control.xacro │ ├── rizon10_joints_links.xacro │ ├── rizon4.ros2_control.xacro │ ├── rizon4_joints_links.xacro │ └── rizon_macro.xacro ├── flexiv_gripper ├── CMakeLists.txt ├── config │ └── flexiv_gripper_node.yaml ├── include │ └── flexiv_gripper │ │ └── gripper_action_server.hpp ├── launch │ └── flexiv_gripper.launch.py ├── package.xml └── src │ └── gripper_action_server.cpp ├── flexiv_hardware ├── CMakeLists.txt ├── flexiv_hardware_interface_plugin.xml ├── include │ └── flexiv_hardware │ │ ├── flexiv_hardware_interface.hpp │ │ └── visibility_control.h ├── package.xml └── src │ └── flexiv_hardware_interface.cpp ├── flexiv_moveit_config ├── CMakeLists.txt ├── config │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── moveit_controllers.yaml │ ├── ompl_planning.yaml │ └── rizon_moveit_servo_config.yaml ├── package.xml ├── rviz │ └── moveit.rviz └── srdf │ ├── grav.srdf.xacro │ ├── rizon.srdf.xacro │ └── rizon_macro.srdf.xacro ├── flexiv_msgs ├── CMakeLists.txt ├── action │ ├── Grasp.action │ └── Move.action ├── msg │ ├── Digital.msg │ ├── GPIOStates.msg │ ├── JointPosVel.msg │ ├── Mode.msg │ └── RobotStates.msg └── package.xml └── flexiv_test_nodes ├── flexiv_test_nodes ├── __init__.py ├── publisher_joint_trajectory_controller.py ├── sine_sweep_impedance_controller.py └── sine_sweep_position_controller.py ├── package.xml ├── resource └── flexiv_test_nodes ├── setup.cfg └── setup.py /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Proto 3 | BasedOnStyle: Google 4 | --- 5 | Language: Cpp 6 | AccessModifierOffset: -4 7 | AlignAfterOpenBracket: false 8 | AlignConsecutiveAssignments: false 9 | AlignConsecutiveDeclarations: false 10 | AlignEscapedNewlinesLeft: false 11 | AlignOperands: true 12 | AlignTrailingComments: true 13 | AllowAllParametersOfDeclarationOnNextLine: false 14 | AllowShortBlocksOnASingleLine: false 15 | AllowShortCaseLabelsOnASingleLine: false 16 | AllowShortFunctionsOnASingleLine: Inline 17 | AllowShortIfStatementsOnASingleLine: false 18 | AllowShortLoopsOnASingleLine: false 19 | AlwaysBreakAfterDefinitionReturnType: None 20 | AlwaysBreakAfterReturnType: None 21 | AlwaysBreakBeforeMultilineStrings: true 22 | AlwaysBreakTemplateDeclarations: true 23 | BasedOnStyle: WebKit 24 | BinPackArguments: true 25 | BinPackParameters: true 26 | BreakBeforeBraces: Mozilla 27 | BreakBeforeTernaryOperators: true 28 | BreakConstructorInitializersBeforeComma: true 29 | ColumnLimit: 100 30 | CommentPragmas: "^ IWYU pragma:" 31 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 32 | ConstructorInitializerIndentWidth: 0 33 | ContinuationIndentWidth: 4 34 | Cpp11BracedListStyle: true 35 | DerivePointerAlignment: false 36 | DisableFormat: false 37 | ExperimentalAutoDetectBinPacking: false 38 | ForEachMacros: [foreach, Q_FOREACH, BOOST_FOREACH] 39 | IndentCaseLabels: true 40 | IndentWidth: 4 41 | IndentWrappedFunctionNames: false 42 | KeepEmptyLinesAtTheStartOfBlocks: true 43 | MacroBlockBegin: "" 44 | MacroBlockEnd: "" 45 | MaxEmptyLinesToKeep: 1 46 | NamespaceIndentation: None 47 | PenaltyBreakBeforeFirstCallParameter: 100 48 | PenaltyBreakComment: 300 49 | PenaltyBreakFirstLessLess: 120 50 | PenaltyBreakString: 1000 51 | PenaltyExcessCharacter: 10000 52 | PointerAlignment: Left 53 | ReflowComments: true 54 | SortIncludes: false 55 | SpaceAfterCStyleCast: false 56 | SpaceBeforeAssignmentOperators: true 57 | SpaceBeforeParens: ControlStatements 58 | SpaceInEmptyParentheses: false 59 | SpacesInContainerLiterals: true 60 | SpacesInCStyleCastParentheses: false 61 | SpacesInParentheses: false 62 | SpacesInSquareBrackets: false 63 | Standard: Cpp11 64 | TabWidth: 4 65 | UseTab: Never 66 | -------------------------------------------------------------------------------- /.github/workflows/foxy-binary-build.yml: -------------------------------------------------------------------------------- 1 | name: Foxy Binary Build 2 | 3 | on: 4 | push: 5 | branches: 6 | - foxy 7 | pull_request: 8 | branches: 9 | - foxy 10 | 11 | jobs: 12 | foxy_binary: 13 | name: Foxy binary job 14 | runs-on: ubuntu-20.04 15 | container: 16 | image: osrf/ros:foxy-desktop 17 | strategy: 18 | fail-fast: false 19 | env: 20 | ROS_DISTRO: foxy 21 | steps: 22 | - name: Checkout 23 | uses: actions/checkout@v3 24 | - name: Install dependencies 25 | run: | 26 | sudo apt-get update && sudo apt-get install -y \ 27 | libeigen3-dev \ 28 | ros-foxy-xacro \ 29 | ros-foxy-tinyxml2-vendor \ 30 | ros-foxy-ros2-control \ 31 | ros-foxy-realtime-tools \ 32 | ros-foxy-control-toolbox \ 33 | ros-foxy-moveit \ 34 | ros-foxy-ros2-controllers \ 35 | ros-foxy-test-msgs \ 36 | ros-foxy-joint-state-publisher \ 37 | ros-foxy-joint-state-publisher-gui \ 38 | ros-foxy-robot-state-publisher 39 | - name: Build and run tests 40 | id: action-ros-ci 41 | uses: ros-tooling/action-ros-ci@v0.3 42 | with: 43 | target-ros2-distro: ${{ env.ROS_DISTRO }} 44 | package-name: | 45 | flexiv_bringup 46 | flexiv_controllers 47 | flexiv_description 48 | flexiv_hardware 49 | flexiv_moveit_config 50 | flexiv_msgs 51 | flexiv_test_nodes 52 | skip-tests: true 53 | -------------------------------------------------------------------------------- /.github/workflows/humble-binary-build.yml: -------------------------------------------------------------------------------- 1 | name: Humble Binary Build 2 | 3 | on: 4 | push: 5 | branches: 6 | - humble 7 | pull_request: 8 | branches: 9 | - humble 10 | 11 | jobs: 12 | humble_binary: 13 | name: Humble binary job 14 | runs-on: ubuntu-22.04 15 | container: 16 | image: osrf/ros:humble-desktop 17 | strategy: 18 | fail-fast: false 19 | env: 20 | ROS_DISTRO: humble 21 | steps: 22 | - name: Checkout 23 | uses: actions/checkout@v3 24 | - name: Install dependencies 25 | run: | 26 | sudo apt-get update && sudo apt-get install -y \ 27 | libeigen3-dev \ 28 | ros-humble-xacro \ 29 | ros-humble-tinyxml2-vendor \ 30 | ros-humble-ros2-control \ 31 | ros-humble-realtime-tools \ 32 | ros-humble-control-toolbox \ 33 | ros-humble-moveit \ 34 | ros-humble-ros2-controllers \ 35 | ros-humble-test-msgs \ 36 | ros-humble-joint-state-publisher \ 37 | ros-humble-joint-state-publisher-gui \ 38 | ros-humble-robot-state-publisher 39 | - name: Build and run tests 40 | id: action-ros-ci 41 | uses: ros-tooling/action-ros-ci@v0.3 42 | with: 43 | target-ros2-distro: ${{ env.ROS_DISTRO }} 44 | package-name: | 45 | flexiv_bringup 46 | flexiv_description 47 | flexiv_gripper 48 | flexiv_hardware 49 | flexiv_moveit_config 50 | flexiv_msgs 51 | flexiv_robot_states_broadcaster 52 | flexiv_test_nodes 53 | gpio_controller 54 | joint_impedance_controller 55 | skip-tests: true 56 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeCache.txt 2 | CMakeFiles 3 | Makefile 4 | cmake_install.cmake 5 | install_manifest.txt 6 | *~ 7 | .idea 8 | .directory 9 | .vscode/ 10 | build/ 11 | install/ 12 | log/ 13 | __pycache__/ 14 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "flexiv_hardware/rdk"] 2 | path = flexiv_hardware/rdk 3 | url = https://github.com/flexivrobotics/flexiv_rdk 4 | branch = main 5 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To run checks over all the files in the repo manually: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or run checks automatically every time before commit: 6 | # 7 | # pre-commit install 8 | # 9 | # See https://pre-commit.com for more information 10 | # See https://pre-commit.com/hooks.html for more hooks 11 | repos: 12 | - repo: https://github.com/pre-commit/pre-commit-hooks 13 | rev: v4.4.0 14 | hooks: 15 | - id: trailing-whitespace 16 | - id: end-of-file-fixer 17 | - id: check-yaml 18 | args: [--allow-multiple-documents] 19 | - id: check-added-large-files 20 | args: [--maxkb=600] 21 | - id: pretty-format-json 22 | args: [--no-sort-keys, --autofix, --indent=4] 23 | 24 | - repo: https://github.com/myint/docformatter 25 | rev: v1.7.5 26 | hooks: 27 | - id: docformatter 28 | args: [--in-place] 29 | 30 | - repo: https://github.com/psf/black 31 | rev: 23.9.1 32 | hooks: 33 | - id: black 34 | language_version: python3 35 | args: [--line-length=88] 36 | 37 | - repo: local 38 | hooks: 39 | - id: clang-format 40 | name: clang-format 41 | description: Format files with ClangFormat. 42 | entry: clang-format -i 43 | language: system 44 | files: \.(c|cc|cxx|cpp|cu|h|hh|hpp|hxx|java|js|m|proto)$ 45 | args: ["-fallback-style=none"] 46 | -------------------------------------------------------------------------------- /flexiv_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_bringup) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | # find dependencies 10 | find_package(ament_cmake REQUIRED) 11 | 12 | install( 13 | DIRECTORY config launch 14 | DESTINATION share/${PROJECT_NAME}) 15 | 16 | ament_package() 17 | -------------------------------------------------------------------------------- /flexiv_bringup/README.md: -------------------------------------------------------------------------------- 1 | # flexiv_bringup 2 | 3 | This package contains launch files: the main driver launcher, the MoveIt launch file and demo examples: 4 | 5 | - `rizon.launch.py` - the main launcher: starts *ros2_control* node including hardware interface, runs joint states, Flexiv robot states broadcaster, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller. 6 | - `rizon_moveit.launch.py` - runs MoveIt together with the main driver. The controller for robot joints started in this launch file is *rizon_arm_controller*. 7 | - `test_joint_trajectory_controller.launch` - sends joint trajectory goals to the *rizon_arm_controller*. 8 | - `sine_sweep_position.launch.py` - gets current joint states and then performs a sine-sweep motion with *forward_position_controller*. 9 | - `sine_sweep_impedance.launch.py` - gets current joint states and then performs a sine-sweep motion with *joint_impedance_controller*. 10 | 11 | **NOTE**: The example launch files run the demo nodes from the `flexiv_test_nodes` package, with the parameters defined in `/config`. 12 | -------------------------------------------------------------------------------- /flexiv_bringup/config/joint_trajectory_position_publisher.yaml: -------------------------------------------------------------------------------- 1 | publisher_joint_trajectory_controller: 2 | ros__parameters: 3 | 4 | controller_name: "rizon_arm_controller" 5 | wait_sec_between_publish: 6 6 | 7 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 8 | pos1: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0] 9 | pos2: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698] 10 | pos3: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0] 11 | pos4: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698] 12 | 13 | joints: 14 | - $(var robot_sn)_joint1 15 | - $(var robot_sn)_joint2 16 | - $(var robot_sn)_joint3 17 | - $(var robot_sn)_joint4 18 | - $(var robot_sn)_joint5 19 | - $(var robot_sn)_joint6 20 | - $(var robot_sn)_joint7 21 | 22 | check_starting_point: true 23 | -------------------------------------------------------------------------------- /flexiv_bringup/config/rizon_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 1000 # Hz 4 | 5 | forward_position_controller: 6 | type: position_controllers/JointGroupPositionController 7 | 8 | rizon_arm_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | joint_state_broadcaster: 12 | type: joint_state_broadcaster/JointStateBroadcaster 13 | 14 | joint_impedance_controller: 15 | type: joint_impedance_controller/JointImpedanceController 16 | 17 | gpio_controller: 18 | type: gpio_controller/GPIOController 19 | 20 | flexiv_robot_states_broadcaster: 21 | type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster 22 | 23 | forward_position_controller: 24 | ros__parameters: 25 | joints: 26 | - $(var robot_sn)_joint1 27 | - $(var robot_sn)_joint2 28 | - $(var robot_sn)_joint3 29 | - $(var robot_sn)_joint4 30 | - $(var robot_sn)_joint5 31 | - $(var robot_sn)_joint6 32 | - $(var robot_sn)_joint7 33 | 34 | joint_impedance_controller: 35 | ros__parameters: 36 | joints: 37 | - $(var robot_sn)_joint1 38 | - $(var robot_sn)_joint2 39 | - $(var robot_sn)_joint3 40 | - $(var robot_sn)_joint4 41 | - $(var robot_sn)_joint5 42 | - $(var robot_sn)_joint6 43 | - $(var robot_sn)_joint7 44 | k_p: [3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0] 45 | k_d: [80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0] 46 | 47 | rizon_arm_controller: 48 | ros__parameters: 49 | joints: 50 | - $(var robot_sn)_joint1 51 | - $(var robot_sn)_joint2 52 | - $(var robot_sn)_joint3 53 | - $(var robot_sn)_joint4 54 | - $(var robot_sn)_joint5 55 | - $(var robot_sn)_joint6 56 | - $(var robot_sn)_joint7 57 | command_interfaces: 58 | - position 59 | state_interfaces: 60 | - position 61 | - velocity 62 | state_publish_rate: 100.0 63 | action_monitor_rate: 20.0 64 | allow_partial_joints_goal: false 65 | constraints: 66 | stopped_velocity_tolerance: 0.01 67 | goal_time: 0.0 68 | -------------------------------------------------------------------------------- /flexiv_bringup/config/sine_sweep_impedance_config.yaml: -------------------------------------------------------------------------------- 1 | sine_sweep_impedance_controller: 2 | ros__parameters: 3 | 4 | controller_name: "joint_impedance_controller" 5 | joints: 6 | - $(var robot_sn)_joint1 7 | - $(var robot_sn)_joint2 8 | - $(var robot_sn)_joint3 9 | - $(var robot_sn)_joint4 10 | - $(var robot_sn)_joint5 11 | - $(var robot_sn)_joint6 12 | - $(var robot_sn)_joint7 13 | wait_sec_between_publish: 0.001 14 | speed_scaling: 1.0 15 | -------------------------------------------------------------------------------- /flexiv_bringup/config/sine_sweep_position_config.yaml: -------------------------------------------------------------------------------- 1 | sine_sweep_position_controller: 2 | ros__parameters: 3 | 4 | controller_name: "forward_position_controller" 5 | joints: 6 | - $(var robot_sn)_joint1 7 | - $(var robot_sn)_joint2 8 | - $(var robot_sn)_joint3 9 | - $(var robot_sn)_joint4 10 | - $(var robot_sn)_joint5 11 | - $(var robot_sn)_joint6 12 | - $(var robot_sn)_joint7 13 | wait_sec_between_publish: 0.001 14 | speed_scaling: 1.0 15 | -------------------------------------------------------------------------------- /flexiv_bringup/launch/sine_sweep_impedance.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 4 | from launch_ros.actions import Node 5 | from launch_ros.parameter_descriptions import ParameterFile 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | def generate_launch_description(): 10 | robot_sn = LaunchConfiguration("robot_sn") 11 | 12 | sine_sweep_config = PathJoinSubstitution( 13 | [ 14 | FindPackageShare("flexiv_bringup"), 15 | "config", 16 | "sine_sweep_impedance_config.yaml", 17 | ] 18 | ) 19 | 20 | return LaunchDescription( 21 | [ 22 | DeclareLaunchArgument( 23 | name="robot_sn", 24 | description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", 25 | ), 26 | Node( 27 | package="flexiv_test_nodes", 28 | executable="sine_sweep_impedance_controller", 29 | name="sine_sweep_impedance_controller", 30 | parameters=[ParameterFile(sine_sweep_config, allow_substs=True)], 31 | output={ 32 | "stdout": "screen", 33 | "stderr": "screen", 34 | }, 35 | ), 36 | ] 37 | ) 38 | -------------------------------------------------------------------------------- /flexiv_bringup/launch/sine_sweep_position.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 4 | from launch_ros.actions import Node 5 | from launch_ros.parameter_descriptions import ParameterFile 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | def generate_launch_description(): 10 | robot_sn = LaunchConfiguration("robot_sn") 11 | 12 | sine_sweep_config = PathJoinSubstitution( 13 | [ 14 | FindPackageShare("flexiv_bringup"), 15 | "config", 16 | "sine_sweep_position_config.yaml", 17 | ] 18 | ) 19 | 20 | return LaunchDescription( 21 | [ 22 | DeclareLaunchArgument( 23 | name="robot_sn", 24 | description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", 25 | ), 26 | Node( 27 | package="flexiv_test_nodes", 28 | executable="sine_sweep_position_controller", 29 | name="sine_sweep_position_controller", 30 | parameters=[ParameterFile(sine_sweep_config, allow_substs=True)], 31 | output={ 32 | "stdout": "screen", 33 | "stderr": "screen", 34 | }, 35 | ), 36 | ] 37 | ) 38 | -------------------------------------------------------------------------------- /flexiv_bringup/launch/test_joint_trajectory_controller.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 4 | from launch_ros.actions import Node 5 | from launch_ros.parameter_descriptions import ParameterFile 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | def generate_launch_description(): 10 | robot_sn = LaunchConfiguration("robot_sn") 11 | 12 | position_goals = PathJoinSubstitution( 13 | [ 14 | FindPackageShare("flexiv_bringup"), 15 | "config", 16 | "joint_trajectory_position_publisher.yaml", 17 | ] 18 | ) 19 | 20 | return LaunchDescription( 21 | [ 22 | DeclareLaunchArgument( 23 | name="robot_sn", 24 | description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", 25 | ), 26 | Node( 27 | package="flexiv_test_nodes", 28 | executable="publisher_joint_trajectory_controller", 29 | name="publisher_joint_trajectory_controller", 30 | parameters=[ParameterFile(position_goals, allow_substs=True)], 31 | output={ 32 | "stdout": "screen", 33 | "stderr": "screen", 34 | }, 35 | ), 36 | ] 37 | ) 38 | -------------------------------------------------------------------------------- /flexiv_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_bringup 5 | 1.6.0 6 | Package with launch files and run-time configurations for Flexiv robots with `ros2_control` 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | controller_manager 14 | flexiv_description 15 | flexiv_gripper 16 | flexiv_hardware 17 | flexiv_robot_states_broadcaster 18 | flexiv_test_nodes 19 | forward_command_controller 20 | joint_state_broadcaster 21 | joint_state_publisher 22 | joint_state_publisher_gui 23 | joint_trajectory_controller 24 | robot_state_publisher 25 | rviz2 26 | xacro 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_robot_states_broadcaster LANGUAGES CXX) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(controller_interface REQUIRED) 16 | find_package(generate_parameter_library REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(flexiv_hardware REQUIRED) 19 | find_package(flexiv_msgs REQUIRED) 20 | find_package(hardware_interface REQUIRED) 21 | find_package(pluginlib REQUIRED) 22 | find_package(rclcpp REQUIRED) 23 | find_package(rclcpp_lifecycle REQUIRED) 24 | find_package(realtime_tools REQUIRED) 25 | 26 | set(THIS_PACKAGE_INCLUDE_DEPENDS 27 | builtin_interfaces 28 | controller_interface 29 | flexiv_hardware 30 | flexiv_msgs 31 | generate_parameter_library 32 | geometry_msgs 33 | hardware_interface 34 | pluginlib 35 | rclcpp_lifecycle 36 | rcutils 37 | realtime_tools 38 | ) 39 | 40 | generate_parameter_library(flexiv_robot_states_broadcaster_parameters 41 | src/flexiv_robot_states_broadcaster_parameters.yaml 42 | ) 43 | 44 | add_library(flexiv_robot_states_broadcaster 45 | SHARED 46 | src/flexiv_robot_states_broadcaster.cpp 47 | ) 48 | 49 | target_include_directories(flexiv_robot_states_broadcaster PRIVATE 50 | ${CMAKE_CURRENT_SOURCE_DIR}/include 51 | ${CMAKE_CURRENT_SOURCE_DIR}/../../flexiv_hardware/rdk/include 52 | ) 53 | 54 | target_link_libraries(flexiv_robot_states_broadcaster PUBLIC 55 | flexiv_robot_states_broadcaster_parameters 56 | ) 57 | 58 | ament_target_dependencies(flexiv_robot_states_broadcaster PUBLIC 59 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 60 | ) 61 | 62 | # prevent pluginlib from using boost 63 | target_compile_definitions(flexiv_robot_states_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 64 | pluginlib_export_plugin_description_file(controller_interface flexiv_robot_states_broadcaster.xml) 65 | 66 | install( 67 | DIRECTORY include/ 68 | DESTINATION include/flexiv_robot_states_broadcaster 69 | ) 70 | 71 | install( 72 | TARGETS 73 | flexiv_robot_states_broadcaster 74 | flexiv_robot_states_broadcaster_parameters 75 | EXPORT export_flexiv_robot_states_broadcaster 76 | ARCHIVE DESTINATION lib 77 | LIBRARY DESTINATION lib 78 | RUNTIME DESTINATION bin 79 | ) 80 | 81 | ament_export_targets(export_flexiv_robot_states_broadcaster HAS_LIBRARY_TARGET) 82 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 83 | 84 | ament_package() 85 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | The robot states broadcaster publishes the Flexiv robot states. 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file flexiv_robot_states.hpp 3 | * @brief Semantic component interface to read the Flexiv robot states. 4 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 5 | */ 6 | 7 | #ifndef SEMANTIC_COMPONENTS__FLEXIV_ROBOT_STATES_HPP_ 8 | #define SEMANTIC_COMPONENTS__FLEXIV_ROBOT_STATES_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "hardware_interface/loaned_state_interface.hpp" 18 | #include "semantic_components/semantic_component_interface.hpp" 19 | #include "flexiv/rdk/data.hpp" 20 | #include "flexiv_msgs/msg/robot_states.hpp" 21 | 22 | #include "geometry_msgs/msg/accel.hpp" 23 | #include "geometry_msgs/msg/point.hpp" 24 | #include "geometry_msgs/msg/pose.hpp" 25 | #include "geometry_msgs/msg/quaternion.hpp" 26 | #include "geometry_msgs/msg/twist.hpp" 27 | #include "geometry_msgs/msg/vector3.hpp" 28 | #include "geometry_msgs/msg/wrench.hpp" 29 | 30 | namespace { 31 | 32 | const std::string kWorldFrameId = "world"; 33 | const std::string kFlangeFrameId = "flange"; 34 | 35 | // Example implementation of bit_cast: https://en.cppreference.com/w/cpp/numeric/bit_cast 36 | template 37 | std::enable_if_t::value 38 | && std::is_trivially_copyable::value, 39 | To> 40 | bit_cast(const From& src) noexcept 41 | { 42 | static_assert(std::is_trivially_constructible::value, 43 | "This implementation additionally requires " 44 | "destination type to be trivially constructible"); 45 | 46 | To dst; 47 | std::memcpy(&dst, &src, sizeof(To)); 48 | return dst; 49 | } 50 | } // namespace 51 | 52 | namespace semantic_components { 53 | 54 | class FlexivRobotStates : public SemanticComponentInterface 55 | { 56 | public: 57 | FlexivRobotStates(const std::string& name) 58 | : SemanticComponentInterface(name, 1) 59 | { 60 | interface_names_.emplace_back(name_ + "/" + state_interface_name_); 61 | } 62 | 63 | virtual ~FlexivRobotStates() = default; 64 | 65 | void init_robot_states_message(flexiv_msgs::msg::RobotStates& message) 66 | { 67 | message.tcp_pose.header.frame_id = kWorldFrameId; 68 | message.tcp_vel.header.frame_id = kWorldFrameId; 69 | message.flange_pose.header.frame_id = kWorldFrameId; 70 | message.ft_sensor_raw.header.frame_id = name_ + "_" + kFlangeFrameId; 71 | message.ext_wrench_in_tcp.header.frame_id = name_ + "_" + kFlangeFrameId; 72 | message.ext_wrench_in_world.header.frame_id = name_ + "_" + kFlangeFrameId; 73 | } 74 | 75 | /// Return RobotStates message 76 | bool get_values_as_message(flexiv_msgs::msg::RobotStates& message) 77 | { 78 | const std::string flexiv_robot_states_interface_name = name_ + "/" + state_interface_name_; 79 | 80 | auto flexiv_robot_states_interface 81 | = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), 82 | [&flexiv_robot_states_interface_name](const auto& state_interface) { 83 | return state_interface.get().get_name() == flexiv_robot_states_interface_name; 84 | }); 85 | 86 | if (flexiv_robot_states_interface != state_interfaces_.end()) { 87 | // Get the robot states pointer via bit_cast 88 | flexiv_robot_states_ptr = bit_cast( 89 | (*flexiv_robot_states_interface).get().get_value()); 90 | } else { 91 | RCLCPP_ERROR( 92 | rclcpp::get_logger("FlexivRobotStates"), "Robot states interface not found."); 93 | return false; 94 | } 95 | 96 | // Update timestamps 97 | message.tcp_pose.header.stamp = message.header.stamp; 98 | message.tcp_vel.header.stamp = message.header.stamp; 99 | message.flange_pose.header.stamp = message.header.stamp; 100 | message.ft_sensor_raw.header.stamp = message.header.stamp; 101 | message.ext_wrench_in_tcp.header.stamp = message.header.stamp; 102 | message.ext_wrench_in_world.header.stamp = message.header.stamp; 103 | 104 | // Fill the RobotStates message 105 | message.q = toJointStateMsg(flexiv_robot_states_ptr->q); 106 | message.theta = toJointStateMsg(flexiv_robot_states_ptr->theta); 107 | message.dq = toJointStateMsg(flexiv_robot_states_ptr->dq); 108 | message.dtheta = toJointStateMsg(flexiv_robot_states_ptr->dtheta); 109 | message.tau = toJointStateMsg(flexiv_robot_states_ptr->tau); 110 | message.tau_des = toJointStateMsg(flexiv_robot_states_ptr->tau_des); 111 | message.tau_dot = toJointStateMsg(flexiv_robot_states_ptr->tau_dot); 112 | message.tau_ext = toJointStateMsg(flexiv_robot_states_ptr->tau_ext); 113 | 114 | message.tcp_pose.pose = toPoseMsg(flexiv_robot_states_ptr->tcp_pose); 115 | message.tcp_vel.accel = toAccelMsg(flexiv_robot_states_ptr->tcp_vel); 116 | message.flange_pose.pose = toPoseMsg(flexiv_robot_states_ptr->flange_pose); 117 | message.ft_sensor_raw.wrench = toWrenchMsg(flexiv_robot_states_ptr->ft_sensor_raw); 118 | message.ext_wrench_in_tcp.wrench = toWrenchMsg(flexiv_robot_states_ptr->ext_wrench_in_tcp); 119 | message.ext_wrench_in_world.wrench 120 | = toWrenchMsg(flexiv_robot_states_ptr->ext_wrench_in_world); 121 | 122 | return true; 123 | } 124 | 125 | protected: 126 | flexiv::rdk::RobotStates* flexiv_robot_states_ptr; 127 | 128 | const std::string state_interface_name_ {"flexiv_robot_states"}; 129 | 130 | // Convert std::vector to std::array 131 | std::array toJointStateMsg(const std::vector& joint_values) 132 | { 133 | std::array joint_state_msg; 134 | for (size_t i = 0; i < joint_values.size(); ++i) { 135 | joint_state_msg[i] = joint_values[i]; 136 | } 137 | return joint_state_msg; 138 | } 139 | 140 | // Convert std::array to geometry_msgs::msg::Pose 141 | geometry_msgs::msg::Pose toPoseMsg( 142 | const std::array& pose_values) 143 | { 144 | geometry_msgs::msg::Pose pose_msg; 145 | pose_msg.position = geometry_msgs::build() 146 | .x(pose_values[0]) 147 | .y(pose_values[1]) 148 | .z(pose_values[2]); 149 | pose_msg.orientation = geometry_msgs::build() 150 | .x(pose_values[4]) 151 | .y(pose_values[5]) 152 | .z(pose_values[6]) 153 | .w(pose_values[3]); 154 | return pose_msg; 155 | } 156 | 157 | // Convert std::array to geometry_msgs::msg::Accel 158 | geometry_msgs::msg::Accel toAccelMsg( 159 | const std::array& accel_values) 160 | { 161 | geometry_msgs::msg::Accel accel_msg; 162 | accel_msg.linear = geometry_msgs::build() 163 | .x(accel_values[0]) 164 | .y(accel_values[1]) 165 | .z(accel_values[2]); 166 | accel_msg.angular = geometry_msgs::build() 167 | .x(accel_values[3]) 168 | .y(accel_values[4]) 169 | .z(accel_values[5]); 170 | return accel_msg; 171 | } 172 | 173 | // Convert std::array to geometry_msgs::msg::Wrench 174 | geometry_msgs::msg::Wrench toWrenchMsg( 175 | const std::array& wrench_values) 176 | { 177 | geometry_msgs::msg::Wrench wrench_msg; 178 | wrench_msg.force = geometry_msgs::build() 179 | .x(wrench_values[0]) 180 | .y(wrench_values[1]) 181 | .z(wrench_values[2]); 182 | wrench_msg.torque = geometry_msgs::build() 183 | .x(wrench_values[3]) 184 | .y(wrench_values[4]) 185 | .z(wrench_values[5]); 186 | return wrench_msg; 187 | } 188 | }; 189 | 190 | } /* namespace semantic_components */ 191 | 192 | #endif /* SEMANTIC_COMPONENTS__FLEXIV_ROBOT_STATES_HPP_ */ 193 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file flexiv_robot_states_broadcaster.hpp 3 | * @brief Header file for FlexivRobotStatesBroadcaster class 4 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 5 | */ 6 | 7 | #ifndef FLEXIV_ROBOT_STATES_BROADCASTER__FLEXIV_ROBOT_STATES_BROADCASTER_HPP_ 8 | #define FLEXIV_ROBOT_STATES_BROADCASTER__FLEXIV_ROBOT_STATES_BROADCASTER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "controller_interface/controller_interface.hpp" 15 | #include "flexiv_msgs/msg/robot_states.hpp" 16 | #include "flexiv_robot_states_broadcaster/flexiv_robot_states.hpp" 17 | #include "flexiv_robot_states_broadcaster_parameters.hpp" 18 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 19 | #include "rclcpp_lifecycle/state.hpp" 20 | #include "realtime_tools/realtime_publisher.hpp" 21 | 22 | namespace flexiv_robot_states_broadcaster { 23 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 24 | 25 | const std::string kRobotStatesTopic = "/flexiv_robot_states"; 26 | const std::string kTcpPoseTopic = "/tcp_pose"; 27 | const std::string kTcpVelocityTopic = "/tcp_velocity"; 28 | const std::string kFlangePoseTopic = "/flange_pose"; 29 | const std::string kFTSensorTopic = "/ft_sensor_wrench"; 30 | const std::string kExternalWrenchInTcpFrameTopic = "/external_wrench_in_tcp"; 31 | const std::string kExternalWrenchInWorldFrameTopic = "/external_wrench_in_world"; 32 | 33 | class FlexivRobotStatesBroadcaster : public controller_interface::ControllerInterface 34 | { 35 | public: 36 | FlexivRobotStatesBroadcaster(); 37 | 38 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 39 | 40 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 41 | 42 | controller_interface::return_type update( 43 | const rclcpp::Time& time, const rclcpp::Duration& period) override; 44 | 45 | CallbackReturn on_init() override; 46 | 47 | CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; 48 | 49 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 50 | 51 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 52 | 53 | protected: 54 | std::shared_ptr param_listener_; 55 | Params params_; 56 | 57 | std::unique_ptr flexiv_robot_states_; 58 | 59 | std::shared_ptr> 60 | flexiv_robot_states_publisher_; 61 | using StatePublisher = realtime_tools::RealtimePublisher; 62 | std::shared_ptr realtime_flexiv_robot_states_publisher_; 63 | 64 | using PoseStampedPublisher = rclcpp::Publisher; 65 | std::shared_ptr tcp_pose_publisher_; 66 | std::shared_ptr flange_pose_publisher_; 67 | 68 | using AccelStampedPublisher = rclcpp::Publisher; 69 | std::shared_ptr tcp_velocity_publisher_; 70 | 71 | using WrenchStampedPublisher = rclcpp::Publisher; 72 | std::shared_ptr ft_sensor_publisher_; 73 | std::shared_ptr external_wrench_in_tcp_publisher_; 74 | std::shared_ptr external_wrench_in_world_publisher_; 75 | }; 76 | 77 | } /* namespace flexiv_robot_states_broadcaster */ 78 | 79 | #endif /* FLEXIV_ROBOT_STATES_BROADCASTER__FLEXIV_ROBOT_STATES_BROADCASTER_HPP_ */ 80 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_robot_states_broadcaster 5 | 1.6.0 6 | The robot states broadcaster publishes the Flexiv robot states. 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | controller_interface 14 | flexiv_hardware 15 | flexiv_msgs 16 | generate_parameter_library 17 | geometry_msgs 18 | hardware_interface 19 | pluginlib 20 | rclcpp 21 | rclcpp_lifecycle 22 | realtime_tools 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/src/flexiv_robot_states_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file flexiv_robot_states_broadcaster.cpp 3 | * @brief Controller to publish the Flexiv robot states. 4 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 5 | */ 6 | 7 | #include "flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster.hpp" 8 | 9 | #include 10 | #include 11 | 12 | namespace flexiv_robot_states_broadcaster { 13 | 14 | FlexivRobotStatesBroadcaster::FlexivRobotStatesBroadcaster() 15 | : controller_interface::ControllerInterface() 16 | { 17 | } 18 | 19 | controller_interface::InterfaceConfiguration 20 | FlexivRobotStatesBroadcaster::command_interface_configuration() const 21 | { 22 | controller_interface::InterfaceConfiguration command_interfaces_config; 23 | command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; 24 | return command_interfaces_config; 25 | } 26 | 27 | controller_interface::InterfaceConfiguration 28 | FlexivRobotStatesBroadcaster::state_interface_configuration() const 29 | { 30 | controller_interface::InterfaceConfiguration state_interfaces_config; 31 | state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 32 | state_interfaces_config.names = flexiv_robot_states_->get_state_interface_names(); 33 | return state_interfaces_config; 34 | } 35 | 36 | CallbackReturn FlexivRobotStatesBroadcaster::on_init() 37 | { 38 | try { 39 | param_listener_ = std::make_shared(get_node()); 40 | params_ = param_listener_->get_params(); 41 | } catch (const std::exception& e) { 42 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 43 | return CallbackReturn::ERROR; 44 | } 45 | 46 | return CallbackReturn::SUCCESS; 47 | } 48 | 49 | CallbackReturn FlexivRobotStatesBroadcaster::on_configure( 50 | const rclcpp_lifecycle::State& /*previous_state*/) 51 | { 52 | params_ = param_listener_->get_params(); 53 | 54 | std::string robot_sn = params_.robot_sn; 55 | if (robot_sn.empty()) { 56 | RCLCPP_ERROR(get_node()->get_logger(), "'robot_sn' parameter has to be specified."); 57 | return CallbackReturn::ERROR; 58 | } 59 | 60 | if (!flexiv_robot_states_) { 61 | flexiv_robot_states_ = std::make_unique( 62 | semantic_components::FlexivRobotStates(robot_sn)); 63 | } 64 | 65 | // Replace "-" with "_" in robot_sn to match the topic name 66 | std::replace(robot_sn.begin(), robot_sn.end(), '-', '_'); 67 | 68 | // Create the publishers for the robot states 69 | tcp_pose_publisher_ = get_node()->create_publisher( 70 | "/" + robot_sn + kTcpPoseTopic, rclcpp::SystemDefaultsQoS()); 71 | tcp_velocity_publisher_ = get_node()->create_publisher( 72 | "/" + robot_sn + kTcpVelocityTopic, rclcpp::SystemDefaultsQoS()); 73 | flange_pose_publisher_ = get_node()->create_publisher( 74 | "/" + robot_sn + kFlangePoseTopic, rclcpp::SystemDefaultsQoS()); 75 | ft_sensor_publisher_ = get_node()->create_publisher( 76 | "/" + robot_sn + kFTSensorTopic, rclcpp::SystemDefaultsQoS()); 77 | external_wrench_in_tcp_publisher_ 78 | = get_node()->create_publisher( 79 | "/" + robot_sn + kExternalWrenchInTcpFrameTopic, rclcpp::SystemDefaultsQoS()); 80 | external_wrench_in_world_publisher_ 81 | = get_node()->create_publisher( 82 | "/" + robot_sn + kExternalWrenchInWorldFrameTopic, rclcpp::SystemDefaultsQoS()); 83 | 84 | try { 85 | flexiv_robot_states_publisher_ 86 | = get_node()->create_publisher( 87 | "/" + robot_sn + kRobotStatesTopic, rclcpp::SystemDefaultsQoS()); 88 | realtime_flexiv_robot_states_publisher_ 89 | = std::make_unique(flexiv_robot_states_publisher_); 90 | // Initialize the robot states message 91 | flexiv_robot_states_->init_robot_states_message( 92 | realtime_flexiv_robot_states_publisher_->msg_); 93 | } catch (const std::exception& e) { 94 | fprintf(stderr, "Exception thrown during publisher creation with message: %s \n", e.what()); 95 | return CallbackReturn::ERROR; 96 | } 97 | 98 | RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); 99 | return CallbackReturn::SUCCESS; 100 | } 101 | 102 | controller_interface::return_type FlexivRobotStatesBroadcaster::update( 103 | const rclcpp::Time& time, const rclcpp::Duration& /*period*/) 104 | { 105 | if (realtime_flexiv_robot_states_publisher_ 106 | && realtime_flexiv_robot_states_publisher_->trylock()) { 107 | realtime_flexiv_robot_states_publisher_->msg_.header.stamp = time; 108 | 109 | if (!flexiv_robot_states_->get_values_as_message( 110 | realtime_flexiv_robot_states_publisher_->msg_)) { 111 | RCLCPP_ERROR(get_node()->get_logger(), 112 | "Failed to get fleixv robot states via flexiv robot states interface."); 113 | realtime_flexiv_robot_states_publisher_->unlock(); 114 | return controller_interface::return_type::ERROR; 115 | } 116 | 117 | realtime_flexiv_robot_states_publisher_->unlockAndPublish(); 118 | 119 | const auto& flexiv_robot_states_msg = realtime_flexiv_robot_states_publisher_->msg_; 120 | tcp_pose_publisher_->publish(flexiv_robot_states_msg.tcp_pose); 121 | tcp_velocity_publisher_->publish(flexiv_robot_states_msg.tcp_vel); 122 | flange_pose_publisher_->publish(flexiv_robot_states_msg.flange_pose); 123 | ft_sensor_publisher_->publish(flexiv_robot_states_msg.ft_sensor_raw); 124 | external_wrench_in_tcp_publisher_->publish(flexiv_robot_states_msg.ext_wrench_in_tcp); 125 | external_wrench_in_world_publisher_->publish(flexiv_robot_states_msg.ext_wrench_in_world); 126 | } 127 | // TODO: Enable the error message when the realtime_publisher is updated in ROS 2 128 | // else { 129 | // RCLCPP_ERROR(get_node()->get_logger(), "Failed to lock the realtime publisher."); 130 | // return controller_interface::return_type::ERROR; 131 | // } 132 | 133 | return controller_interface::return_type::OK; 134 | } 135 | 136 | CallbackReturn FlexivRobotStatesBroadcaster::on_activate( 137 | const rclcpp_lifecycle::State& /*previous_state*/) 138 | { 139 | flexiv_robot_states_->assign_loaned_state_interfaces(state_interfaces_); 140 | return CallbackReturn::SUCCESS; 141 | } 142 | 143 | CallbackReturn FlexivRobotStatesBroadcaster::on_deactivate( 144 | const rclcpp_lifecycle::State& /*previous_state*/) 145 | { 146 | flexiv_robot_states_->release_interfaces(); 147 | return CallbackReturn::SUCCESS; 148 | } 149 | 150 | } /* namespace flexiv_robot_states_broadcaster */ 151 | 152 | #include "pluginlib/class_list_macros.hpp" 153 | 154 | PLUGINLIB_EXPORT_CLASS(flexiv_robot_states_broadcaster::FlexivRobotStatesBroadcaster, 155 | controller_interface::ControllerInterface) 156 | -------------------------------------------------------------------------------- /flexiv_controllers/flexiv_robot_states_broadcaster/src/flexiv_robot_states_broadcaster_parameters.yaml: -------------------------------------------------------------------------------- 1 | flexiv_robot_states_broadcaster: 2 | robot_sn: { 3 | type: string, 4 | default_value: "", 5 | description: "Robot arm serial number", 6 | } 7 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(gpio_controller LANGUAGES CXX) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(controller_interface REQUIRED) 16 | find_package(generate_parameter_library REQUIRED) 17 | find_package(flexiv_msgs REQUIRED) 18 | find_package(hardware_interface REQUIRED) 19 | find_package(pluginlib REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclcpp_lifecycle REQUIRED) 22 | find_package(realtime_tools REQUIRED) 23 | 24 | set(THIS_PACKAGE_INCLUDE_DEPENDS 25 | builtin_interfaces 26 | controller_interface 27 | flexiv_msgs 28 | generate_parameter_library 29 | hardware_interface 30 | pluginlib 31 | rclcpp_lifecycle 32 | rcutils 33 | realtime_tools 34 | ) 35 | 36 | generate_parameter_library(gpio_controller_parameters 37 | src/gpio_controller_parameters.yaml 38 | ) 39 | 40 | add_library(${PROJECT_NAME} 41 | SHARED 42 | src/gpio_controller.cpp 43 | ) 44 | 45 | target_include_directories(${PROJECT_NAME} PRIVATE include) 46 | 47 | target_link_libraries(${PROJECT_NAME} PUBLIC 48 | gpio_controller_parameters 49 | ) 50 | 51 | ament_target_dependencies(${PROJECT_NAME} PUBLIC 52 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 53 | ) 54 | 55 | # prevent pluginlib from using boost 56 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 57 | pluginlib_export_plugin_description_file(controller_interface gpio_controller.xml) 58 | 59 | install( 60 | DIRECTORY include/ 61 | DESTINATION include/${PROJECT_NAME} 62 | ) 63 | 64 | install( 65 | TARGETS 66 | ${PROJECT_NAME} 67 | gpio_controller_parameters 68 | EXPORT export_${PROJECT_NAME} 69 | ARCHIVE DESTINATION lib 70 | LIBRARY DESTINATION lib 71 | RUNTIME DESTINATION bin 72 | ) 73 | 74 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 75 | ament_export_include_directories(include) 76 | ament_export_libraries(${PROJECT_NAME}) 77 | 78 | ament_package() 79 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/README.md: -------------------------------------------------------------------------------- 1 | gpio_controller 2 | ================== 3 | 4 | Controller to control the GPIOs of the Flexiv robot. 5 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/gpio_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | The GPIO controller publishes the GPIO states. 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/include/gpio_controller/gpio_controller.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gpio_controller.hpp 3 | * @brief GPIO controller as ROS 2 controller. Adapted from 4 | * ros2_control_demos/example_10/gpio_controller 5 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 6 | * @author Flexiv 7 | */ 8 | 9 | #ifndef GPIO_CONTROLLER__GPIO_CONTROLLER_HPP_ 10 | #define GPIO_CONTROLLER__GPIO_CONTROLLER_HPP_ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "controller_interface/controller_interface.hpp" 17 | #include "gpio_controller_parameters.hpp" 18 | #include "flexiv_msgs/msg/gpio_states.hpp" 19 | 20 | namespace gpio_controller { 21 | 22 | /** Number of digital IO ports (16 on control box + 2 inside the wrist connector) */ 23 | constexpr size_t kIOPorts = 18; 24 | 25 | const std::string kGPIOInputsTopic = "/gpio_inputs"; 26 | const std::string kGPIOOutputsTopic = "/gpio_outputs"; 27 | 28 | using CmdType = flexiv_msgs::msg::GPIOStates; 29 | 30 | class GPIOController : public controller_interface::ControllerInterface 31 | { 32 | public: 33 | GPIOController(); 34 | 35 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 36 | 37 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 38 | 39 | controller_interface::return_type update( 40 | const rclcpp::Time& time, const rclcpp::Duration& period) override; 41 | 42 | CallbackReturn on_init() override; 43 | 44 | CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; 45 | 46 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 47 | 48 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 49 | 50 | protected: 51 | std::shared_ptr param_listener_; 52 | Params params_; 53 | 54 | void initMsgs(); 55 | 56 | // internal commands 57 | std::array digital_outputs_cmd_; 58 | 59 | // publisher 60 | std::shared_ptr> gpio_inputs_publisher_; 61 | CmdType gpio_inputs_msg_; 62 | 63 | // subscriber 64 | rclcpp::Subscription::SharedPtr gpio_outputs_command_; 65 | }; 66 | 67 | } /* namespace gpio_controller */ 68 | #endif /* GPIO_CONTROLLER__GPIO_CONTROLLER_HPP_ */ 69 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gpio_controller 5 | 1.6.0 6 | Flexiv custom gpio controller to control the GPIOs of the robot. 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | controller_interface 14 | flexiv_msgs 15 | generate_parameter_library 16 | hardware_interface 17 | pluginlib 18 | rclcpp 19 | rclcpp_lifecycle 20 | realtime_tools 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/src/gpio_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gpio_controller.cpp 3 | * @brief GPIO controller as ROS 2 controller. Adapted from 4 | * ros2_control_demos/example_10/gpio_controller 5 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 6 | * @author Flexiv 7 | */ 8 | 9 | #include "gpio_controller/gpio_controller.hpp" 10 | 11 | #include 12 | 13 | namespace gpio_controller { 14 | 15 | GPIOController::GPIOController() 16 | : controller_interface::ControllerInterface() 17 | { 18 | } 19 | 20 | controller_interface::CallbackReturn GPIOController::on_init() 21 | { 22 | try { 23 | param_listener_ = std::make_shared(get_node()); 24 | params_ = param_listener_->get_params(); 25 | initMsgs(); 26 | } catch (const std::exception& e) { 27 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 28 | return controller_interface::CallbackReturn::ERROR; 29 | } 30 | return controller_interface::CallbackReturn::SUCCESS; 31 | } 32 | 33 | controller_interface::InterfaceConfiguration GPIOController::command_interface_configuration() const 34 | { 35 | controller_interface::InterfaceConfiguration config; 36 | config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 37 | 38 | std::string robot_sn = params_.robot_sn; 39 | for (size_t i = 0; i < kIOPorts; ++i) { 40 | config.names.emplace_back(robot_sn + "_gpio/digital_output_" + std::to_string(i)); 41 | } 42 | 43 | return config; 44 | } 45 | 46 | controller_interface::InterfaceConfiguration GPIOController::state_interface_configuration() const 47 | { 48 | controller_interface::InterfaceConfiguration config; 49 | config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 50 | 51 | std::string robot_sn = params_.robot_sn; 52 | for (size_t i = 0; i < kIOPorts; ++i) { 53 | config.names.emplace_back(robot_sn + "_gpio/digital_input_" + std::to_string(i)); 54 | } 55 | 56 | return config; 57 | } 58 | 59 | controller_interface::return_type GPIOController::update( 60 | const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) 61 | { 62 | // get inputs 63 | for (size_t i = 0; i < kIOPorts; ++i) { 64 | gpio_inputs_msg_.states[i].pin = i; 65 | gpio_inputs_msg_.states[i].state = static_cast(state_interfaces_[i].get_value()); 66 | } 67 | gpio_inputs_publisher_->publish(gpio_inputs_msg_); 68 | 69 | // set outputs 70 | for (size_t i = 0; i < command_interfaces_.size(); ++i) { 71 | command_interfaces_[i].set_value(digital_outputs_cmd_[i]); 72 | } 73 | 74 | return controller_interface::return_type::OK; 75 | } 76 | 77 | controller_interface::CallbackReturn GPIOController::on_configure( 78 | const rclcpp_lifecycle::State& /*previous_state*/) 79 | { 80 | params_ = param_listener_->get_params(); 81 | 82 | std::string robot_sn = params_.robot_sn; 83 | if (robot_sn.empty()) { 84 | RCLCPP_ERROR(get_node()->get_logger(), "'robot_sn' parameter has to be specified."); 85 | return CallbackReturn::ERROR; 86 | } else { 87 | // Replace "-" with "_" in robot_sn to match the topic name 88 | std::replace(robot_sn.begin(), robot_sn.end(), '-', '_'); 89 | } 90 | 91 | try { 92 | // register publisher 93 | gpio_inputs_publisher_ = get_node()->create_publisher( 94 | "/" + robot_sn + kGPIOInputsTopic, rclcpp::SystemDefaultsQoS()); 95 | 96 | // register subscriber 97 | gpio_outputs_command_ 98 | = get_node()->create_subscription("/" + robot_sn + kGPIOOutputsTopic, 99 | rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { 100 | for (size_t i = 0; i < msg->states.size(); ++i) { 101 | if (msg->states[i].pin >= kIOPorts) { 102 | RCLCPP_WARN(get_node()->get_logger(), 103 | "Received command for pin %d, but only pins 0-15 are supported.", 104 | msg->states[i].pin); 105 | continue; 106 | } else { 107 | digital_outputs_cmd_[msg->states[i].pin] 108 | = static_cast(msg->states[i].state); 109 | } 110 | } 111 | }); 112 | } catch (...) { 113 | return LifecycleNodeInterface::CallbackReturn::ERROR; 114 | } 115 | return LifecycleNodeInterface::CallbackReturn::SUCCESS; 116 | } 117 | 118 | void GPIOController::initMsgs() 119 | { 120 | gpio_inputs_msg_.states.resize(digital_outputs_cmd_.size()); 121 | digital_outputs_cmd_.fill(0.0); 122 | } 123 | 124 | controller_interface::CallbackReturn GPIOController::on_activate( 125 | const rclcpp_lifecycle::State& /*previous_state*/) 126 | { 127 | return LifecycleNodeInterface::CallbackReturn::SUCCESS; 128 | } 129 | 130 | controller_interface::CallbackReturn GPIOController::on_deactivate( 131 | const rclcpp_lifecycle::State& /*previous_state*/) 132 | { 133 | try { 134 | // reset publisher 135 | gpio_inputs_publisher_.reset(); 136 | } catch (...) { 137 | return LifecycleNodeInterface::CallbackReturn::ERROR; 138 | } 139 | return LifecycleNodeInterface::CallbackReturn::SUCCESS; 140 | } 141 | 142 | } // namespace gpio_controller 143 | 144 | #include "pluginlib/class_list_macros.hpp" 145 | 146 | PLUGINLIB_EXPORT_CLASS(gpio_controller::GPIOController, controller_interface::ControllerInterface) 147 | -------------------------------------------------------------------------------- /flexiv_controllers/gpio_controller/src/gpio_controller_parameters.yaml: -------------------------------------------------------------------------------- 1 | gpio_controller: 2 | robot_sn: { 3 | type: string, 4 | default_value: "", 5 | description: "Robot arm serial number", 6 | } 7 | -------------------------------------------------------------------------------- /flexiv_controllers/joint_impedance_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(joint_impedance_controller LANGUAGES CXX) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(controller_interface REQUIRED) 16 | find_package(flexiv_msgs REQUIRED) 17 | find_package(hardware_interface REQUIRED) 18 | find_package(pluginlib REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rclcpp_lifecycle REQUIRED) 21 | find_package(realtime_tools REQUIRED) 22 | 23 | set(THIS_PACKAGE_INCLUDE_DEPENDS 24 | builtin_interfaces 25 | controller_interface 26 | flexiv_msgs 27 | hardware_interface 28 | pluginlib 29 | rclcpp_lifecycle 30 | rcutils 31 | realtime_tools 32 | ) 33 | 34 | add_library(${PROJECT_NAME} 35 | SHARED 36 | src/joint_impedance_controller.cpp 37 | ) 38 | 39 | target_include_directories(${PROJECT_NAME} PRIVATE include) 40 | 41 | ament_target_dependencies(${PROJECT_NAME} 42 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 43 | ) 44 | 45 | # prevent pluginlib from using boost 46 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 47 | pluginlib_export_plugin_description_file(controller_interface joint_impedance_controller.xml) 48 | 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include/${PROJECT_NAME} 52 | ) 53 | 54 | install(TARGETS ${PROJECT_NAME} 55 | ARCHIVE DESTINATION lib 56 | LIBRARY DESTINATION lib 57 | RUNTIME DESTINATION bin 58 | ) 59 | 60 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 61 | ament_export_include_directories(include) 62 | ament_export_libraries(${PROJECT_NAME}) 63 | 64 | ament_package() 65 | -------------------------------------------------------------------------------- /flexiv_controllers/joint_impedance_controller/include/joint_impedance_controller/joint_impedance_controller.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_impedance_controller.hpp 3 | * @brief Joint impedance control as ROS 2 controller. Adapted from 4 | * ros2_controllers/forward_command_controller 5 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 6 | * @author Flexiv 7 | */ 8 | 9 | #ifndef JOINT_IMPEDANCE_CONTROLLER__JOINT_IMPEDANCE_CONTROLLER_HPP_ 10 | #define JOINT_IMPEDANCE_CONTROLLER__JOINT_IMPEDANCE_CONTROLLER_HPP_ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "controller_interface/controller_interface.hpp" 17 | #include "flexiv_msgs/msg/joint_pos_vel.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "rclcpp/subscription.hpp" 20 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp" 21 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 22 | #include "rclcpp_lifecycle/state.hpp" 23 | #include "realtime_tools/realtime_buffer.hpp" 24 | 25 | namespace joint_impedance_controller { 26 | using CmdType = flexiv_msgs::msg::JointPosVel; 27 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 28 | 29 | class JointImpedanceController : public controller_interface::ControllerInterface 30 | { 31 | public: 32 | JointImpedanceController(); 33 | 34 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 35 | 36 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 37 | 38 | controller_interface::return_type update( 39 | const rclcpp::Time& time, const rclcpp::Duration& period) override; 40 | 41 | CallbackReturn on_init() override; 42 | 43 | CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; 44 | 45 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 46 | 47 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 48 | 49 | protected: 50 | std::vector joint_names_; 51 | std::vector k_p_; 52 | std::vector k_d_; 53 | const int num_joints = 7; 54 | 55 | realtime_tools::RealtimeBuffer> rt_command_ptr_; 56 | rclcpp::Subscription::SharedPtr joints_command_subscriber_; 57 | }; 58 | 59 | } /* namespace joint_impedance_controller */ 60 | #endif /* JOINT_IMPEDANCE_CONTROLLER__JOINT_IMPEDANCE_CONTROLLER_HPP_ */ 61 | -------------------------------------------------------------------------------- /flexiv_controllers/joint_impedance_controller/joint_impedance_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | The joint impedance controller commands a group of joints in effort interface. 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /flexiv_controllers/joint_impedance_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | joint_impedance_controller 5 | 1.6.0 6 | The joint impedance controller commands a group of joints in effort interface. 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | controller_interface 14 | flexiv_msgs 15 | hardware_interface 16 | pluginlib 17 | rclcpp 18 | rclcpp_lifecycle 19 | realtime_tools 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /flexiv_controllers/joint_impedance_controller/src/joint_impedance_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_impedance_controller.cpp 3 | * @brief Joint impedance control as ROS 2 controller. Adapted from 4 | * ros2_controllers/forward_command_controller 5 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 6 | * @author Flexiv 7 | */ 8 | 9 | #include "joint_impedance_controller/joint_impedance_controller.hpp" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace joint_impedance_controller { 24 | using hardware_interface::LoanedCommandInterface; 25 | 26 | JointImpedanceController::JointImpedanceController() 27 | : controller_interface::ControllerInterface() 28 | , rt_command_ptr_(nullptr) 29 | , joints_command_subscriber_(nullptr) 30 | { 31 | } 32 | 33 | controller_interface::InterfaceConfiguration 34 | JointImpedanceController::command_interface_configuration() const 35 | { 36 | controller_interface::InterfaceConfiguration config; 37 | config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 38 | config.names.reserve(joint_names_.size()); 39 | for (const auto& joint_name : joint_names_) { 40 | config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_EFFORT); 41 | } 42 | return config; 43 | } 44 | 45 | controller_interface::InterfaceConfiguration 46 | JointImpedanceController::state_interface_configuration() const 47 | { 48 | controller_interface::InterfaceConfiguration config; 49 | config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 50 | config.names.reserve(joint_names_.size() * 2); 51 | for (const auto& joint_name : joint_names_) { 52 | config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); 53 | config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); 54 | } 55 | return config; 56 | } 57 | 58 | // Fill ordered_interfaces with references to the matching interfaces 59 | // in the same order as in joint_names 60 | template 61 | bool get_ordered_interfaces(std::vector& unordered_interfaces, 62 | const std::vector& joint_names, const std::string& interface_type, 63 | std::vector>& ordered_interfaces) 64 | { 65 | for (const auto& joint_name : joint_names) { 66 | for (auto& command_interface : unordered_interfaces) { 67 | if ((command_interface.get_name() == joint_name) 68 | && (command_interface.get_interface_name() == interface_type)) { 69 | ordered_interfaces.push_back(std::ref(command_interface)); 70 | } 71 | } 72 | } 73 | 74 | return joint_names.size() == ordered_interfaces.size(); 75 | } 76 | 77 | CallbackReturn JointImpedanceController::on_init() 78 | { 79 | try { 80 | // definition of the parameters that need to be queried from the 81 | // controller configuration file with default values 82 | auto_declare>("joints", std::vector()); 83 | auto_declare>("k_p", std::vector()); 84 | auto_declare>("k_d", std::vector()); 85 | } catch (const std::exception& e) { 86 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 87 | return CallbackReturn::ERROR; 88 | } 89 | 90 | return CallbackReturn::SUCCESS; 91 | } 92 | 93 | CallbackReturn JointImpedanceController::on_configure( 94 | const rclcpp_lifecycle::State& /*previous_state*/) 95 | { 96 | // getting the names of the joints 97 | joint_names_ = get_node()->get_parameter("joints").as_string_array(); 98 | 99 | if (joint_names_.empty()) { 100 | RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter not set"); 101 | return CallbackReturn::FAILURE; 102 | } 103 | // getting the impedance parameters 104 | k_p_ = get_node()->get_parameter("k_p").as_double_array(); 105 | k_d_ = get_node()->get_parameter("k_d").as_double_array(); 106 | 107 | if (k_p_.empty()) { 108 | k_p_.resize(joint_names_.size(), 50.0); 109 | } 110 | if (k_d_.empty()) { 111 | k_d_.resize(joint_names_.size(), 10.0); 112 | } 113 | 114 | if (k_p_.size() != static_cast(num_joints)) { 115 | RCLCPP_ERROR(get_node()->get_logger(), "k_p should be of size %d but is of size %ld", 116 | num_joints, k_p_.size()); 117 | return CallbackReturn::FAILURE; 118 | } 119 | if (k_d_.size() != static_cast(num_joints)) { 120 | RCLCPP_ERROR(get_node()->get_logger(), "k_d should be of size %d but is of size %ld", 121 | num_joints, k_d_.size()); 122 | return CallbackReturn::FAILURE; 123 | } 124 | 125 | for (auto i = 0ul; i < k_p_.size(); i++) { 126 | if (k_p_[i] < 0 || k_d_[i] < 0) { 127 | RCLCPP_ERROR(get_node()->get_logger(), "Wrong Impedance parameters!"); 128 | return CallbackReturn::FAILURE; 129 | } 130 | } 131 | 132 | joints_command_subscriber_ 133 | = get_node()->create_subscription("~/commands", rclcpp::SystemDefaultsQoS(), 134 | [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); 135 | 136 | RCLCPP_INFO(get_node()->get_logger(), "Configure successful"); 137 | 138 | return CallbackReturn::SUCCESS; 139 | } 140 | 141 | controller_interface::return_type JointImpedanceController::update( 142 | const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) 143 | { 144 | auto joint_commands = rt_command_ptr_.readFromRT(); 145 | 146 | // no command received yet 147 | if (!joint_commands || !(*joint_commands)) { 148 | return controller_interface::return_type::OK; 149 | } 150 | 151 | // check command size if correct 152 | if ((*joint_commands)->positions.size() != joint_names_.size()) { 153 | RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 1000, 154 | "command size (%zu) does not match number of joints (%zu)", 155 | (*joint_commands)->positions.size(), joint_names_.size()); 156 | return controller_interface::return_type::ERROR; 157 | } 158 | 159 | // receive desired joint commands 160 | for (auto index = 0ul; index < joint_names_.size(); ++index) { 161 | double q = state_interfaces_[2 * index].get_value(); 162 | double dq = state_interfaces_[2 * index + 1].get_value(); 163 | double q_des = (*joint_commands)->positions[index]; 164 | 165 | double dq_des = 0; 166 | if ((*joint_commands)->velocities.size() == joint_names_.size()) { 167 | dq_des = (*joint_commands)->velocities[index]; 168 | } 169 | 170 | // compute torque 171 | double tau = k_p_[index] * (q_des - q) + k_d_[index] * (dq_des - dq); 172 | command_interfaces_[index].set_value(tau); 173 | } 174 | 175 | return controller_interface::return_type::OK; 176 | } 177 | 178 | CallbackReturn JointImpedanceController::on_activate( 179 | const rclcpp_lifecycle::State& /*previous_state*/) 180 | { 181 | // check if we have all resources defined in the "points" parameter 182 | // also verify that we *only* have the resources defined in the "points" 183 | // parameter 184 | std::vector> ordered_interfaces; 185 | if (!controller_interface::get_ordered_interfaces( 186 | command_interfaces_, joint_names_, hardware_interface::HW_IF_EFFORT, ordered_interfaces) 187 | || command_interfaces_.size() != ordered_interfaces.size()) { 188 | RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu position command interfaces, got %zu", 189 | joint_names_.size(), ordered_interfaces.size()); 190 | return CallbackReturn::ERROR; 191 | } 192 | 193 | // reset command buffer if a command came through callback when controller was inactive 194 | rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); 195 | 196 | RCLCPP_INFO(get_node()->get_logger(), "Activate successful"); 197 | 198 | return CallbackReturn::SUCCESS; 199 | } 200 | 201 | CallbackReturn JointImpedanceController::on_deactivate( 202 | const rclcpp_lifecycle::State& /*previous_state*/) 203 | { 204 | // reset command buffer 205 | rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); 206 | return CallbackReturn::SUCCESS; 207 | } 208 | 209 | } /* namespace joint_impedance_controller */ 210 | #include "pluginlib/class_list_macros.hpp" 211 | 212 | PLUGINLIB_EXPORT_CLASS( 213 | joint_impedance_controller::JointImpedanceController, controller_interface::ControllerInterface) 214 | -------------------------------------------------------------------------------- /flexiv_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_description) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY launch meshes rviz urdf 8 | DESTINATION share/${PROJECT_NAME}/ 9 | ) 10 | 11 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /flexiv_description/launch/view_rizon.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.conditions import IfCondition, UnlessCondition 4 | from launch_ros.parameter_descriptions import ParameterValue 5 | from launch.substitutions import ( 6 | Command, 7 | FindExecutable, 8 | LaunchConfiguration, 9 | PathJoinSubstitution, 10 | ) 11 | from launch_ros.actions import Node 12 | from launch_ros.substitutions import FindPackageShare 13 | 14 | 15 | def generate_launch_description(): 16 | pkg_share = FindPackageShare("flexiv_description") 17 | robot_sn = LaunchConfiguration("robot_sn") 18 | rizon_type = LaunchConfiguration("rizon_type") 19 | load_gripper = LaunchConfiguration("load_gripper") 20 | gripper_name = LaunchConfiguration("gripper_name") 21 | default_rviz_config_path = PathJoinSubstitution( 22 | [pkg_share, "rviz", "view_rizon.rviz"] 23 | ) 24 | robot_description_content = ParameterValue( 25 | Command( 26 | [ 27 | PathJoinSubstitution([FindExecutable(name="xacro")]), 28 | " ", 29 | PathJoinSubstitution( 30 | [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] 31 | ), 32 | " ", 33 | "robot_sn:=", 34 | robot_sn, 35 | " ", 36 | "rizon_type:=", 37 | rizon_type, 38 | " ", 39 | "load_gripper:=", 40 | load_gripper, 41 | " ", 42 | "gripper_name:=", 43 | gripper_name, 44 | ] 45 | ), 46 | value_type=str, 47 | ) 48 | 49 | # Robot state publisher 50 | robot_state_publisher_node = Node( 51 | package="robot_state_publisher", 52 | executable="robot_state_publisher", 53 | name="robot_state_publisher", 54 | output="screen", 55 | parameters=[{"robot_description": robot_description_content}], 56 | ) 57 | 58 | joint_state_publisher_node = Node( 59 | package="joint_state_publisher", 60 | executable="joint_state_publisher", 61 | name="joint_state_publisher", 62 | condition=UnlessCondition(LaunchConfiguration("gui")), 63 | ) 64 | 65 | joint_state_publisher_gui_node = Node( 66 | package="joint_state_publisher_gui", 67 | executable="joint_state_publisher_gui", 68 | name="joint_state_publisher_gui", 69 | condition=IfCondition(LaunchConfiguration("gui")), 70 | ) 71 | 72 | # RViz 73 | rviz_node = Node( 74 | package="rviz2", 75 | executable="rviz2", 76 | name="rviz2", 77 | output="screen", 78 | arguments=["-d", LaunchConfiguration("rvizconfig")], 79 | ) 80 | 81 | return LaunchDescription( 82 | [ 83 | DeclareLaunchArgument( 84 | name="robot_sn", 85 | description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", 86 | ), 87 | DeclareLaunchArgument( 88 | name="rizon_type", 89 | default_value="rizon4", 90 | description="Type of the Flexiv Rizon robot.", 91 | choices=["rizon4", "rizon4s", "rizon10", "rizon10s"], 92 | ), 93 | DeclareLaunchArgument( 94 | name="load_gripper", 95 | default_value="False", 96 | description="Flag to load the Flexiv Grav gripper", 97 | ), 98 | DeclareLaunchArgument( 99 | name="gripper_name", 100 | default_value="Flexiv-GN01", 101 | description="Full name of the gripper to be controlled", 102 | ), 103 | DeclareLaunchArgument( 104 | name="gui", 105 | default_value="False", 106 | description="Flag to enable joint_state_publisher_gui", 107 | ), 108 | DeclareLaunchArgument( 109 | name="rvizconfig", 110 | default_value=default_rviz_config_path, 111 | description="Absolute path to rviz config file", 112 | ), 113 | robot_state_publisher_node, 114 | joint_state_publisher_node, 115 | joint_state_publisher_gui_node, 116 | rviz_node, 117 | ] 118 | ) 119 | -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/collision/base.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/collision/finger_tip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/collision/finger_tip.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/collision/inner_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/collision/inner_bar.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/collision/outer_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/collision/outer_bar.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/collision/static_assembly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/collision/static_assembly.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/visual/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/visual/base.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/visual/finger_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/visual/finger_mount.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/visual/finger_tip.mtl: -------------------------------------------------------------------------------- 1 | # Free SolidWorks .OBJ Exporter v2.1 2 | # num_materials: 3 3 | 4 | newmtl mat1 5 | Ka 0.556862745098039 0.556862745098039 0.556862745098039 6 | Kd 0.556862745098039 0.556862745098039 0.556862745098039 7 | Ks 0.27843137254902 0.27843137254902 0.27843137254902 8 | d 1.0 9 | Tf 1.0 10 | illum 2 11 | Ns 599.999994039536 12 | Ni 1.0 13 | 14 | newmtl mat2 15 | Ka 0.898039215686275 0.917647058823529 0.929411764705882 16 | Kd 0.898039215686275 0.917647058823529 0.929411764705882 17 | Ks 0.449019607843137 0.458823529411765 0.464705882352941 18 | d 1.0 19 | Tf 1.0 20 | illum 2 21 | Ns 687.5 22 | Ni 1.0 23 | 24 | newmtl mat3 25 | Ka 0.223529411764706 0.223529411764706 0.223529411764706 26 | Kd 0.223529411764706 0.223529411764706 0.223529411764706 27 | Ks 0.111764705882353 0.111764705882353 0.111764705882353 28 | d 1.0 29 | Tf 1.0 30 | illum 2 31 | Ns 599.999994039536 32 | Ni 1.0 33 | -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/visual/inner_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/visual/inner_bar.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/visual/outer_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/visual/outer_bar.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/grav/visual/static_assembly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/grav/visual/static_assembly.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link0.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link1.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link2.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link3.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link4.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link5.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link6.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10/collision/link7.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10/visual/ring.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.93.6 OBJ File: '' 2 | # www.blender.org 3 | o Cylinder 4 | v 0.000000 1.000000 -1.000000 5 | v 0.000000 1.000000 1.000000 6 | v 0.195090 0.980785 1.000000 7 | v 0.195090 0.980785 -1.000000 8 | v 0.382683 0.923879 1.000000 9 | v 0.382683 0.923880 -1.000000 10 | v 0.555570 0.831470 1.000000 11 | v 0.555570 0.831470 -1.000000 12 | v 0.707107 0.707107 1.000000 13 | v 0.707107 0.707107 -1.000000 14 | v 0.831470 0.555570 1.000000 15 | v 0.831470 0.555570 -1.000000 16 | v 0.923880 0.382683 1.000000 17 | v 0.923880 0.382683 -1.000000 18 | v 0.980785 0.195090 1.000000 19 | v 0.980785 0.195090 -1.000000 20 | v 1.000000 -0.000000 1.000000 21 | v 1.000000 0.000000 -1.000000 22 | v 0.980785 -0.195090 1.000000 23 | v 0.980785 -0.195090 -1.000000 24 | v 0.923880 -0.382683 1.000000 25 | v 0.923880 -0.382683 -1.000000 26 | v 0.831470 -0.555570 1.000000 27 | v 0.831470 -0.555570 -1.000000 28 | v 0.707107 -0.707107 1.000000 29 | v 0.707107 -0.707107 -1.000000 30 | v 0.555570 -0.831470 1.000000 31 | v 0.555570 -0.831470 -1.000000 32 | v 0.382683 -0.923880 1.000000 33 | v 0.382683 -0.923879 -1.000000 34 | v 0.195090 -0.980785 1.000000 35 | v 0.195090 -0.980785 -1.000000 36 | v 0.000000 -1.000000 1.000000 37 | v 0.000000 -1.000000 -1.000000 38 | v -0.195090 -0.980785 1.000000 39 | v -0.195090 -0.980785 -1.000000 40 | v -0.382683 -0.923880 1.000000 41 | v -0.382683 -0.923879 -1.000000 42 | v -0.555570 -0.831470 1.000000 43 | v -0.555570 -0.831469 -1.000000 44 | v -0.707107 -0.707107 1.000000 45 | v -0.707107 -0.707107 -1.000000 46 | v -0.831469 -0.555570 1.000000 47 | v -0.831469 -0.555570 -1.000000 48 | v -0.923880 -0.382684 1.000000 49 | v -0.923880 -0.382684 -1.000000 50 | v -0.980785 -0.195090 1.000000 51 | v -0.980785 -0.195090 -1.000000 52 | v -1.000000 -0.000000 1.000000 53 | v -1.000000 0.000000 -1.000000 54 | v -0.980785 0.195090 1.000000 55 | v -0.980785 0.195090 -1.000000 56 | v -0.923879 0.382684 1.000000 57 | v -0.923879 0.382684 -1.000000 58 | v -0.831470 0.555570 1.000000 59 | v -0.831470 0.555570 -1.000000 60 | v -0.707107 0.707107 1.000000 61 | v -0.707107 0.707107 -1.000000 62 | v -0.555570 0.831470 1.000000 63 | v -0.555570 0.831470 -1.000000 64 | v -0.382683 0.923880 1.000000 65 | v -0.382683 0.923880 -1.000000 66 | v -0.195090 0.980785 1.000000 67 | v -0.195090 0.980785 -1.000000 68 | vt 1.000000 0.500000 69 | vt 1.000000 1.000000 70 | vt 0.968750 1.000000 71 | vt 0.968750 0.500000 72 | vt 0.937500 1.000000 73 | vt 0.937500 0.500000 74 | vt 0.906250 1.000000 75 | vt 0.906250 0.500000 76 | vt 0.875000 1.000000 77 | vt 0.875000 0.500000 78 | vt 0.843750 1.000000 79 | vt 0.843750 0.500000 80 | vt 0.812500 1.000000 81 | vt 0.812500 0.500000 82 | vt 0.781250 1.000000 83 | vt 0.781250 0.500000 84 | vt 0.750000 1.000000 85 | vt 0.750000 0.500000 86 | vt 0.718750 1.000000 87 | vt 0.718750 0.500000 88 | vt 0.687500 1.000000 89 | vt 0.687500 0.500000 90 | vt 0.656250 1.000000 91 | vt 0.656250 0.500000 92 | vt 0.625000 1.000000 93 | vt 0.625000 0.500000 94 | vt 0.593750 1.000000 95 | vt 0.593750 0.500000 96 | vt 0.562500 1.000000 97 | vt 0.562500 0.500000 98 | vt 0.531250 1.000000 99 | vt 0.531250 0.500000 100 | vt 0.500000 1.000000 101 | vt 0.500000 0.500000 102 | vt 0.468750 1.000000 103 | vt 0.468750 0.500000 104 | vt 0.437500 1.000000 105 | vt 0.437500 0.500000 106 | vt 0.406250 1.000000 107 | vt 0.406250 0.500000 108 | vt 0.375000 1.000000 109 | vt 0.375000 0.500000 110 | vt 0.343750 1.000000 111 | vt 0.343750 0.500000 112 | vt 0.312500 1.000000 113 | vt 0.312500 0.500000 114 | vt 0.281250 1.000000 115 | vt 0.281250 0.500000 116 | vt 0.250000 1.000000 117 | vt 0.250000 0.500000 118 | vt 0.218750 1.000000 119 | vt 0.218750 0.500000 120 | vt 0.187500 1.000000 121 | vt 0.187500 0.500000 122 | vt 0.156250 1.000000 123 | vt 0.156250 0.500000 124 | vt 0.125000 1.000000 125 | vt 0.125000 0.500000 126 | vt 0.093750 1.000000 127 | vt 0.093750 0.500000 128 | vt 0.062500 1.000000 129 | vt 0.062500 0.500000 130 | vt 0.296822 0.485388 131 | vt 0.250000 0.490000 132 | vt 0.203178 0.485388 133 | vt 0.158156 0.471731 134 | vt 0.116663 0.449553 135 | vt 0.080294 0.419706 136 | vt 0.050447 0.383337 137 | vt 0.028269 0.341844 138 | vt 0.014612 0.296822 139 | vt 0.010000 0.250000 140 | vt 0.014612 0.203178 141 | vt 0.028269 0.158156 142 | vt 0.050447 0.116663 143 | vt 0.080294 0.080294 144 | vt 0.116663 0.050447 145 | vt 0.158156 0.028269 146 | vt 0.203178 0.014612 147 | vt 0.250000 0.010000 148 | vt 0.296822 0.014612 149 | vt 0.341844 0.028269 150 | vt 0.383337 0.050447 151 | vt 0.419706 0.080294 152 | vt 0.449553 0.116663 153 | vt 0.471731 0.158156 154 | vt 0.485388 0.203178 155 | vt 0.490000 0.250000 156 | vt 0.485388 0.296822 157 | vt 0.471731 0.341844 158 | vt 0.449553 0.383337 159 | vt 0.419706 0.419706 160 | vt 0.383337 0.449553 161 | vt 0.341844 0.471731 162 | vt 0.031250 1.000000 163 | vt 0.031250 0.500000 164 | vt 0.000000 1.000000 165 | vt 0.000000 0.500000 166 | vt 0.750000 0.490000 167 | vt 0.796822 0.485388 168 | vt 0.841844 0.471731 169 | vt 0.883337 0.449553 170 | vt 0.919706 0.419706 171 | vt 0.949553 0.383337 172 | vt 0.971731 0.341844 173 | vt 0.985388 0.296822 174 | vt 0.990000 0.250000 175 | vt 0.985388 0.203178 176 | vt 0.971731 0.158156 177 | vt 0.949553 0.116663 178 | vt 0.919706 0.080294 179 | vt 0.883337 0.050447 180 | vt 0.841844 0.028269 181 | vt 0.796822 0.014612 182 | vt 0.750000 0.010000 183 | vt 0.703178 0.014612 184 | vt 0.658156 0.028269 185 | vt 0.616663 0.050447 186 | vt 0.580294 0.080294 187 | vt 0.550447 0.116663 188 | vt 0.528269 0.158156 189 | vt 0.514612 0.203178 190 | vt 0.510000 0.250000 191 | vt 0.514612 0.296822 192 | vt 0.528269 0.341844 193 | vt 0.550447 0.383337 194 | vt 0.580294 0.419706 195 | vt 0.616663 0.449553 196 | vt 0.658156 0.471731 197 | vt 0.703178 0.485388 198 | vn 0.0980 0.9952 0.0000 199 | vn 0.2903 0.9569 0.0000 200 | vn 0.4714 0.8819 0.0000 201 | vn 0.6344 0.7730 0.0000 202 | vn 0.7730 0.6344 0.0000 203 | vn 0.8819 0.4714 0.0000 204 | vn 0.9569 0.2903 0.0000 205 | vn 0.9952 0.0980 0.0000 206 | vn 0.9952 -0.0980 -0.0000 207 | vn 0.9569 -0.2903 -0.0000 208 | vn 0.8819 -0.4714 -0.0000 209 | vn 0.7730 -0.6344 -0.0000 210 | vn 0.6344 -0.7730 -0.0000 211 | vn 0.4714 -0.8819 -0.0000 212 | vn 0.2903 -0.9569 -0.0000 213 | vn 0.0980 -0.9952 -0.0000 214 | vn -0.0980 -0.9952 -0.0000 215 | vn -0.2903 -0.9569 -0.0000 216 | vn -0.4714 -0.8819 -0.0000 217 | vn -0.6344 -0.7730 -0.0000 218 | vn -0.7730 -0.6344 -0.0000 219 | vn -0.8819 -0.4714 -0.0000 220 | vn -0.9569 -0.2903 -0.0000 221 | vn -0.9952 -0.0980 -0.0000 222 | vn -0.9952 0.0980 0.0000 223 | vn -0.9569 0.2903 0.0000 224 | vn -0.8819 0.4714 0.0000 225 | vn -0.7730 0.6344 0.0000 226 | vn -0.6344 0.7730 0.0000 227 | vn -0.4714 0.8819 0.0000 228 | vn -0.0000 -0.0000 1.0000 229 | vn -0.2903 0.9569 0.0000 230 | vn -0.0980 0.9952 0.0000 231 | vn 0.0000 0.0000 -1.0000 232 | usemtl Material 233 | s 1 234 | f 1/1/1 2/2/1 3/3/1 4/4/1 235 | f 4/4/2 3/3/2 5/5/2 6/6/2 236 | f 6/6/3 5/5/3 7/7/3 8/8/3 237 | f 8/8/4 7/7/4 9/9/4 10/10/4 238 | f 10/10/5 9/9/5 11/11/5 12/12/5 239 | f 12/12/6 11/11/6 13/13/6 14/14/6 240 | f 14/14/7 13/13/7 15/15/7 16/16/7 241 | f 16/16/8 15/15/8 17/17/8 18/18/8 242 | f 18/18/9 17/17/9 19/19/9 20/20/9 243 | f 20/20/10 19/19/10 21/21/10 22/22/10 244 | f 22/22/11 21/21/11 23/23/11 24/24/11 245 | f 24/24/12 23/23/12 25/25/12 26/26/12 246 | f 26/26/13 25/25/13 27/27/13 28/28/13 247 | f 28/28/14 27/27/14 29/29/14 30/30/14 248 | f 30/30/15 29/29/15 31/31/15 32/32/15 249 | f 32/32/16 31/31/16 33/33/16 34/34/16 250 | f 34/34/17 33/33/17 35/35/17 36/36/17 251 | f 36/36/18 35/35/18 37/37/18 38/38/18 252 | f 38/38/19 37/37/19 39/39/19 40/40/19 253 | f 40/40/20 39/39/20 41/41/20 42/42/20 254 | f 42/42/21 41/41/21 43/43/21 44/44/21 255 | f 44/44/22 43/43/22 45/45/22 46/46/22 256 | f 46/46/23 45/45/23 47/47/23 48/48/23 257 | f 48/48/24 47/47/24 49/49/24 50/50/24 258 | f 50/50/25 49/49/25 51/51/25 52/52/25 259 | f 52/52/26 51/51/26 53/53/26 54/54/26 260 | f 54/54/27 53/53/27 55/55/27 56/56/27 261 | f 56/56/28 55/55/28 57/57/28 58/58/28 262 | f 58/58/29 57/57/29 59/59/29 60/60/29 263 | f 60/60/30 59/59/30 61/61/30 62/62/30 264 | f 3/63/31 2/64/31 63/65/31 61/66/31 59/67/31 57/68/31 55/69/31 53/70/31 51/71/31 49/72/31 47/73/31 45/74/31 43/75/31 41/76/31 39/77/31 37/78/31 35/79/31 33/80/31 31/81/31 29/82/31 27/83/31 25/84/31 23/85/31 21/86/31 19/87/31 17/88/31 15/89/31 13/90/31 11/91/31 9/92/31 7/93/31 5/94/31 265 | f 62/62/32 61/61/32 63/95/32 64/96/32 266 | f 64/96/33 63/95/33 2/97/33 1/98/33 267 | f 1/99/34 4/100/34 6/101/34 8/102/34 10/103/34 12/104/34 14/105/34 16/106/34 18/107/34 20/108/34 22/109/34 24/110/34 26/111/34 28/112/34 30/113/34 32/114/34 34/115/34 36/116/34 38/117/34 40/118/34 42/119/34 44/120/34 46/121/34 48/122/34 50/123/34 52/124/34 54/125/34 56/126/34 58/127/34 60/128/34 62/129/34 64/130/34 268 | -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link0.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link1.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link2.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link3.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link4.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link5.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link6.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon10s/collision/link7.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon10s/visual/ring.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.93.6 OBJ File: '' 2 | # www.blender.org 3 | o Cylinder 4 | v 0.000000 1.000000 -1.000000 5 | v 0.000000 1.000000 1.000000 6 | v 0.195090 0.980785 1.000000 7 | v 0.195090 0.980785 -1.000000 8 | v 0.382683 0.923879 1.000000 9 | v 0.382683 0.923880 -1.000000 10 | v 0.555570 0.831470 1.000000 11 | v 0.555570 0.831470 -1.000000 12 | v 0.707107 0.707107 1.000000 13 | v 0.707107 0.707107 -1.000000 14 | v 0.831470 0.555570 1.000000 15 | v 0.831470 0.555570 -1.000000 16 | v 0.923880 0.382683 1.000000 17 | v 0.923880 0.382683 -1.000000 18 | v 0.980785 0.195090 1.000000 19 | v 0.980785 0.195090 -1.000000 20 | v 1.000000 -0.000000 1.000000 21 | v 1.000000 0.000000 -1.000000 22 | v 0.980785 -0.195090 1.000000 23 | v 0.980785 -0.195090 -1.000000 24 | v 0.923880 -0.382683 1.000000 25 | v 0.923880 -0.382683 -1.000000 26 | v 0.831470 -0.555570 1.000000 27 | v 0.831470 -0.555570 -1.000000 28 | v 0.707107 -0.707107 1.000000 29 | v 0.707107 -0.707107 -1.000000 30 | v 0.555570 -0.831470 1.000000 31 | v 0.555570 -0.831470 -1.000000 32 | v 0.382683 -0.923880 1.000000 33 | v 0.382683 -0.923879 -1.000000 34 | v 0.195090 -0.980785 1.000000 35 | v 0.195090 -0.980785 -1.000000 36 | v 0.000000 -1.000000 1.000000 37 | v 0.000000 -1.000000 -1.000000 38 | v -0.195090 -0.980785 1.000000 39 | v -0.195090 -0.980785 -1.000000 40 | v -0.382683 -0.923880 1.000000 41 | v -0.382683 -0.923879 -1.000000 42 | v -0.555570 -0.831470 1.000000 43 | v -0.555570 -0.831469 -1.000000 44 | v -0.707107 -0.707107 1.000000 45 | v -0.707107 -0.707107 -1.000000 46 | v -0.831469 -0.555570 1.000000 47 | v -0.831469 -0.555570 -1.000000 48 | v -0.923880 -0.382684 1.000000 49 | v -0.923880 -0.382684 -1.000000 50 | v -0.980785 -0.195090 1.000000 51 | v -0.980785 -0.195090 -1.000000 52 | v -1.000000 -0.000000 1.000000 53 | v -1.000000 0.000000 -1.000000 54 | v -0.980785 0.195090 1.000000 55 | v -0.980785 0.195090 -1.000000 56 | v -0.923879 0.382684 1.000000 57 | v -0.923879 0.382684 -1.000000 58 | v -0.831470 0.555570 1.000000 59 | v -0.831470 0.555570 -1.000000 60 | v -0.707107 0.707107 1.000000 61 | v -0.707107 0.707107 -1.000000 62 | v -0.555570 0.831470 1.000000 63 | v -0.555570 0.831470 -1.000000 64 | v -0.382683 0.923880 1.000000 65 | v -0.382683 0.923880 -1.000000 66 | v -0.195090 0.980785 1.000000 67 | v -0.195090 0.980785 -1.000000 68 | vt 1.000000 0.500000 69 | vt 1.000000 1.000000 70 | vt 0.968750 1.000000 71 | vt 0.968750 0.500000 72 | vt 0.937500 1.000000 73 | vt 0.937500 0.500000 74 | vt 0.906250 1.000000 75 | vt 0.906250 0.500000 76 | vt 0.875000 1.000000 77 | vt 0.875000 0.500000 78 | vt 0.843750 1.000000 79 | vt 0.843750 0.500000 80 | vt 0.812500 1.000000 81 | vt 0.812500 0.500000 82 | vt 0.781250 1.000000 83 | vt 0.781250 0.500000 84 | vt 0.750000 1.000000 85 | vt 0.750000 0.500000 86 | vt 0.718750 1.000000 87 | vt 0.718750 0.500000 88 | vt 0.687500 1.000000 89 | vt 0.687500 0.500000 90 | vt 0.656250 1.000000 91 | vt 0.656250 0.500000 92 | vt 0.625000 1.000000 93 | vt 0.625000 0.500000 94 | vt 0.593750 1.000000 95 | vt 0.593750 0.500000 96 | vt 0.562500 1.000000 97 | vt 0.562500 0.500000 98 | vt 0.531250 1.000000 99 | vt 0.531250 0.500000 100 | vt 0.500000 1.000000 101 | vt 0.500000 0.500000 102 | vt 0.468750 1.000000 103 | vt 0.468750 0.500000 104 | vt 0.437500 1.000000 105 | vt 0.437500 0.500000 106 | vt 0.406250 1.000000 107 | vt 0.406250 0.500000 108 | vt 0.375000 1.000000 109 | vt 0.375000 0.500000 110 | vt 0.343750 1.000000 111 | vt 0.343750 0.500000 112 | vt 0.312500 1.000000 113 | vt 0.312500 0.500000 114 | vt 0.281250 1.000000 115 | vt 0.281250 0.500000 116 | vt 0.250000 1.000000 117 | vt 0.250000 0.500000 118 | vt 0.218750 1.000000 119 | vt 0.218750 0.500000 120 | vt 0.187500 1.000000 121 | vt 0.187500 0.500000 122 | vt 0.156250 1.000000 123 | vt 0.156250 0.500000 124 | vt 0.125000 1.000000 125 | vt 0.125000 0.500000 126 | vt 0.093750 1.000000 127 | vt 0.093750 0.500000 128 | vt 0.062500 1.000000 129 | vt 0.062500 0.500000 130 | vt 0.296822 0.485388 131 | vt 0.250000 0.490000 132 | vt 0.203178 0.485388 133 | vt 0.158156 0.471731 134 | vt 0.116663 0.449553 135 | vt 0.080294 0.419706 136 | vt 0.050447 0.383337 137 | vt 0.028269 0.341844 138 | vt 0.014612 0.296822 139 | vt 0.010000 0.250000 140 | vt 0.014612 0.203178 141 | vt 0.028269 0.158156 142 | vt 0.050447 0.116663 143 | vt 0.080294 0.080294 144 | vt 0.116663 0.050447 145 | vt 0.158156 0.028269 146 | vt 0.203178 0.014612 147 | vt 0.250000 0.010000 148 | vt 0.296822 0.014612 149 | vt 0.341844 0.028269 150 | vt 0.383337 0.050447 151 | vt 0.419706 0.080294 152 | vt 0.449553 0.116663 153 | vt 0.471731 0.158156 154 | vt 0.485388 0.203178 155 | vt 0.490000 0.250000 156 | vt 0.485388 0.296822 157 | vt 0.471731 0.341844 158 | vt 0.449553 0.383337 159 | vt 0.419706 0.419706 160 | vt 0.383337 0.449553 161 | vt 0.341844 0.471731 162 | vt 0.031250 1.000000 163 | vt 0.031250 0.500000 164 | vt 0.000000 1.000000 165 | vt 0.000000 0.500000 166 | vt 0.750000 0.490000 167 | vt 0.796822 0.485388 168 | vt 0.841844 0.471731 169 | vt 0.883337 0.449553 170 | vt 0.919706 0.419706 171 | vt 0.949553 0.383337 172 | vt 0.971731 0.341844 173 | vt 0.985388 0.296822 174 | vt 0.990000 0.250000 175 | vt 0.985388 0.203178 176 | vt 0.971731 0.158156 177 | vt 0.949553 0.116663 178 | vt 0.919706 0.080294 179 | vt 0.883337 0.050447 180 | vt 0.841844 0.028269 181 | vt 0.796822 0.014612 182 | vt 0.750000 0.010000 183 | vt 0.703178 0.014612 184 | vt 0.658156 0.028269 185 | vt 0.616663 0.050447 186 | vt 0.580294 0.080294 187 | vt 0.550447 0.116663 188 | vt 0.528269 0.158156 189 | vt 0.514612 0.203178 190 | vt 0.510000 0.250000 191 | vt 0.514612 0.296822 192 | vt 0.528269 0.341844 193 | vt 0.550447 0.383337 194 | vt 0.580294 0.419706 195 | vt 0.616663 0.449553 196 | vt 0.658156 0.471731 197 | vt 0.703178 0.485388 198 | vn 0.0980 0.9952 0.0000 199 | vn 0.2903 0.9569 0.0000 200 | vn 0.4714 0.8819 0.0000 201 | vn 0.6344 0.7730 0.0000 202 | vn 0.7730 0.6344 0.0000 203 | vn 0.8819 0.4714 0.0000 204 | vn 0.9569 0.2903 0.0000 205 | vn 0.9952 0.0980 0.0000 206 | vn 0.9952 -0.0980 -0.0000 207 | vn 0.9569 -0.2903 -0.0000 208 | vn 0.8819 -0.4714 -0.0000 209 | vn 0.7730 -0.6344 -0.0000 210 | vn 0.6344 -0.7730 -0.0000 211 | vn 0.4714 -0.8819 -0.0000 212 | vn 0.2903 -0.9569 -0.0000 213 | vn 0.0980 -0.9952 -0.0000 214 | vn -0.0980 -0.9952 -0.0000 215 | vn -0.2903 -0.9569 -0.0000 216 | vn -0.4714 -0.8819 -0.0000 217 | vn -0.6344 -0.7730 -0.0000 218 | vn -0.7730 -0.6344 -0.0000 219 | vn -0.8819 -0.4714 -0.0000 220 | vn -0.9569 -0.2903 -0.0000 221 | vn -0.9952 -0.0980 -0.0000 222 | vn -0.9952 0.0980 0.0000 223 | vn -0.9569 0.2903 0.0000 224 | vn -0.8819 0.4714 0.0000 225 | vn -0.7730 0.6344 0.0000 226 | vn -0.6344 0.7730 0.0000 227 | vn -0.4714 0.8819 0.0000 228 | vn -0.0000 -0.0000 1.0000 229 | vn -0.2903 0.9569 0.0000 230 | vn -0.0980 0.9952 0.0000 231 | vn 0.0000 0.0000 -1.0000 232 | usemtl Material 233 | s 1 234 | f 1/1/1 2/2/1 3/3/1 4/4/1 235 | f 4/4/2 3/3/2 5/5/2 6/6/2 236 | f 6/6/3 5/5/3 7/7/3 8/8/3 237 | f 8/8/4 7/7/4 9/9/4 10/10/4 238 | f 10/10/5 9/9/5 11/11/5 12/12/5 239 | f 12/12/6 11/11/6 13/13/6 14/14/6 240 | f 14/14/7 13/13/7 15/15/7 16/16/7 241 | f 16/16/8 15/15/8 17/17/8 18/18/8 242 | f 18/18/9 17/17/9 19/19/9 20/20/9 243 | f 20/20/10 19/19/10 21/21/10 22/22/10 244 | f 22/22/11 21/21/11 23/23/11 24/24/11 245 | f 24/24/12 23/23/12 25/25/12 26/26/12 246 | f 26/26/13 25/25/13 27/27/13 28/28/13 247 | f 28/28/14 27/27/14 29/29/14 30/30/14 248 | f 30/30/15 29/29/15 31/31/15 32/32/15 249 | f 32/32/16 31/31/16 33/33/16 34/34/16 250 | f 34/34/17 33/33/17 35/35/17 36/36/17 251 | f 36/36/18 35/35/18 37/37/18 38/38/18 252 | f 38/38/19 37/37/19 39/39/19 40/40/19 253 | f 40/40/20 39/39/20 41/41/20 42/42/20 254 | f 42/42/21 41/41/21 43/43/21 44/44/21 255 | f 44/44/22 43/43/22 45/45/22 46/46/22 256 | f 46/46/23 45/45/23 47/47/23 48/48/23 257 | f 48/48/24 47/47/24 49/49/24 50/50/24 258 | f 50/50/25 49/49/25 51/51/25 52/52/25 259 | f 52/52/26 51/51/26 53/53/26 54/54/26 260 | f 54/54/27 53/53/27 55/55/27 56/56/27 261 | f 56/56/28 55/55/28 57/57/28 58/58/28 262 | f 58/58/29 57/57/29 59/59/29 60/60/29 263 | f 60/60/30 59/59/30 61/61/30 62/62/30 264 | f 3/63/31 2/64/31 63/65/31 61/66/31 59/67/31 57/68/31 55/69/31 53/70/31 51/71/31 49/72/31 47/73/31 45/74/31 43/75/31 41/76/31 39/77/31 37/78/31 35/79/31 33/80/31 31/81/31 29/82/31 27/83/31 25/84/31 23/85/31 21/86/31 19/87/31 17/88/31 15/89/31 13/90/31 11/91/31 9/92/31 7/93/31 5/94/31 265 | f 62/62/32 61/61/32 63/95/32 64/96/32 266 | f 64/96/33 63/95/33 2/97/33 1/98/33 267 | f 1/99/34 4/100/34 6/101/34 8/102/34 10/103/34 12/104/34 14/105/34 16/106/34 18/107/34 20/108/34 22/109/34 24/110/34 26/111/34 28/112/34 30/113/34 32/114/34 34/115/34 36/116/34 38/117/34 40/118/34 42/119/34 44/120/34 46/121/34 48/122/34 50/123/34 52/124/34 54/125/34 56/126/34 58/127/34 60/128/34 62/129/34 64/130/34 268 | -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link0.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link1.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link2.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link3.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link4.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link5.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link6.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4/collision/link7.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4/visual/ring.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.93.6 OBJ File: '' 2 | # www.blender.org 3 | o Cylinder 4 | v 0.000000 1.000000 -1.000000 5 | v 0.000000 1.000000 1.000000 6 | v 0.195090 0.980785 1.000000 7 | v 0.195090 0.980785 -1.000000 8 | v 0.382683 0.923879 1.000000 9 | v 0.382683 0.923880 -1.000000 10 | v 0.555570 0.831470 1.000000 11 | v 0.555570 0.831470 -1.000000 12 | v 0.707107 0.707107 1.000000 13 | v 0.707107 0.707107 -1.000000 14 | v 0.831470 0.555570 1.000000 15 | v 0.831470 0.555570 -1.000000 16 | v 0.923880 0.382683 1.000000 17 | v 0.923880 0.382683 -1.000000 18 | v 0.980785 0.195090 1.000000 19 | v 0.980785 0.195090 -1.000000 20 | v 1.000000 -0.000000 1.000000 21 | v 1.000000 0.000000 -1.000000 22 | v 0.980785 -0.195090 1.000000 23 | v 0.980785 -0.195090 -1.000000 24 | v 0.923880 -0.382683 1.000000 25 | v 0.923880 -0.382683 -1.000000 26 | v 0.831470 -0.555570 1.000000 27 | v 0.831470 -0.555570 -1.000000 28 | v 0.707107 -0.707107 1.000000 29 | v 0.707107 -0.707107 -1.000000 30 | v 0.555570 -0.831470 1.000000 31 | v 0.555570 -0.831470 -1.000000 32 | v 0.382683 -0.923880 1.000000 33 | v 0.382683 -0.923879 -1.000000 34 | v 0.195090 -0.980785 1.000000 35 | v 0.195090 -0.980785 -1.000000 36 | v 0.000000 -1.000000 1.000000 37 | v 0.000000 -1.000000 -1.000000 38 | v -0.195090 -0.980785 1.000000 39 | v -0.195090 -0.980785 -1.000000 40 | v -0.382683 -0.923880 1.000000 41 | v -0.382683 -0.923879 -1.000000 42 | v -0.555570 -0.831470 1.000000 43 | v -0.555570 -0.831469 -1.000000 44 | v -0.707107 -0.707107 1.000000 45 | v -0.707107 -0.707107 -1.000000 46 | v -0.831469 -0.555570 1.000000 47 | v -0.831469 -0.555570 -1.000000 48 | v -0.923880 -0.382684 1.000000 49 | v -0.923880 -0.382684 -1.000000 50 | v -0.980785 -0.195090 1.000000 51 | v -0.980785 -0.195090 -1.000000 52 | v -1.000000 -0.000000 1.000000 53 | v -1.000000 0.000000 -1.000000 54 | v -0.980785 0.195090 1.000000 55 | v -0.980785 0.195090 -1.000000 56 | v -0.923879 0.382684 1.000000 57 | v -0.923879 0.382684 -1.000000 58 | v -0.831470 0.555570 1.000000 59 | v -0.831470 0.555570 -1.000000 60 | v -0.707107 0.707107 1.000000 61 | v -0.707107 0.707107 -1.000000 62 | v -0.555570 0.831470 1.000000 63 | v -0.555570 0.831470 -1.000000 64 | v -0.382683 0.923880 1.000000 65 | v -0.382683 0.923880 -1.000000 66 | v -0.195090 0.980785 1.000000 67 | v -0.195090 0.980785 -1.000000 68 | vt 1.000000 0.500000 69 | vt 1.000000 1.000000 70 | vt 0.968750 1.000000 71 | vt 0.968750 0.500000 72 | vt 0.937500 1.000000 73 | vt 0.937500 0.500000 74 | vt 0.906250 1.000000 75 | vt 0.906250 0.500000 76 | vt 0.875000 1.000000 77 | vt 0.875000 0.500000 78 | vt 0.843750 1.000000 79 | vt 0.843750 0.500000 80 | vt 0.812500 1.000000 81 | vt 0.812500 0.500000 82 | vt 0.781250 1.000000 83 | vt 0.781250 0.500000 84 | vt 0.750000 1.000000 85 | vt 0.750000 0.500000 86 | vt 0.718750 1.000000 87 | vt 0.718750 0.500000 88 | vt 0.687500 1.000000 89 | vt 0.687500 0.500000 90 | vt 0.656250 1.000000 91 | vt 0.656250 0.500000 92 | vt 0.625000 1.000000 93 | vt 0.625000 0.500000 94 | vt 0.593750 1.000000 95 | vt 0.593750 0.500000 96 | vt 0.562500 1.000000 97 | vt 0.562500 0.500000 98 | vt 0.531250 1.000000 99 | vt 0.531250 0.500000 100 | vt 0.500000 1.000000 101 | vt 0.500000 0.500000 102 | vt 0.468750 1.000000 103 | vt 0.468750 0.500000 104 | vt 0.437500 1.000000 105 | vt 0.437500 0.500000 106 | vt 0.406250 1.000000 107 | vt 0.406250 0.500000 108 | vt 0.375000 1.000000 109 | vt 0.375000 0.500000 110 | vt 0.343750 1.000000 111 | vt 0.343750 0.500000 112 | vt 0.312500 1.000000 113 | vt 0.312500 0.500000 114 | vt 0.281250 1.000000 115 | vt 0.281250 0.500000 116 | vt 0.250000 1.000000 117 | vt 0.250000 0.500000 118 | vt 0.218750 1.000000 119 | vt 0.218750 0.500000 120 | vt 0.187500 1.000000 121 | vt 0.187500 0.500000 122 | vt 0.156250 1.000000 123 | vt 0.156250 0.500000 124 | vt 0.125000 1.000000 125 | vt 0.125000 0.500000 126 | vt 0.093750 1.000000 127 | vt 0.093750 0.500000 128 | vt 0.062500 1.000000 129 | vt 0.062500 0.500000 130 | vt 0.296822 0.485388 131 | vt 0.250000 0.490000 132 | vt 0.203178 0.485388 133 | vt 0.158156 0.471731 134 | vt 0.116663 0.449553 135 | vt 0.080294 0.419706 136 | vt 0.050447 0.383337 137 | vt 0.028269 0.341844 138 | vt 0.014612 0.296822 139 | vt 0.010000 0.250000 140 | vt 0.014612 0.203178 141 | vt 0.028269 0.158156 142 | vt 0.050447 0.116663 143 | vt 0.080294 0.080294 144 | vt 0.116663 0.050447 145 | vt 0.158156 0.028269 146 | vt 0.203178 0.014612 147 | vt 0.250000 0.010000 148 | vt 0.296822 0.014612 149 | vt 0.341844 0.028269 150 | vt 0.383337 0.050447 151 | vt 0.419706 0.080294 152 | vt 0.449553 0.116663 153 | vt 0.471731 0.158156 154 | vt 0.485388 0.203178 155 | vt 0.490000 0.250000 156 | vt 0.485388 0.296822 157 | vt 0.471731 0.341844 158 | vt 0.449553 0.383337 159 | vt 0.419706 0.419706 160 | vt 0.383337 0.449553 161 | vt 0.341844 0.471731 162 | vt 0.031250 1.000000 163 | vt 0.031250 0.500000 164 | vt 0.000000 1.000000 165 | vt 0.000000 0.500000 166 | vt 0.750000 0.490000 167 | vt 0.796822 0.485388 168 | vt 0.841844 0.471731 169 | vt 0.883337 0.449553 170 | vt 0.919706 0.419706 171 | vt 0.949553 0.383337 172 | vt 0.971731 0.341844 173 | vt 0.985388 0.296822 174 | vt 0.990000 0.250000 175 | vt 0.985388 0.203178 176 | vt 0.971731 0.158156 177 | vt 0.949553 0.116663 178 | vt 0.919706 0.080294 179 | vt 0.883337 0.050447 180 | vt 0.841844 0.028269 181 | vt 0.796822 0.014612 182 | vt 0.750000 0.010000 183 | vt 0.703178 0.014612 184 | vt 0.658156 0.028269 185 | vt 0.616663 0.050447 186 | vt 0.580294 0.080294 187 | vt 0.550447 0.116663 188 | vt 0.528269 0.158156 189 | vt 0.514612 0.203178 190 | vt 0.510000 0.250000 191 | vt 0.514612 0.296822 192 | vt 0.528269 0.341844 193 | vt 0.550447 0.383337 194 | vt 0.580294 0.419706 195 | vt 0.616663 0.449553 196 | vt 0.658156 0.471731 197 | vt 0.703178 0.485388 198 | vn 0.0980 0.9952 0.0000 199 | vn 0.2903 0.9569 0.0000 200 | vn 0.4714 0.8819 0.0000 201 | vn 0.6344 0.7730 0.0000 202 | vn 0.7730 0.6344 0.0000 203 | vn 0.8819 0.4714 0.0000 204 | vn 0.9569 0.2903 0.0000 205 | vn 0.9952 0.0980 0.0000 206 | vn 0.9952 -0.0980 -0.0000 207 | vn 0.9569 -0.2903 -0.0000 208 | vn 0.8819 -0.4714 -0.0000 209 | vn 0.7730 -0.6344 -0.0000 210 | vn 0.6344 -0.7730 -0.0000 211 | vn 0.4714 -0.8819 -0.0000 212 | vn 0.2903 -0.9569 -0.0000 213 | vn 0.0980 -0.9952 -0.0000 214 | vn -0.0980 -0.9952 -0.0000 215 | vn -0.2903 -0.9569 -0.0000 216 | vn -0.4714 -0.8819 -0.0000 217 | vn -0.6344 -0.7730 -0.0000 218 | vn -0.7730 -0.6344 -0.0000 219 | vn -0.8819 -0.4714 -0.0000 220 | vn -0.9569 -0.2903 -0.0000 221 | vn -0.9952 -0.0980 -0.0000 222 | vn -0.9952 0.0980 0.0000 223 | vn -0.9569 0.2903 0.0000 224 | vn -0.8819 0.4714 0.0000 225 | vn -0.7730 0.6344 0.0000 226 | vn -0.6344 0.7730 0.0000 227 | vn -0.4714 0.8819 0.0000 228 | vn -0.0000 -0.0000 1.0000 229 | vn -0.2903 0.9569 0.0000 230 | vn -0.0980 0.9952 0.0000 231 | vn 0.0000 0.0000 -1.0000 232 | usemtl Material 233 | s 1 234 | f 1/1/1 2/2/1 3/3/1 4/4/1 235 | f 4/4/2 3/3/2 5/5/2 6/6/2 236 | f 6/6/3 5/5/3 7/7/3 8/8/3 237 | f 8/8/4 7/7/4 9/9/4 10/10/4 238 | f 10/10/5 9/9/5 11/11/5 12/12/5 239 | f 12/12/6 11/11/6 13/13/6 14/14/6 240 | f 14/14/7 13/13/7 15/15/7 16/16/7 241 | f 16/16/8 15/15/8 17/17/8 18/18/8 242 | f 18/18/9 17/17/9 19/19/9 20/20/9 243 | f 20/20/10 19/19/10 21/21/10 22/22/10 244 | f 22/22/11 21/21/11 23/23/11 24/24/11 245 | f 24/24/12 23/23/12 25/25/12 26/26/12 246 | f 26/26/13 25/25/13 27/27/13 28/28/13 247 | f 28/28/14 27/27/14 29/29/14 30/30/14 248 | f 30/30/15 29/29/15 31/31/15 32/32/15 249 | f 32/32/16 31/31/16 33/33/16 34/34/16 250 | f 34/34/17 33/33/17 35/35/17 36/36/17 251 | f 36/36/18 35/35/18 37/37/18 38/38/18 252 | f 38/38/19 37/37/19 39/39/19 40/40/19 253 | f 40/40/20 39/39/20 41/41/20 42/42/20 254 | f 42/42/21 41/41/21 43/43/21 44/44/21 255 | f 44/44/22 43/43/22 45/45/22 46/46/22 256 | f 46/46/23 45/45/23 47/47/23 48/48/23 257 | f 48/48/24 47/47/24 49/49/24 50/50/24 258 | f 50/50/25 49/49/25 51/51/25 52/52/25 259 | f 52/52/26 51/51/26 53/53/26 54/54/26 260 | f 54/54/27 53/53/27 55/55/27 56/56/27 261 | f 56/56/28 55/55/28 57/57/28 58/58/28 262 | f 58/58/29 57/57/29 59/59/29 60/60/29 263 | f 60/60/30 59/59/30 61/61/30 62/62/30 264 | f 3/63/31 2/64/31 63/65/31 61/66/31 59/67/31 57/68/31 55/69/31 53/70/31 51/71/31 49/72/31 47/73/31 45/74/31 43/75/31 41/76/31 39/77/31 37/78/31 35/79/31 33/80/31 31/81/31 29/82/31 27/83/31 25/84/31 23/85/31 21/86/31 19/87/31 17/88/31 15/89/31 13/90/31 11/91/31 9/92/31 7/93/31 5/94/31 265 | f 62/62/32 61/61/32 63/95/32 64/96/32 266 | f 64/96/33 63/95/33 2/97/33 1/98/33 267 | f 1/99/34 4/100/34 6/101/34 8/102/34 10/103/34 12/104/34 14/105/34 16/106/34 18/107/34 20/108/34 22/109/34 24/110/34 26/111/34 28/112/34 30/113/34 32/114/34 34/115/34 36/116/34 38/117/34 40/118/34 42/119/34 44/120/34 46/121/34 48/122/34 50/123/34 52/124/34 54/125/34 56/126/34 58/127/34 60/128/34 62/129/34 64/130/34 268 | -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link0.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link1.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link2.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link3.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link4.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link5.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link6.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_description/meshes/rizon4s/collision/link7.stl -------------------------------------------------------------------------------- /flexiv_description/meshes/rizon4s/visual/ring.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.93.6 OBJ File: '' 2 | # www.blender.org 3 | o Cylinder 4 | v 0.000000 1.000000 -1.000000 5 | v 0.000000 1.000000 1.000000 6 | v 0.195090 0.980785 1.000000 7 | v 0.195090 0.980785 -1.000000 8 | v 0.382683 0.923879 1.000000 9 | v 0.382683 0.923880 -1.000000 10 | v 0.555570 0.831470 1.000000 11 | v 0.555570 0.831470 -1.000000 12 | v 0.707107 0.707107 1.000000 13 | v 0.707107 0.707107 -1.000000 14 | v 0.831470 0.555570 1.000000 15 | v 0.831470 0.555570 -1.000000 16 | v 0.923880 0.382683 1.000000 17 | v 0.923880 0.382683 -1.000000 18 | v 0.980785 0.195090 1.000000 19 | v 0.980785 0.195090 -1.000000 20 | v 1.000000 -0.000000 1.000000 21 | v 1.000000 0.000000 -1.000000 22 | v 0.980785 -0.195090 1.000000 23 | v 0.980785 -0.195090 -1.000000 24 | v 0.923880 -0.382683 1.000000 25 | v 0.923880 -0.382683 -1.000000 26 | v 0.831470 -0.555570 1.000000 27 | v 0.831470 -0.555570 -1.000000 28 | v 0.707107 -0.707107 1.000000 29 | v 0.707107 -0.707107 -1.000000 30 | v 0.555570 -0.831470 1.000000 31 | v 0.555570 -0.831470 -1.000000 32 | v 0.382683 -0.923880 1.000000 33 | v 0.382683 -0.923879 -1.000000 34 | v 0.195090 -0.980785 1.000000 35 | v 0.195090 -0.980785 -1.000000 36 | v 0.000000 -1.000000 1.000000 37 | v 0.000000 -1.000000 -1.000000 38 | v -0.195090 -0.980785 1.000000 39 | v -0.195090 -0.980785 -1.000000 40 | v -0.382683 -0.923880 1.000000 41 | v -0.382683 -0.923879 -1.000000 42 | v -0.555570 -0.831470 1.000000 43 | v -0.555570 -0.831469 -1.000000 44 | v -0.707107 -0.707107 1.000000 45 | v -0.707107 -0.707107 -1.000000 46 | v -0.831469 -0.555570 1.000000 47 | v -0.831469 -0.555570 -1.000000 48 | v -0.923880 -0.382684 1.000000 49 | v -0.923880 -0.382684 -1.000000 50 | v -0.980785 -0.195090 1.000000 51 | v -0.980785 -0.195090 -1.000000 52 | v -1.000000 -0.000000 1.000000 53 | v -1.000000 0.000000 -1.000000 54 | v -0.980785 0.195090 1.000000 55 | v -0.980785 0.195090 -1.000000 56 | v -0.923879 0.382684 1.000000 57 | v -0.923879 0.382684 -1.000000 58 | v -0.831470 0.555570 1.000000 59 | v -0.831470 0.555570 -1.000000 60 | v -0.707107 0.707107 1.000000 61 | v -0.707107 0.707107 -1.000000 62 | v -0.555570 0.831470 1.000000 63 | v -0.555570 0.831470 -1.000000 64 | v -0.382683 0.923880 1.000000 65 | v -0.382683 0.923880 -1.000000 66 | v -0.195090 0.980785 1.000000 67 | v -0.195090 0.980785 -1.000000 68 | vt 1.000000 0.500000 69 | vt 1.000000 1.000000 70 | vt 0.968750 1.000000 71 | vt 0.968750 0.500000 72 | vt 0.937500 1.000000 73 | vt 0.937500 0.500000 74 | vt 0.906250 1.000000 75 | vt 0.906250 0.500000 76 | vt 0.875000 1.000000 77 | vt 0.875000 0.500000 78 | vt 0.843750 1.000000 79 | vt 0.843750 0.500000 80 | vt 0.812500 1.000000 81 | vt 0.812500 0.500000 82 | vt 0.781250 1.000000 83 | vt 0.781250 0.500000 84 | vt 0.750000 1.000000 85 | vt 0.750000 0.500000 86 | vt 0.718750 1.000000 87 | vt 0.718750 0.500000 88 | vt 0.687500 1.000000 89 | vt 0.687500 0.500000 90 | vt 0.656250 1.000000 91 | vt 0.656250 0.500000 92 | vt 0.625000 1.000000 93 | vt 0.625000 0.500000 94 | vt 0.593750 1.000000 95 | vt 0.593750 0.500000 96 | vt 0.562500 1.000000 97 | vt 0.562500 0.500000 98 | vt 0.531250 1.000000 99 | vt 0.531250 0.500000 100 | vt 0.500000 1.000000 101 | vt 0.500000 0.500000 102 | vt 0.468750 1.000000 103 | vt 0.468750 0.500000 104 | vt 0.437500 1.000000 105 | vt 0.437500 0.500000 106 | vt 0.406250 1.000000 107 | vt 0.406250 0.500000 108 | vt 0.375000 1.000000 109 | vt 0.375000 0.500000 110 | vt 0.343750 1.000000 111 | vt 0.343750 0.500000 112 | vt 0.312500 1.000000 113 | vt 0.312500 0.500000 114 | vt 0.281250 1.000000 115 | vt 0.281250 0.500000 116 | vt 0.250000 1.000000 117 | vt 0.250000 0.500000 118 | vt 0.218750 1.000000 119 | vt 0.218750 0.500000 120 | vt 0.187500 1.000000 121 | vt 0.187500 0.500000 122 | vt 0.156250 1.000000 123 | vt 0.156250 0.500000 124 | vt 0.125000 1.000000 125 | vt 0.125000 0.500000 126 | vt 0.093750 1.000000 127 | vt 0.093750 0.500000 128 | vt 0.062500 1.000000 129 | vt 0.062500 0.500000 130 | vt 0.296822 0.485388 131 | vt 0.250000 0.490000 132 | vt 0.203178 0.485388 133 | vt 0.158156 0.471731 134 | vt 0.116663 0.449553 135 | vt 0.080294 0.419706 136 | vt 0.050447 0.383337 137 | vt 0.028269 0.341844 138 | vt 0.014612 0.296822 139 | vt 0.010000 0.250000 140 | vt 0.014612 0.203178 141 | vt 0.028269 0.158156 142 | vt 0.050447 0.116663 143 | vt 0.080294 0.080294 144 | vt 0.116663 0.050447 145 | vt 0.158156 0.028269 146 | vt 0.203178 0.014612 147 | vt 0.250000 0.010000 148 | vt 0.296822 0.014612 149 | vt 0.341844 0.028269 150 | vt 0.383337 0.050447 151 | vt 0.419706 0.080294 152 | vt 0.449553 0.116663 153 | vt 0.471731 0.158156 154 | vt 0.485388 0.203178 155 | vt 0.490000 0.250000 156 | vt 0.485388 0.296822 157 | vt 0.471731 0.341844 158 | vt 0.449553 0.383337 159 | vt 0.419706 0.419706 160 | vt 0.383337 0.449553 161 | vt 0.341844 0.471731 162 | vt 0.031250 1.000000 163 | vt 0.031250 0.500000 164 | vt 0.000000 1.000000 165 | vt 0.000000 0.500000 166 | vt 0.750000 0.490000 167 | vt 0.796822 0.485388 168 | vt 0.841844 0.471731 169 | vt 0.883337 0.449553 170 | vt 0.919706 0.419706 171 | vt 0.949553 0.383337 172 | vt 0.971731 0.341844 173 | vt 0.985388 0.296822 174 | vt 0.990000 0.250000 175 | vt 0.985388 0.203178 176 | vt 0.971731 0.158156 177 | vt 0.949553 0.116663 178 | vt 0.919706 0.080294 179 | vt 0.883337 0.050447 180 | vt 0.841844 0.028269 181 | vt 0.796822 0.014612 182 | vt 0.750000 0.010000 183 | vt 0.703178 0.014612 184 | vt 0.658156 0.028269 185 | vt 0.616663 0.050447 186 | vt 0.580294 0.080294 187 | vt 0.550447 0.116663 188 | vt 0.528269 0.158156 189 | vt 0.514612 0.203178 190 | vt 0.510000 0.250000 191 | vt 0.514612 0.296822 192 | vt 0.528269 0.341844 193 | vt 0.550447 0.383337 194 | vt 0.580294 0.419706 195 | vt 0.616663 0.449553 196 | vt 0.658156 0.471731 197 | vt 0.703178 0.485388 198 | vn 0.0980 0.9952 0.0000 199 | vn 0.2903 0.9569 0.0000 200 | vn 0.4714 0.8819 0.0000 201 | vn 0.6344 0.7730 0.0000 202 | vn 0.7730 0.6344 0.0000 203 | vn 0.8819 0.4714 0.0000 204 | vn 0.9569 0.2903 0.0000 205 | vn 0.9952 0.0980 0.0000 206 | vn 0.9952 -0.0980 -0.0000 207 | vn 0.9569 -0.2903 -0.0000 208 | vn 0.8819 -0.4714 -0.0000 209 | vn 0.7730 -0.6344 -0.0000 210 | vn 0.6344 -0.7730 -0.0000 211 | vn 0.4714 -0.8819 -0.0000 212 | vn 0.2903 -0.9569 -0.0000 213 | vn 0.0980 -0.9952 -0.0000 214 | vn -0.0980 -0.9952 -0.0000 215 | vn -0.2903 -0.9569 -0.0000 216 | vn -0.4714 -0.8819 -0.0000 217 | vn -0.6344 -0.7730 -0.0000 218 | vn -0.7730 -0.6344 -0.0000 219 | vn -0.8819 -0.4714 -0.0000 220 | vn -0.9569 -0.2903 -0.0000 221 | vn -0.9952 -0.0980 -0.0000 222 | vn -0.9952 0.0980 0.0000 223 | vn -0.9569 0.2903 0.0000 224 | vn -0.8819 0.4714 0.0000 225 | vn -0.7730 0.6344 0.0000 226 | vn -0.6344 0.7730 0.0000 227 | vn -0.4714 0.8819 0.0000 228 | vn -0.0000 -0.0000 1.0000 229 | vn -0.2903 0.9569 0.0000 230 | vn -0.0980 0.9952 0.0000 231 | vn 0.0000 0.0000 -1.0000 232 | usemtl Material 233 | s 1 234 | f 1/1/1 2/2/1 3/3/1 4/4/1 235 | f 4/4/2 3/3/2 5/5/2 6/6/2 236 | f 6/6/3 5/5/3 7/7/3 8/8/3 237 | f 8/8/4 7/7/4 9/9/4 10/10/4 238 | f 10/10/5 9/9/5 11/11/5 12/12/5 239 | f 12/12/6 11/11/6 13/13/6 14/14/6 240 | f 14/14/7 13/13/7 15/15/7 16/16/7 241 | f 16/16/8 15/15/8 17/17/8 18/18/8 242 | f 18/18/9 17/17/9 19/19/9 20/20/9 243 | f 20/20/10 19/19/10 21/21/10 22/22/10 244 | f 22/22/11 21/21/11 23/23/11 24/24/11 245 | f 24/24/12 23/23/12 25/25/12 26/26/12 246 | f 26/26/13 25/25/13 27/27/13 28/28/13 247 | f 28/28/14 27/27/14 29/29/14 30/30/14 248 | f 30/30/15 29/29/15 31/31/15 32/32/15 249 | f 32/32/16 31/31/16 33/33/16 34/34/16 250 | f 34/34/17 33/33/17 35/35/17 36/36/17 251 | f 36/36/18 35/35/18 37/37/18 38/38/18 252 | f 38/38/19 37/37/19 39/39/19 40/40/19 253 | f 40/40/20 39/39/20 41/41/20 42/42/20 254 | f 42/42/21 41/41/21 43/43/21 44/44/21 255 | f 44/44/22 43/43/22 45/45/22 46/46/22 256 | f 46/46/23 45/45/23 47/47/23 48/48/23 257 | f 48/48/24 47/47/24 49/49/24 50/50/24 258 | f 50/50/25 49/49/25 51/51/25 52/52/25 259 | f 52/52/26 51/51/26 53/53/26 54/54/26 260 | f 54/54/27 53/53/27 55/55/27 56/56/27 261 | f 56/56/28 55/55/28 57/57/28 58/58/28 262 | f 58/58/29 57/57/29 59/59/29 60/60/29 263 | f 60/60/30 59/59/30 61/61/30 62/62/30 264 | f 3/63/31 2/64/31 63/65/31 61/66/31 59/67/31 57/68/31 55/69/31 53/70/31 51/71/31 49/72/31 47/73/31 45/74/31 43/75/31 41/76/31 39/77/31 37/78/31 35/79/31 33/80/31 31/81/31 29/82/31 27/83/31 25/84/31 23/85/31 21/86/31 19/87/31 17/88/31 15/89/31 13/90/31 11/91/31 9/92/31 7/93/31 5/94/31 265 | f 62/62/32 61/61/32 63/95/32 64/96/32 266 | f 64/96/33 63/95/33 2/97/33 1/98/33 267 | f 1/99/34 4/100/34 6/101/34 8/102/34 10/103/34 12/104/34 14/105/34 16/106/34 18/107/34 20/108/34 22/109/34 24/110/34 26/111/34 28/112/34 30/113/34 32/114/34 34/115/34 36/116/34 38/117/34 40/118/34 42/119/34 44/120/34 46/121/34 48/122/34 50/123/34 52/124/34 54/125/34 56/126/34 58/127/34 60/128/34 62/129/34 64/130/34 268 | -------------------------------------------------------------------------------- /flexiv_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_description 5 | 1.6.0 6 | URDF description for Flexiv robots 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | urdf 14 | xacro 15 | rviz2 16 | joint_state_publisher 17 | joint_state_publisher_gui 18 | robot_state_publisher 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /flexiv_description/rviz/view_rizon.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1/Description Topic1 10 | Splitter Ratio: 0.5 11 | Tree Height: 634 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 1 30 | Class: rviz_default_plugins/Grid 31 | Color: 160; 160; 164 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 10 44 | Reference Frame: 45 | Value: true 46 | - Alpha: 1 47 | Class: rviz_default_plugins/RobotModel 48 | Collision Enabled: false 49 | Description File: "" 50 | Description Source: Topic 51 | Description Topic: 52 | Depth: 5 53 | Durability Policy: Volatile 54 | History Policy: Keep Last 55 | Reliability Policy: Reliable 56 | Value: /robot_description 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | base_link: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | flange: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | link1: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | link2: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | link3: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | link4: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | link5: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | link6: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | link7: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | world: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Name: RobotModel 113 | TF Prefix: "" 114 | Update Interval: 0 115 | Value: true 116 | Visual Enabled: true 117 | Enabled: true 118 | Global Options: 119 | Background Color: 48; 48; 48 120 | Fixed Frame: world 121 | Frame Rate: 30 122 | Name: root 123 | Tools: 124 | - Class: rviz_default_plugins/Interact 125 | Hide Inactive Objects: true 126 | - Class: rviz_default_plugins/MoveCamera 127 | - Class: rviz_default_plugins/Select 128 | - Class: rviz_default_plugins/FocusCamera 129 | - Class: rviz_default_plugins/Measure 130 | Line color: 128; 128; 0 131 | - Class: rviz_default_plugins/SetInitialPose 132 | Topic: 133 | Depth: 5 134 | Durability Policy: Volatile 135 | History Policy: Keep Last 136 | Reliability Policy: Reliable 137 | Value: /initialpose 138 | - Class: rviz_default_plugins/SetGoal 139 | Topic: 140 | Depth: 5 141 | Durability Policy: Volatile 142 | History Policy: Keep Last 143 | Reliability Policy: Reliable 144 | Value: /goal_pose 145 | - Class: rviz_default_plugins/PublishPoint 146 | Single click: true 147 | Topic: 148 | Depth: 5 149 | Durability Policy: Volatile 150 | History Policy: Keep Last 151 | Reliability Policy: Reliable 152 | Value: /clicked_point 153 | Transformation: 154 | Current: 155 | Class: rviz_default_plugins/TF 156 | Value: true 157 | Views: 158 | Current: 159 | Class: rviz_default_plugins/Orbit 160 | Distance: 2.4956274032592773 161 | Enable Stereo Rendering: 162 | Stereo Eye Separation: 0.05999999865889549 163 | Stereo Focal Distance: 1 164 | Swap Stereo Eyes: false 165 | Value: false 166 | Focal Point: 167 | X: -0.10040300339460373 168 | Y: 0.27989906072616577 169 | Z: 0.3314317762851715 170 | Focal Shape Fixed Size: true 171 | Focal Shape Size: 0.05000000074505806 172 | Invert Z Axis: false 173 | Name: Current View 174 | Near Clip Distance: 0.009999999776482582 175 | Pitch: 0.7053980827331543 176 | Target Frame: 177 | Value: Orbit (rviz) 178 | Yaw: 5.313577651977539 179 | Saved: ~ 180 | Window Geometry: 181 | Displays: 182 | collapsed: false 183 | Height: 863 184 | Hide Left Dock: false 185 | Hide Right Dock: false 186 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000305fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000305000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000305fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000305000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003cf0000030500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 187 | Selection: 188 | collapsed: false 189 | Tool Properties: 190 | collapsed: false 191 | Views: 192 | collapsed: false 193 | Width: 1600 194 | X: 1920 195 | Y: 0 196 | -------------------------------------------------------------------------------- /flexiv_description/urdf/flexiv_arm.materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /flexiv_description/urdf/rizon.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /flexiv_description/urdf/rizon10.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10 | 11 | 12 | 13 | 14 | mock_components/GenericSystem 15 | ${fake_sensor_commands} 16 | 0.0 17 | 18 | 19 | flexiv_hardware/FlexivHardwareInterface 20 | ${robot_sn} 21 | ${prefix} 22 | 23 | 24 | 25 | 26 | 27 | -2.7925 28 | 2.7925 29 | 30 | 31 | -1.7453 32 | 1.7453 33 | 34 | 35 | 36 | 37 | 0.0 38 | 39 | 40 | 41 | 42 | 43 | 44 | -2.6704 45 | 2.6704 46 | 47 | 48 | -1.7453 49 | 1.7453 50 | 51 | 52 | 53 | 54 | -0.69813 55 | 56 | 57 | 58 | 59 | 60 | 61 | -2.7925 62 | 2.7925 63 | 64 | 65 | -2.0944 66 | 2.0944 67 | 68 | 69 | 70 | 71 | 0.0 72 | 73 | 74 | 75 | 76 | 77 | 78 | -2.7053 79 | 2.7053 80 | 81 | 82 | -2.0944 83 | 2.0944 84 | 85 | 86 | 87 | 88 | 1.570796 89 | 90 | 91 | 92 | 93 | 94 | 95 | -2.9671 96 | 2.9671 97 | 98 | 99 | -3.8397 100 | 3.8397 101 | 102 | 103 | 104 | 105 | 0.0 106 | 107 | 108 | 109 | 110 | 111 | 112 | -1.3963 113 | 4.5379 114 | 115 | 116 | -3.8397 117 | 3.8397 118 | 119 | 120 | 121 | 122 | 0.69813 123 | 124 | 125 | 126 | 127 | 128 | 129 | -2.9671 130 | 2.9671 131 | 132 | 133 | -3.8397 134 | 3.8397 135 | 136 | 137 | 138 | 139 | 0.0 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | -------------------------------------------------------------------------------- /flexiv_description/urdf/rizon4.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10 | 11 | 12 | 13 | 14 | mock_components/GenericSystem 15 | ${fake_sensor_commands} 16 | 0.0 17 | 18 | 19 | flexiv_hardware/FlexivHardwareInterface 20 | ${robot_sn} 21 | ${prefix} 22 | 23 | 24 | 25 | 26 | 27 | -2.7925 28 | 2.7925 29 | 30 | 31 | -2.0944 32 | 2.0944 33 | 34 | 35 | 36 | 37 | 0.0 38 | 39 | 40 | 41 | 42 | 43 | 44 | -2.2689 45 | 2.2689 46 | 47 | 48 | -2.0944 49 | 2.0944 50 | 51 | 52 | 53 | 54 | -0.69813 55 | 56 | 57 | 58 | 59 | 60 | 61 | -2.9671 62 | 2.9671 63 | 64 | 65 | -2.4435 66 | 2.4435 67 | 68 | 69 | 70 | 71 | 0.0 72 | 73 | 74 | 75 | 76 | 77 | 78 | -1.8675 79 | 2.6878 80 | 81 | 82 | -2.4435 83 | 2.4435 84 | 85 | 86 | 87 | 88 | 1.570796 89 | 90 | 91 | 92 | 93 | 94 | 95 | -2.9671 96 | 2.9671 97 | 98 | 99 | -4.8869 100 | 4.8869 101 | 102 | 103 | 104 | 105 | 0.0 106 | 107 | 108 | 109 | 110 | 111 | 112 | -1.3963 113 | 4.5379 114 | 115 | 116 | -4.8869 117 | 4.8869 118 | 119 | 120 | 121 | 122 | 0.69813 123 | 124 | 125 | 126 | 127 | 128 | 129 | -2.9671 130 | 2.9671 131 | 132 | 133 | -4.8869 134 | 4.8869 135 | 136 | 137 | 138 | 139 | 0.0 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | -------------------------------------------------------------------------------- /flexiv_description/urdf/rizon_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /flexiv_gripper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_gripper) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(control_msgs REQUIRED) 16 | find_package(flexiv_hardware REQUIRED) 17 | find_package(flexiv_msgs REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(rclcpp_action REQUIRED) 20 | find_package(rclcpp_components REQUIRED) 21 | find_package(sensor_msgs REQUIRED) 22 | find_package(std_srvs REQUIRED) 23 | 24 | add_library(gripper_action_server 25 | SHARED 26 | src/gripper_action_server.cpp) 27 | 28 | target_include_directories(gripper_action_server PRIVATE 29 | ${CMAKE_CURRENT_SOURCE_DIR}/include 30 | ${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/include) 31 | 32 | # Set static library 33 | message("OS: ${CMAKE_SYSTEM_NAME}") 34 | message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") 35 | if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 36 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") 37 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a") 38 | elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") 39 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a") 40 | else() 41 | message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") 42 | endif() 43 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 44 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64") 45 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/libflexiv_rdk.arm64-darwin.a") 46 | else() 47 | message(FATAL_ERROR "Mac with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") 48 | endif() 49 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") 50 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64") 51 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/flexiv_rdk.win_amd64.lib") 52 | else() 53 | message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") 54 | endif() 55 | endif() 56 | 57 | target_link_libraries(gripper_action_server 58 | ${RDK_STATIC_LIBRARY}) 59 | 60 | ament_target_dependencies(gripper_action_server 61 | control_msgs 62 | flexiv_hardware 63 | flexiv_msgs 64 | rclcpp 65 | rclcpp_action 66 | rclcpp_components 67 | sensor_msgs 68 | std_srvs) 69 | 70 | rclcpp_components_register_node(gripper_action_server 71 | PLUGIN "flexiv_gripper::GripperActionServer" 72 | EXECUTABLE flexiv_gripper_node) 73 | 74 | install(TARGETS gripper_action_server 75 | ARCHIVE DESTINATION lib 76 | LIBRARY DESTINATION lib 77 | RUNTIME DESTINATION bin) 78 | 79 | install(DIRECTORY config launch 80 | DESTINATION share/${PROJECT_NAME}/) 81 | 82 | ament_package() 83 | -------------------------------------------------------------------------------- /flexiv_gripper/config/flexiv_gripper_node.yaml: -------------------------------------------------------------------------------- 1 | flexiv_gripper: 2 | ros__parameters: 3 | state_publish_rate: 50 # Hz 4 | feedback_publish_rate: 30 # Hz 5 | default_velocity: 0.1 # m/s 6 | default_max_force: 20.0 # N 7 | -------------------------------------------------------------------------------- /flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gripper_action_server.hpp 3 | * @brief Header file for GripperActionServer class 4 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 5 | */ 6 | 7 | #ifndef FLEXIV_GRIPPER__GRIPPER_ACTION_SERVER_HPP_ 8 | #define FLEXIV_GRIPPER__GRIPPER_ACTION_SERVER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // ROS 18 | #include "control_msgs/action/gripper_command.hpp" 19 | #include "flexiv_msgs/action/grasp.hpp" 20 | #include "flexiv_msgs/action/move.hpp" 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "rclcpp_action/rclcpp_action.hpp" 23 | #include "sensor_msgs/msg/joint_state.hpp" 24 | #include "std_srvs/srv/trigger.hpp" 25 | 26 | // Flexiv 27 | #include "flexiv/rdk/data.hpp" 28 | #include "flexiv/rdk/gripper.hpp" 29 | #include "flexiv/rdk/robot.hpp" 30 | #include "flexiv/rdk/tool.hpp" 31 | 32 | namespace { 33 | 34 | const int kDefaultStatePublishRate = 30; // [Hz] 35 | const int kDefaultFeedbackPublishRate = 10; // [Hz] 36 | const double kDefaultVelocity = 0.1; // [m/s] 37 | const double kDefaultMaxForce = 20; // [N] 38 | } 39 | 40 | namespace flexiv_gripper { 41 | 42 | /** 43 | * @brief Check if the result of the asynchronous command is ready. 44 | * @tparam T The return type of the future. 45 | * @param[in] result_future The future object to check. 46 | * @param[in] timeout Future wait timeout. 47 | * @return True if the function has finished, false otherwise. 48 | */ 49 | template 50 | bool IsResultReady(std::future& result_future, std::chrono::nanoseconds timeout) 51 | { 52 | return result_future.wait_for(timeout) == std::future_status::ready; 53 | } 54 | 55 | class GripperActionServer : public rclcpp::Node 56 | { 57 | 58 | public: 59 | using Grasp = flexiv_msgs::action::Grasp; 60 | using GoalHandleGrasp = rclcpp_action::ServerGoalHandle; 61 | 62 | using Move = flexiv_msgs::action::Move; 63 | using GoalHandleMove = rclcpp_action::ServerGoalHandle; 64 | 65 | using GripperCommand = control_msgs::action::GripperCommand; 66 | using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle; 67 | 68 | using Trigger = std_srvs::srv::Trigger; 69 | 70 | explicit GripperActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 71 | 72 | private: 73 | // gripper actions 74 | enum class GripperAction 75 | { 76 | kGrasp, 77 | kMove, 78 | kGripperCommand 79 | }; 80 | 81 | // return string of the gripper action 82 | static std::string GetGripperActionName(GripperAction action) 83 | { 84 | switch (action) { 85 | case GripperAction::kGrasp: 86 | return {"Grasping"}; 87 | case GripperAction::kMove: 88 | return {"Moving"}; 89 | case GripperAction::kGripperCommand: 90 | return {"GripperCommand"}; 91 | default: 92 | throw std::invalid_argument("Invalid gripper action"); 93 | } 94 | }; 95 | 96 | // Flexiv RDK 97 | std::unique_ptr robot_; 98 | std::unique_ptr gripper_; 99 | std::unique_ptr tool_; 100 | 101 | rclcpp_action::Server::SharedPtr grasp_action_server_; 102 | rclcpp_action::Server::SharedPtr move_action_server_; 103 | rclcpp_action::Server::SharedPtr gripper_command_action_server_; 104 | rclcpp::Service::SharedPtr stop_service_; 105 | rclcpp::TimerBase::SharedPtr state_publish_timer_; 106 | 107 | std::mutex gripper_states_mutex_; 108 | flexiv::rdk::GripperStates current_gripper_states_; 109 | 110 | double default_velocity_; 111 | double default_max_force_; 112 | std::chrono::nanoseconds future_wait_timeout_ {0}; 113 | 114 | // Gripper joint states publisher 115 | rclcpp::Publisher::SharedPtr gripper_joint_states_publisher_; 116 | std::vector gripper_joint_names_; 117 | 118 | /** 119 | * @brief Publish the current gripper states. 120 | */ 121 | void PublishGripperStates(); 122 | 123 | /** 124 | * @brief Stop the gripper. 125 | * @param[out] response Success or failure of the service call. 126 | */ 127 | void StopServiceCallback(const std::shared_ptr& response); 128 | 129 | /** 130 | * @brief Handle the gripper action cancel request. 131 | * @param[in] action Gripper action to cancel. 132 | */ 133 | rclcpp_action::CancelResponse HandleCancel(GripperAction action); 134 | 135 | /** 136 | * @brief Handle the gripper action goal request. 137 | * @param[in] action Gripper action to handle. 138 | */ 139 | rclcpp_action::GoalResponse HandleGoal(GripperAction action); 140 | 141 | /** 142 | * @brief Perform the gripper move action. 143 | */ 144 | void ExecuteMove(const std::shared_ptr& goal_handle); 145 | 146 | /** 147 | * @brief Perform the gripper grasp action. 148 | */ 149 | void ExecuteGrasp(const std::shared_ptr& goal_handle); 150 | 151 | /** 152 | * @brief Perform the gripper command action. 153 | * @param[in] goal_handle The goal handle of the action. 154 | */ 155 | void ExecuteGripperCommand(const std::shared_ptr& goal_handle); 156 | 157 | /** 158 | * @brief Execute the gripper command and return the result. 159 | * @param[in] goal_handle The goal handle of the action. 160 | * @param[in] command The RDK function to execute the gripper command. 161 | */ 162 | void ExecuteGripperCommandHelper(const std::shared_ptr& goal_handle, 163 | const std::function& command); 164 | 165 | /** 166 | * @brief Execute the gripper command and return the result. 167 | * @tparam T Gripper action message type (Grasp or Move). 168 | * @param[in] goal_handle The goal handle of the action. 169 | * @param[in] action The gripper action to execute. 170 | * @param[in] command The RDK function to execute the gripper command. 171 | */ 172 | template 173 | void ExecuteCommand(const std::shared_ptr>& goal_handle, 174 | GripperAction action, const std::function& command) 175 | { 176 | const auto action_name = GetGripperActionName(action); 177 | RCLCPP_INFO(this->get_logger(), "Gripper %s action has been received", action_name.c_str()); 178 | 179 | auto command_execution_result = CommandExecutionResult(command); 180 | 181 | std::future> result_future 182 | = std::async(std::launch::async, command_execution_result); 183 | 184 | while (!IsResultReady(result_future, future_wait_timeout_) && rclcpp::ok()) { 185 | if (goal_handle->is_canceling()) { 186 | gripper_->Stop(); 187 | auto result = result_future.get(); 188 | RCLCPP_INFO( 189 | this->get_logger(), "Gripper %s action has been canceled", action_name.c_str()); 190 | goal_handle->canceled(result); 191 | return; 192 | } 193 | PublishGripperStatesFeedback(goal_handle); 194 | } 195 | 196 | if (rclcpp::ok()) { 197 | const auto result = result_future.get(); 198 | if (result->success) { 199 | RCLCPP_INFO(this->get_logger(), "Gripper %s action has been completed", 200 | action_name.c_str()); 201 | goal_handle->succeed(result); 202 | } else { 203 | RCLCPP_ERROR( 204 | this->get_logger(), "Gripper %s action has failed", action_name.c_str()); 205 | goal_handle->abort(result); 206 | } 207 | } 208 | } 209 | 210 | /** 211 | * @brief Return the command execution result. 212 | * @tparam T Gripper action message type (Grasp or Move). 213 | * @param[in] command The function to execute the gripper command. 214 | * @return Success or failure of the command execution. 215 | */ 216 | template 217 | std::function()> CommandExecutionResult( 218 | const std::function& command) 219 | { 220 | return std::function()>([command]() { 221 | auto result = std::make_shared(); 222 | try { 223 | command(); 224 | result->success = true; 225 | } catch (const std::exception& e) { 226 | result->success = false; 227 | result->error = e.what(); 228 | } 229 | return result; 230 | }); 231 | } 232 | 233 | /** 234 | * @brief Publish the gripper states feedback in the action server. 235 | * @tparam T Gripper action message type (Grasp or Move). 236 | * @param[in] goal_handle The goal handle of the action. 237 | */ 238 | template 239 | void PublishGripperStatesFeedback( 240 | const std::shared_ptr>& goal_handle) 241 | { 242 | auto feedback = std::make_shared(); 243 | std::lock_guard lock(gripper_states_mutex_); 244 | feedback->current_width = current_gripper_states_.width; 245 | feedback->current_force = current_gripper_states_.force; 246 | feedback->moving = current_gripper_states_.is_moving; 247 | goal_handle->publish_feedback(feedback); 248 | } 249 | 250 | /** 251 | * @brief Publish the gripper command feedback in the action server. 252 | * @param[in] goal_handle The goal handle of the action. 253 | */ 254 | void PublishGripperCommandFeedback( 255 | const std::shared_ptr>& goal_handle); 256 | }; 257 | 258 | } // namespace flexiv_gripper 259 | 260 | #endif /* FLEXIV_GRIPPER__GRIPPER_ACTION_SERVER_HPP_ */ 261 | -------------------------------------------------------------------------------- /flexiv_gripper/launch/flexiv_gripper.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.conditions import IfCondition, UnlessCondition 4 | from launch.substitutions import ( 5 | LaunchConfiguration, 6 | PathJoinSubstitution, 7 | ) 8 | from launch_ros.actions import Node 9 | from launch_ros.substitutions import FindPackageShare 10 | 11 | 12 | def generate_launch_description(): 13 | robot_sn_param_name = "robot_sn" 14 | gripper_name_param_name = "gripper_name" 15 | use_fake_hardware_param_name = "use_fake_hardware" 16 | gripper_joint_names_param_name = "gripper_joint_names" 17 | 18 | # Declare arguments 19 | declared_arguments = [] 20 | 21 | declared_arguments.append( 22 | DeclareLaunchArgument( 23 | robot_sn_param_name, 24 | description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", 25 | ) 26 | ) 27 | 28 | declared_arguments.append( 29 | DeclareLaunchArgument( 30 | gripper_name_param_name, 31 | description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device", 32 | default_value="Flexiv-GN01", 33 | ) 34 | ) 35 | 36 | declared_arguments.append( 37 | DeclareLaunchArgument( 38 | use_fake_hardware_param_name, 39 | default_value="false", 40 | description="Start gripper with fake gripper joint states.", 41 | ) 42 | ) 43 | 44 | declared_arguments.append( 45 | DeclareLaunchArgument( 46 | gripper_joint_names_param_name, 47 | description="Control joint names of the mounted gripper.", 48 | default_value="[finger_width_joint]", 49 | ) 50 | ) 51 | 52 | # Initialize arguments 53 | robot_sn = LaunchConfiguration(robot_sn_param_name) 54 | gripper_name = LaunchConfiguration(gripper_name_param_name) 55 | use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name) 56 | gripper_joint_names = LaunchConfiguration(gripper_joint_names_param_name) 57 | 58 | gripper_config_file = PathJoinSubstitution( 59 | [FindPackageShare("flexiv_gripper"), "config", "flexiv_gripper_node.yaml"] 60 | ) 61 | 62 | # Flexiv gripper node 63 | flexiv_gripper_node = Node( 64 | package="flexiv_gripper", 65 | executable="flexiv_gripper_node", 66 | name="flexiv_gripper_node", 67 | parameters=[ 68 | { 69 | "robot_sn": robot_sn, 70 | "gripper_name": gripper_name, 71 | "gripper_joint_names": gripper_joint_names, 72 | }, 73 | gripper_config_file, 74 | ], 75 | condition=UnlessCondition(use_fake_hardware), 76 | ) 77 | 78 | nodes = [flexiv_gripper_node] 79 | 80 | return LaunchDescription(declared_arguments + nodes) 81 | -------------------------------------------------------------------------------- /flexiv_gripper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_gripper 5 | 1.6.0 6 | Package implementing the action server of Flexiv gripper control 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | control_msgs 14 | flexiv_hardware 15 | flexiv_msgs 16 | rclcpp 17 | rclcpp_action 18 | rclcpp_components 19 | sensor_msgs 20 | std_srvs 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /flexiv_hardware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_hardware) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(rclcpp REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | find_package(hardware_interface REQUIRED) 18 | find_package(pluginlib REQUIRED) 19 | 20 | # spdlog 21 | find_package(spdlog REQUIRED) 22 | if(spdlog_FOUND) 23 | message(STATUS "Found spdlog: ${spdlog_DIR}") 24 | endif() 25 | 26 | # Fast-DDS (Fast-RTPS) 27 | find_package(fastrtps 2.6.7 REQUIRED) 28 | if(fastrtps_FOUND) 29 | message(STATUS "Found fastrtps: ${fastrtps_DIR}") 30 | endif() 31 | 32 | add_library( 33 | ${PROJECT_NAME} 34 | SHARED 35 | src/flexiv_hardware_interface.cpp 36 | ) 37 | 38 | target_include_directories( 39 | ${PROJECT_NAME} 40 | PRIVATE 41 | ${CMAKE_CURRENT_SOURCE_DIR}/include 42 | ${CMAKE_CURRENT_SOURCE_DIR}/rdk/include 43 | ) 44 | 45 | # Set static library 46 | message("OS: ${CMAKE_SYSTEM_NAME}") 47 | message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") 48 | if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 49 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") 50 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a") 51 | elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") 52 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a") 53 | else() 54 | message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") 55 | endif() 56 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 57 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64") 58 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/libflexiv_rdk.arm64-darwin.a") 59 | else() 60 | message(FATAL_ERROR "Mac with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") 61 | endif() 62 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") 63 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64") 64 | set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/rdk/lib/flexiv_rdk.win_amd64.lib") 65 | else() 66 | message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") 67 | endif() 68 | endif() 69 | 70 | target_link_libraries(${PROJECT_NAME} 71 | ${RDK_STATIC_LIBRARY} 72 | spdlog::spdlog 73 | fastrtps 74 | ) 75 | 76 | # Link ROS packages 77 | ament_target_dependencies( 78 | ${PROJECT_NAME} 79 | hardware_interface 80 | pluginlib 81 | rclcpp 82 | rclcpp_lifecycle 83 | ) 84 | 85 | pluginlib_export_plugin_description_file(hardware_interface flexiv_hardware_interface_plugin.xml) 86 | 87 | # INSTALL 88 | install( 89 | TARGETS 90 | ${PROJECT_NAME} 91 | DESTINATION lib 92 | ) 93 | 94 | install( 95 | DIRECTORY include/ 96 | DESTINATION include 97 | ) 98 | 99 | if(BUILD_TESTING) 100 | find_package(ament_cmake_gtest REQUIRED) 101 | endif() 102 | 103 | # EXPORTS 104 | ament_export_include_directories( 105 | include 106 | ) 107 | 108 | ament_export_libraries( 109 | ${PROJECT_NAME} 110 | ) 111 | 112 | ament_export_dependencies( 113 | hardware_interface 114 | pluginlib 115 | rclcpp 116 | rclcpp_lifecycle 117 | ) 118 | 119 | ament_package() 120 | -------------------------------------------------------------------------------- /flexiv_hardware/flexiv_hardware_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | ROS 2 Control Hardware interface for the Flexiv robots. This driver 8 | uses Flexiv RDK. Position, Velocity and Effort interfaces are supported. 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file flexiv_hardware_interface.hpp 3 | * @brief Hardware interface to Flexiv robots for ROS 2 control. Adapted from 4 | * ros2_control_demos/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp 5 | * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. 6 | * @author Flexiv 7 | */ 8 | 9 | #ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ 10 | #define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // ros2_control hardware_interface 25 | #include 26 | #include 27 | #include 28 | 29 | #include "flexiv_hardware/visibility_control.h" 30 | 31 | // Flexiv 32 | #include "flexiv/rdk/robot.hpp" 33 | 34 | namespace flexiv_hardware { 35 | 36 | /** Robot joint space degree of freedoms */ 37 | constexpr size_t kJointDoF = 7; 38 | 39 | enum StoppingInterface 40 | { 41 | NONE, 42 | STOP_POSITION, 43 | STOP_VELOCITY, 44 | STOP_EFFORT 45 | }; 46 | 47 | class FlexivHardwareInterface : public hardware_interface::SystemInterface 48 | { 49 | public: 50 | RCLCPP_SHARED_PTR_DEFINITIONS(FlexivHardwareInterface) 51 | 52 | FLEXIV_HARDWARE_PUBLIC 53 | hardware_interface::CallbackReturn on_init( 54 | const hardware_interface::HardwareInfo& info) override; 55 | 56 | FLEXIV_HARDWARE_PUBLIC 57 | std::vector export_state_interfaces() override; 58 | 59 | FLEXIV_HARDWARE_PUBLIC 60 | std::vector export_command_interfaces() override; 61 | 62 | FLEXIV_HARDWARE_PUBLIC 63 | hardware_interface::return_type prepare_command_mode_switch( 64 | const std::vector& start_interfaces, 65 | const std::vector& stop_interfaces) override; 66 | 67 | FLEXIV_HARDWARE_PUBLIC 68 | hardware_interface::return_type perform_command_mode_switch( 69 | const std::vector& start_interfaces, 70 | const std::vector& stop_interfaces) override; 71 | 72 | FLEXIV_HARDWARE_PUBLIC 73 | hardware_interface::CallbackReturn on_activate( 74 | const rclcpp_lifecycle::State& previous_state) override; 75 | 76 | FLEXIV_HARDWARE_PUBLIC 77 | hardware_interface::CallbackReturn on_deactivate( 78 | const rclcpp_lifecycle::State& previous_state) override; 79 | 80 | FLEXIV_HARDWARE_PUBLIC 81 | hardware_interface::return_type read( 82 | const rclcpp::Time& time, const rclcpp::Duration& period) override; 83 | 84 | FLEXIV_HARDWARE_PUBLIC 85 | hardware_interface::return_type write( 86 | const rclcpp::Time& time, const rclcpp::Duration& period) override; 87 | 88 | private: 89 | // Flexiv RDK 90 | std::unique_ptr robot_; 91 | 92 | // Joint commands 93 | std::vector hw_commands_joint_positions_; 94 | std::vector hw_commands_joint_velocities_; 95 | std::vector hw_commands_joint_efforts_; 96 | 97 | // Joint states 98 | std::vector hw_states_joint_positions_; 99 | std::vector hw_states_joint_velocities_; 100 | std::vector hw_states_joint_efforts_; 101 | 102 | // Robot States 103 | flexiv::rdk::RobotStates hw_flexiv_robot_states_; 104 | flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_ = &hw_flexiv_robot_states_; 105 | 106 | // GPIO commands and states 107 | std::vector hw_commands_gpio_out_; 108 | std::vector hw_states_gpio_in_; 109 | 110 | // Current digital output ports 111 | std::vector current_ports_indices_; 112 | std::vector current_ports_values_; 113 | 114 | static rclcpp::Logger getLogger(); 115 | 116 | // control modes 117 | bool controllers_initialized_; 118 | std::vector stop_modes_; 119 | std::vector start_modes_; 120 | bool position_controller_running_; 121 | bool velocity_controller_running_; 122 | bool torque_controller_running_; 123 | }; 124 | 125 | } /* namespace flexiv_hardware */ 126 | #endif /* FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ */ 127 | -------------------------------------------------------------------------------- /flexiv_hardware/include/flexiv_hardware/visibility_control.h: -------------------------------------------------------------------------------- 1 | #ifndef FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_ 2 | #define FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_ 3 | 4 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 5 | // https://gcc.gnu.org/wiki/Visibility 6 | 7 | #if defined _WIN32 || defined __CYGWIN__ 8 | #ifdef __GNUC__ 9 | #define FLEXIV_HARDWARE_EXPORT __attribute__((dllexport)) 10 | #define FLEXIV_HARDWARE_IMPORT __attribute__((dllimport)) 11 | #else 12 | #define FLEXIV_HARDWARE_EXPORT __declspec(dllexport) 13 | #define FLEXIV_HARDWARE_IMPORT __declspec(dllimport) 14 | #endif 15 | #ifdef FLEXIV_HARDWARE_BUILDING_DLL 16 | #define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_EXPORT 17 | #else 18 | #define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_IMPORT 19 | #endif 20 | #define FLEXIV_HARDWARE_PUBLIC_TYPE FLEXIV_HARDWARE_PUBLIC 21 | #define FLEXIV_HARDWARE_LOCAL 22 | #else 23 | #define FLEXIV_HARDWARE_EXPORT __attribute__((visibility("default"))) 24 | #define FLEXIV_HARDWARE_IMPORT 25 | #if __GNUC__ >= 4 26 | #define FLEXIV_HARDWARE_PUBLIC __attribute__((visibility("default"))) 27 | #define FLEXIV_HARDWARE_LOCAL __attribute__((visibility("hidden"))) 28 | #else 29 | #define FLEXIV_HARDWARE_PUBLIC 30 | #define FLEXIV_HARDWARE_LOCAL 31 | #endif 32 | #define FLEXIV_HARDWARE_PUBLIC_TYPE 33 | #endif 34 | 35 | #endif // FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_ 36 | -------------------------------------------------------------------------------- /flexiv_hardware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_hardware 5 | 1.6.0 6 | Hardware interfaces to Flexiv robots for ROS 2 control 7 | Mun Seng Phoon 8 | Apache 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | rclcpp 14 | rclcpp_lifecycle 15 | hardware_interface 16 | pluginlib 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | ament_cmake_gtest 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /flexiv_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY config srdf rviz 8 | DESTINATION share/${PROJECT_NAME} 9 | ) 10 | 11 | ament_package() 12 | -------------------------------------------------------------------------------- /flexiv_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | $(var robot_sn)_joint1: 12 | has_velocity_limits: true 13 | max_velocity: 2.0944 # Rizon 4 14 | # max_velocity: 1.7453 # Rizon 10 15 | has_acceleration_limits: false 16 | max_acceleration: 0 17 | $(var robot_sn)_joint2: 18 | has_velocity_limits: true 19 | max_velocity: 2.0944 # Rizon 4 20 | # max_velocity: 1.7453 # Rizon 10 21 | has_acceleration_limits: false 22 | max_acceleration: 0 23 | $(var robot_sn)_joint3: 24 | has_velocity_limits: true 25 | max_velocity: 2.4435 # Rizon 4 26 | # max_velocity: 2.0944 # Rizon 10 27 | has_acceleration_limits: false 28 | max_acceleration: 0 29 | $(var robot_sn)_joint4: 30 | has_velocity_limits: true 31 | max_velocity: 2.4435 # Rizon 4 32 | # max_velocity: 2.0944 # Rizon 10 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | $(var robot_sn)_joint5: 36 | has_velocity_limits: true 37 | max_velocity: 4.8869 # Rizon 4 38 | # max_velocity: 3.8397 # Rizon 10 39 | has_acceleration_limits: false 40 | max_acceleration: 0 41 | $(var robot_sn)_joint6: 42 | has_velocity_limits: true 43 | max_velocity: 4.8869 # Rizon 4 44 | # max_velocity: 3.8397 # Rizon 10 45 | has_acceleration_limits: false 46 | max_acceleration: 0 47 | $(var robot_sn)_joint7: 48 | has_velocity_limits: true 49 | max_velocity: 4.8869 # Rizon 4 50 | # max_velocity: 3.8397 # Rizon 10 51 | has_acceleration_limits: false 52 | max_acceleration: 0 53 | $(var robot_sn)_finger_width_joint: 54 | has_velocity_limits: true 55 | max_velocity: 0.5 56 | has_acceleration_limits: false 57 | max_acceleration: 0 58 | -------------------------------------------------------------------------------- /flexiv_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_kinematics: 4 | rizon_arm: 5 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 6 | kinematics_solver_search_resolution: 0.005 7 | kinematics_solver_timeout: 0.05 8 | -------------------------------------------------------------------------------- /flexiv_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_names: 2 | - rizon_arm_controller 3 | - flexiv_gripper_node 4 | 5 | rizon_arm_controller: 6 | action_ns: follow_joint_trajectory 7 | type: FollowJointTrajectory 8 | default: true 9 | joints: 10 | - $(var robot_sn)_joint1 11 | - $(var robot_sn)_joint2 12 | - $(var robot_sn)_joint3 13 | - $(var robot_sn)_joint4 14 | - $(var robot_sn)_joint5 15 | - $(var robot_sn)_joint6 16 | - $(var robot_sn)_joint7 17 | 18 | flexiv_gripper_node: 19 | action_ns: gripper_action 20 | type: GripperCommand 21 | default: true 22 | joints: 23 | - $(var robot_sn)_finger_width_joint 24 | -------------------------------------------------------------------------------- /flexiv_moveit_config/config/rizon_moveit_servo_config.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | use_gazebo: false # Whether the robot is started in a Gazebo simulation environment 5 | 6 | ## Properties of incoming commands 7 | command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 8 | scale: 9 | # Scale parameters are only used if command_in_type=="unitless" 10 | linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. 11 | rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. 12 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. 13 | joint: 0.5 14 | 15 | # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) 16 | # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] 17 | 18 | ## Properties of outgoing commands 19 | publish_period: 0.034 # 1/Nominal publish rate [seconds] 20 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 21 | 22 | # What type of topic does your robot driver expect? 23 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 24 | command_out_type: trajectory_msgs/JointTrajectory 25 | 26 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 27 | publish_joint_positions: true 28 | publish_joint_velocities: true 29 | publish_joint_accelerations: false 30 | 31 | ## Plugins for smoothing outgoing commands 32 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" 33 | 34 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, 35 | # which other nodes can use as a source for information about the planning environment. 36 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), 37 | # then is_primary_planning_scene_monitor needs to be set to false. 38 | is_primary_planning_scene_monitor: true 39 | 40 | ## MoveIt properties 41 | move_group_name: rizon_arm # Often 'manipulator' or 'arm' 42 | planning_frame: $(var robot_sn)_base_link # The MoveIt planning frame. Often 'base_link' or 'world' 43 | 44 | ## Other frames 45 | ee_frame_name: $(var robot_sn)_flange # The name of the end effector link, used to return the EE pose 46 | robot_link_command_frame: $(var robot_sn)_flange # commands must be given in the frame of a robot link. Usually either the base or end effector 47 | 48 | ## Stopping behaviour 49 | incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command 50 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 51 | # Important because ROS may drop some messages and we need the robot to halt reliably. 52 | num_outgoing_halt_msgs_to_publish: 4 53 | 54 | ## Configure handling of singularities and joint limits 55 | lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) 56 | hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this 57 | joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 58 | 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) 59 | 60 | ## Topic names 61 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands 62 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands 63 | joint_topic: /joint_states 64 | status_topic: ~/status # Publish status to this topic 65 | command_out_topic: /rizon_arm_controller/joint_trajectory # Publish outgoing commands here 66 | 67 | ## Collision checking for the entire robot body 68 | check_collisions: true # Check collisions? 69 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 70 | self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] 71 | scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] 72 | -------------------------------------------------------------------------------- /flexiv_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_moveit_config 5 | 1.6.0 6 | MoveIt2 configuration and launch files for Flexiv Rizon robots 7 | Mun Seng Phoon 8 | Apache 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | 13 | robot_state_publisher 14 | joint_state_publisher 15 | joint_state_publisher_gui 16 | joint_state_broadcaster 17 | moveit_ros_move_group 18 | moveit_kinematics 19 | moveit_planners_ompl 20 | moveit_ros_visualization 21 | moveit_servo 22 | moveit_simple_controller_manager 23 | rviz2 24 | flexiv_description 25 | urdf 26 | warehouse_ros_sqlite 27 | xacro 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /flexiv_moveit_config/srdf/grav.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /flexiv_moveit_config/srdf/rizon.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /flexiv_moveit_config/srdf/rizon_macro.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /flexiv_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flexiv_msgs) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rosidl_default_generators REQUIRED) 6 | find_package(std_msgs REQUIRED) 7 | find_package(geometry_msgs REQUIRED) 8 | find_package(builtin_interfaces REQUIRED) 9 | 10 | set(msg_files 11 | msg/Digital.msg 12 | msg/GPIOStates.msg 13 | msg/JointPosVel.msg 14 | msg/Mode.msg 15 | msg/RobotStates.msg 16 | ) 17 | 18 | # set(srv_files 19 | # ) 20 | 21 | set(action_files 22 | action/Grasp.action 23 | action/Move.action 24 | ) 25 | 26 | rosidl_generate_interfaces(${PROJECT_NAME} 27 | ${msg_files} 28 | # ${srv_files} 29 | ${action_files} 30 | DEPENDENCIES 31 | std_msgs 32 | geometry_msgs 33 | builtin_interfaces 34 | ) 35 | 36 | if(BUILD_TESTING) 37 | find_package(ament_lint_auto REQUIRED) 38 | ament_lint_auto_find_test_dependencies() 39 | endif() 40 | 41 | ament_export_dependencies(rosidl_default_runtime) 42 | 43 | ament_package() 44 | -------------------------------------------------------------------------------- /flexiv_msgs/action/Grasp.action: -------------------------------------------------------------------------------- 1 | float64 force # N 2 | --- 3 | bool success 4 | string error 5 | --- 6 | float64 current_width # m 7 | float64 current_force # N 8 | bool moving 9 | -------------------------------------------------------------------------------- /flexiv_msgs/action/Move.action: -------------------------------------------------------------------------------- 1 | float64 width # m 2 | float64 velocity # m/s 3 | float64 max_force # N 4 | --- 5 | bool success 6 | string error 7 | --- 8 | float64 current_width # m 9 | float64 current_force # N 10 | bool moving 11 | -------------------------------------------------------------------------------- /flexiv_msgs/msg/Digital.msg: -------------------------------------------------------------------------------- 1 | uint8 pin 2 | bool state 3 | -------------------------------------------------------------------------------- /flexiv_msgs/msg/GPIOStates.msg: -------------------------------------------------------------------------------- 1 | Digital[] states 2 | -------------------------------------------------------------------------------- /flexiv_msgs/msg/JointPosVel.msg: -------------------------------------------------------------------------------- 1 | float64[7] positions 2 | float64[7] velocities 3 | -------------------------------------------------------------------------------- /flexiv_msgs/msg/Mode.msg: -------------------------------------------------------------------------------- 1 | int8 MODE_UNKNOWN = -1 2 | int8 MODE_IDLE = 0 3 | int8 MODE_RT_JOINT_TORQUE = 1 4 | int8 MODE_RT_JOINT_IMPEDANCE = 2 5 | int8 MODE_NRT_JOINT_IMPEDANCE = 3 6 | int8 MODE_RT_JOINT_POSITION = 4 7 | int8 MODE_NRT_JOINT_POSITION = 5 8 | int8 MODE_NRT_PLAN_EXECUTION = 6 9 | int8 MODE_NRT_PRIMITIVE_EXECUTION = 7 10 | int8 MODE_RT_CARTESIAN_MOTION_FORCE = 8 11 | int8 MODE_NRT_CARTESIAN_MOTION_FORCE = 9 12 | 13 | int8 mode 14 | -------------------------------------------------------------------------------- /flexiv_msgs/msg/RobotStates.msg: -------------------------------------------------------------------------------- 1 | # Header 2 | std_msgs/Header header 3 | 4 | # Measured joint positions using link-side encoder 5 | float64[7] q 6 | 7 | # Measured joint positions using motor-side encoder 8 | float64[7] theta 9 | 10 | # Measured joint velocities using link-side encoder 11 | float64[7] dq 12 | 13 | # Measured joint velocities using motor-side encoder 14 | float64[7] dtheta 15 | 16 | # Measured joint torques 17 | float64[7] tau 18 | 19 | # Desired joint torques 20 | float64[7] tau_des 21 | 22 | # Numerical derivative of measured joint torques 23 | float64[7] tau_dot 24 | 25 | # Estimated external joint torques 26 | float64[7] tau_ext 27 | 28 | # Measured TCP pose expressed in world frame 29 | geometry_msgs/PoseStamped tcp_pose 30 | 31 | # Measured TCP velocity expressed in world frame 32 | geometry_msgs/AccelStamped tcp_vel 33 | 34 | # Measured flange pose expressed in world frame 35 | geometry_msgs/PoseStamped flange_pose 36 | 37 | # Force-torque (FT) sensor raw reading in flange frame 38 | geometry_msgs/WrenchStamped ft_sensor_raw 39 | 40 | # Estimated external wrench applied on TCP and expressed in TCP frame 41 | geometry_msgs/WrenchStamped ext_wrench_in_tcp 42 | 43 | # Estimated external wrench applied on TCP and expressed in world frame 44 | geometry_msgs/WrenchStamped ext_wrench_in_world 45 | -------------------------------------------------------------------------------- /flexiv_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_msgs 5 | 1.6.0 6 | Robot states messages definiton used in RDK 7 | Mun Seng Phoon 8 | Apache License 2.0 9 | Mun Seng Phoon 10 | 11 | ament_cmake 12 | rosidl_default_generators 13 | std_msgs 14 | geometry_msgs 15 | rosidl_default_runtime 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | rosidl_interface_packages 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /flexiv_test_nodes/flexiv_test_nodes/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_test_nodes/flexiv_test_nodes/__init__.py -------------------------------------------------------------------------------- /flexiv_test_nodes/flexiv_test_nodes/publisher_joint_trajectory_controller.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rclpy 16 | from builtin_interfaces.msg import Duration 17 | from rclpy.node import Node 18 | from sensor_msgs.msg import JointState 19 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 20 | from rcl_interfaces.msg import ParameterDescriptor 21 | 22 | 23 | class PublisherJointTrajectory(Node): 24 | def __init__(self): 25 | super().__init__("publisher_position_trajectory_controller") 26 | # Declare all parameters 27 | self.declare_parameter("controller_name", "joint_trajectory_controller") 28 | self.declare_parameter("wait_sec_between_publish", 6) 29 | self.declare_parameter("goal_names", ["pos1", "pos2"]) 30 | self.declare_parameter("joints", [""]) 31 | self.declare_parameter("check_starting_point", False) 32 | self.declare_parameter("starting_point_limits", False) 33 | 34 | # Read parameters 35 | controller_name = self.get_parameter("controller_name").value 36 | wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value 37 | goal_names = self.get_parameter("goal_names").value 38 | self.joints = self.get_parameter("joints").value 39 | self.check_starting_point = self.get_parameter("check_starting_point").value 40 | self.starting_point = {} 41 | 42 | if self.joints is None or len(self.joints) == 0: 43 | raise Exception('"joints" parameter is not set!') 44 | 45 | # starting point stuff 46 | if self.check_starting_point: 47 | # declare nested params 48 | for name in self.joints: 49 | param_name_tmp = "starting_point_limits" + "." + name 50 | self.declare_parameter(param_name_tmp, [-2 * 3.14159, 2 * 3.14159]) 51 | self.starting_point[name] = self.get_parameter(param_name_tmp).value 52 | 53 | for name in self.joints: 54 | if len(self.starting_point[name]) != 2: 55 | raise Exception('"starting_point" parameter is not set correctly!') 56 | self.joint_state_sub = self.create_subscription( 57 | JointState, "joint_states", self.joint_state_callback, 10 58 | ) 59 | # initialize starting point status 60 | self.starting_point_ok = not self.check_starting_point 61 | 62 | self.joint_state_msg_received = False 63 | 64 | # Read all positions from parameters 65 | self.goals = [] 66 | for name in goal_names: 67 | self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) 68 | goal = self.get_parameter(name).value 69 | if goal is None or len(goal) == 0: 70 | raise Exception(f'Values for goal "{name}" not set!') 71 | 72 | float_goal = [] 73 | for value in goal: 74 | float_goal.append(float(value)) 75 | self.goals.append(float_goal) 76 | 77 | publish_topic = "/" + controller_name + "/" + "joint_trajectory" 78 | 79 | self.get_logger().info( 80 | 'Publishing {} goals on topic "{}" every {} s'.format( 81 | len(goal_names), publish_topic, wait_sec_between_publish 82 | ) 83 | ) 84 | 85 | self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) 86 | self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) 87 | self.i = 0 88 | 89 | def timer_callback(self): 90 | 91 | if self.starting_point_ok: 92 | 93 | traj = JointTrajectory() 94 | traj.joint_names = self.joints 95 | point = JointTrajectoryPoint() 96 | point.positions = self.goals[self.i] 97 | point.time_from_start = Duration(sec=1) 98 | 99 | traj.points.append(point) 100 | self.publisher_.publish(traj) 101 | 102 | self.i += 1 103 | self.i %= len(self.goals) 104 | 105 | elif self.check_starting_point and not self.joint_state_msg_received: 106 | self.get_logger().warn( 107 | 'Start configuration could not be checked! Check "joint_state" topic!' 108 | ) 109 | else: 110 | self.get_logger().warn( 111 | "Start configuration is not within configured limits!" 112 | ) 113 | 114 | def joint_state_callback(self, msg): 115 | 116 | if not self.joint_state_msg_received: 117 | 118 | # check start state 119 | limit_exceeded = [False] * len(msg.name) 120 | for idx, enum in enumerate(msg.name): 121 | if (msg.position[idx] < self.starting_point[enum][0]) or ( 122 | msg.position[idx] > self.starting_point[enum][1] 123 | ): 124 | self.get_logger().warn( 125 | f"Starting point limits exceeded for joint {enum} !" 126 | ) 127 | limit_exceeded[idx] = True 128 | 129 | if any(limit_exceeded): 130 | self.starting_point_ok = False 131 | else: 132 | self.starting_point_ok = True 133 | 134 | self.joint_state_msg_received = True 135 | else: 136 | return 137 | 138 | 139 | def main(args=None): 140 | rclpy.init(args=args) 141 | 142 | publisher_joint_trajectory = PublisherJointTrajectory() 143 | 144 | rclpy.spin(publisher_joint_trajectory) 145 | publisher_joint_trajectory.destroy_node() 146 | rclpy.shutdown() 147 | 148 | 149 | if __name__ == "__main__": 150 | main() 151 | -------------------------------------------------------------------------------- /flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from sensor_msgs.msg import JointState 6 | 7 | from flexiv_msgs.msg import JointPosVel 8 | 9 | # Joint sine-sweep amplitude [rad] 10 | SWING_AMP = 0.035 11 | 12 | # TCP sine-sweep frequency [Hz] 13 | SWING_FREQ = 0.3 14 | 15 | 16 | class SineSweepImpedance(Node): 17 | def __init__(self): 18 | super().__init__("sine_sweep_impedance_controller") 19 | # Declare all parameters 20 | self.declare_parameter("controller_name", "joint_impedance_controller") 21 | self.declare_parameter("joints", [""]) 22 | self.declare_parameter("wait_sec_between_publish", 0.001) 23 | self.declare_parameter("speed_scaling", 1.0) 24 | 25 | # Read parameters 26 | controller_name = self.get_parameter("controller_name").value 27 | self.joints = self.get_parameter("joints").value 28 | wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value 29 | self.speed_scaling = self.get_parameter("speed_scaling").value 30 | 31 | if self.joints is None or len(self.joints) == 0: 32 | raise Exception('"joints" parameter is not set!') 33 | 34 | publish_topic = "/" + controller_name + "/" + "commands" 35 | 36 | self.init_pos = [0.0] * len(self.joints) 37 | 38 | self.publisher_ = self.create_publisher(JointPosVel, publish_topic, 1) 39 | self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) 40 | self.joint_state_sub = self.create_subscription( 41 | JointState, "joint_states", self.joint_state_callback, 10 42 | ) 43 | self.joint_state_msg_received = False 44 | self.loop_time = 0.0 45 | self.period = wait_sec_between_publish 46 | 47 | def timer_callback(self): 48 | if self.joint_state_msg_received: 49 | target_pos = self.init_pos.copy() 50 | for i in range(len(self.joints)): 51 | target_pos[i] = self.init_pos[i] + SWING_AMP * math.sin( 52 | 2 * math.pi * SWING_FREQ * self.speed_scaling * self.loop_time 53 | ) 54 | msg = JointPosVel() 55 | msg.positions = target_pos 56 | self.publisher_.publish(msg) 57 | self.loop_time += self.period 58 | 59 | def joint_state_callback(self, msg): 60 | if not self.joint_state_msg_received: 61 | # retrieve joint states by the order of the joint names 62 | for name in self.joints: 63 | if name not in msg.name: 64 | raise Exception(f"Joint {name} not found in joint_states!") 65 | joint_msg_index = msg.name.index(name) 66 | joint_position = msg.position[joint_msg_index] 67 | index = self.joints.index(name) 68 | self.init_pos[index] = joint_position 69 | self.joint_state_msg_received = True 70 | else: 71 | return 72 | 73 | 74 | def main(args=None): 75 | rclpy.init(args=args) 76 | 77 | sine_sweep_impedance = SineSweepImpedance() 78 | 79 | rclpy.spin(sine_sweep_impedance) 80 | sine_sweep_impedance.destroy_node() 81 | rclpy.shutdown() 82 | 83 | 84 | if __name__ == "__main__": 85 | main() 86 | -------------------------------------------------------------------------------- /flexiv_test_nodes/flexiv_test_nodes/sine_sweep_position_controller.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from sensor_msgs.msg import JointState 6 | from std_msgs.msg import Float64MultiArray 7 | 8 | # Joint sine-sweep amplitude [rad] 9 | SWING_AMP = 0.035 10 | 11 | # TCP sine-sweep frequency [Hz] 12 | SWING_FREQ = 0.3 13 | 14 | 15 | class SineSweepPosition(Node): 16 | def __init__(self): 17 | super().__init__("sine_sweep_position_controller") 18 | # Declare all parameters 19 | self.declare_parameter("controller_name", "forward_position_controller") 20 | self.declare_parameter("joints", [""]) 21 | self.declare_parameter("wait_sec_between_publish", 0.001) 22 | self.declare_parameter("speed_scaling", 1.0) 23 | 24 | # Read parameters 25 | controller_name = self.get_parameter("controller_name").value 26 | self.joints = self.get_parameter("joints").value 27 | wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value 28 | self.speed_scaling = self.get_parameter("speed_scaling").value 29 | 30 | if self.joints is None or len(self.joints) == 0: 31 | raise Exception('"joints" parameter is not set!') 32 | 33 | publish_topic = "/" + controller_name + "/" + "commands" 34 | 35 | self.init_pos = [0.0] * len(self.joints) 36 | 37 | self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) 38 | self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) 39 | self.joint_state_sub = self.create_subscription( 40 | JointState, "joint_states", self.joint_state_callback, 10 41 | ) 42 | self.joint_state_msg_received = False 43 | self.loop_time = 0.0 44 | self.period = wait_sec_between_publish 45 | 46 | def timer_callback(self): 47 | if self.joint_state_msg_received: 48 | target_pos = self.init_pos.copy() 49 | for i in range(len(self.joints)): 50 | target_pos[i] = self.init_pos[i] + SWING_AMP * math.sin( 51 | 2 * math.pi * SWING_FREQ * self.speed_scaling * self.loop_time 52 | ) 53 | msg = Float64MultiArray() 54 | msg.data = target_pos 55 | self.publisher_.publish(msg) 56 | self.loop_time += self.period 57 | 58 | def joint_state_callback(self, msg): 59 | if not self.joint_state_msg_received: 60 | # retrieve joint states by the order of the joint names 61 | for name in self.joints: 62 | if name not in msg.name: 63 | raise Exception(f"Joint {name} not found in joint_states!") 64 | joint_msg_index = msg.name.index(name) 65 | joint_position = msg.position[joint_msg_index] 66 | index = self.joints.index(name) 67 | self.init_pos[index] = joint_position 68 | self.joint_state_msg_received = True 69 | else: 70 | return 71 | 72 | 73 | def main(args=None): 74 | rclpy.init(args=args) 75 | 76 | sine_sweep_position = SineSweepPosition() 77 | 78 | rclpy.spin(sine_sweep_position) 79 | sine_sweep_position.destroy_node() 80 | rclpy.shutdown() 81 | 82 | 83 | if __name__ == "__main__": 84 | main() 85 | -------------------------------------------------------------------------------- /flexiv_test_nodes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | flexiv_test_nodes 5 | 1.6.0 6 | Demo nodes for testing flexiv_ros2 7 | Mun Seng Phoon 8 | Apache-2.0 9 | Mun Seng Phoon 10 | 11 | rclpy 12 | std_msgs 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /flexiv_test_nodes/resource/flexiv_test_nodes: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flexivrobotics/flexiv_ros2/2da41a6719cb029719a9b26aebae432823790060/flexiv_test_nodes/resource/flexiv_test_nodes -------------------------------------------------------------------------------- /flexiv_test_nodes/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/flexiv_test_nodes 3 | [install] 4 | install_scripts=$base/lib/flexiv_test_nodes 5 | -------------------------------------------------------------------------------- /flexiv_test_nodes/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | 3 | from setuptools import setup 4 | 5 | package_name = "flexiv_test_nodes" 6 | 7 | setup( 8 | name=package_name, 9 | version="0.0.1", 10 | packages=[package_name], 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | ("share/" + package_name, glob("launch/*.launch.py")), 15 | ("share/" + package_name + "/configs", glob("configs/*.*")), 16 | ], 17 | install_requires=["setuptools"], 18 | zip_safe=True, 19 | author="Mun Seng Phoon", 20 | author_email="munseng.phoon@flexiv.com", 21 | maintainer="Mun Seng Phoon", 22 | maintainer_email="munseng.phoon@flexiv.com", 23 | keywords=["ROS"], 24 | classifiers=[ 25 | "Intended Audience :: Developers", 26 | "License :: OSI Approved :: Apache Software License", 27 | "Programming Language :: Python", 28 | "Topic :: Software Development", 29 | ], 30 | description="Demo nodes for testing flexiv_ros2.", 31 | long_description="""\ 32 | Demo nodes for for testing flexiv_ros2.""", 33 | license="Apache License, Version 2.0", 34 | tests_require=["pytest"], 35 | entry_points={ 36 | "console_scripts": [ 37 | "publisher_joint_trajectory_controller = \ 38 | flexiv_test_nodes.publisher_joint_trajectory_controller:main", 39 | "sine_sweep_position_controller = \ 40 | flexiv_test_nodes.sine_sweep_position_controller:main", 41 | "sine_sweep_impedance_controller = \ 42 | flexiv_test_nodes.sine_sweep_impedance_controller:main", 43 | ], 44 | }, 45 | ) 46 | --------------------------------------------------------------------------------