From d3af2053c146cb8c0b1d142e75654d669eace9d1 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 23 Oct 2024 11:31:08 +0100 Subject: [PATCH] added dual arm demo draft (#189) --- .../lbr_dual_arm_description/CMakeLists.txt | 28 +++++ .../doc/lbr_dual_arm_description.rst | 108 ++++++++++++++++++ .../lbr_dual_arm_description/package.xml | 20 ++++ .../ros2_control/dual_arm_controllers.yaml | 50 ++++++++ .../ros2_control/lbr_one_system_config.yaml | 33 ++++++ .../ros2_control/lbr_two_system_config.yaml | 33 ++++++ .../urdf/lbr_dual_arm.xacro | 36 ++++++ 7 files changed, 308 insertions(+) create mode 100644 lbr_demos/lbr_dual_arm_description/CMakeLists.txt create mode 100644 lbr_demos/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst create mode 100644 lbr_demos/lbr_dual_arm_description/package.xml create mode 100644 lbr_demos/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml create mode 100644 lbr_demos/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml create mode 100644 lbr_demos/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml create mode 100644 lbr_demos/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro diff --git a/lbr_demos/lbr_dual_arm_description/CMakeLists.txt b/lbr_demos/lbr_dual_arm_description/CMakeLists.txt new file mode 100644 index 00000000..eec8d874 --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(lbr_dual_arm_description) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(lbr_description REQUIRED) + +install(DIRECTORY ros2_control urdf + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/lbr_demos/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst b/lbr_demos/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst new file mode 100644 index 00000000..a236701b --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst @@ -0,0 +1,108 @@ +lbr_dual_arm_description +======================== +This demo gives a walk through on using the ``lbr_description`` to generate a custom robot description. In this case, we will be investigating a dual-arm setup. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +Creating the Package +-------------------- +#. Inside your workspace, create a package: + +.. code-block:: bash + + ros2 pkg create lbr_dual_arm_description \ + --build-type ament_cmake \ + --license Apache-2.0 \ + --maintainer-name mhubii \ + --maintainer-email "m.huber_1994@hotmail.de" \ + --description "A dual LBR description package using lbr_description." \ + --dependencies lbr_description + +#. Remove unused folders and create a Universal Robot Description File (URDF) folder: + +.. code-block:: bash + + cd lbr_dual_arm_description && \ + rm -r lbr_dual_arm_description/include && \ + rm -r lbr_dual_arm_description/src && \ + mkdir urdf + +Creating the Dual-arm Robot Description +--------------------------------------- +#. Create a dual-arm robot description: + +.. code-block:: bash + + touch urdf/lbr_dual_arm.xacro + +#. Fill it with the following contents (note that `xacro `_:octicon:`link-external` is used): + +.. code-block:: XML + + + + + + + + + + + + + + + + + + + + +#. Create the ROS 2 Control configuration files: + + #. Create a configurations file for the controller manager and respective controllers: + + + + #. FRI system configurations (only necessary when hardware is used): + + .. code-block:: bash + + cp `ros2 pkg prefix lbr_description`/share/lbr_description/ros2_control/lbr_system_config.yaml ros2_control/lbr_one_system_config.yaml && \ + cp `ros2 pkg prefix lbr_description`/share/lbr_description/ros2_control/lbr_system_config.yaml ros2_control/lbr_two_system_config.yaml + +#. Open ``lbr_two_system_config.yaml``: + + #. Change ``port_id`` to ``30201`` (or as desired, but different from ``lbr_one_system_config.yaml``) + #. Update ``estimated_ft_sensor`` frames: + + .. code-black:: yaml + + chain_root: lbr_one_link_0 # and lbr_two_link_0 respectively + chain_tip: lbr_one_link_ee # and lbr_two_link_0 respectively + +#. Add the following to the ``CMakeLists.txt``: + + .. code-block:: cmake + + install(DIRECTORY ros2_control urdf + DESTINATION share/${PROJECT_NAME} + ) + + + +.. #. Build the package in your workspace: + +.. .. code-block:: bash + +.. colcon build --packages-select lbr_dual_arm_description --symlink-install diff --git a/lbr_demos/lbr_dual_arm_description/package.xml b/lbr_demos/lbr_dual_arm_description/package.xml new file mode 100644 index 00000000..cfb79383 --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/package.xml @@ -0,0 +1,20 @@ + + + + lbr_dual_arm_description + 2.1.2 + A dual LBR description package using lbr_description. + mhubii + Apache-2.0 + + ament_cmake + + lbr_description + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/lbr_demos/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml b/lbr_demos/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml new file mode 100644 index 00000000..26ab6f15 --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml @@ -0,0 +1,50 @@ +# Use of /** so that the configurations hold for controller +# managers regardless of their namespace. Usefull in multi-robot setups. +/**/controller_manager: + ros__parameters: + update_rate: 100 + + # ROS 2 control broadcasters + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # # LBR ROS 2 control broadcasters + # lbr_state_broadcaster: + # type: lbr_ros2_control/LBRStateBroadcaster + + # ROS 2 control controllers + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +# # LBR ROS 2 control broadcasters +# /**/lbr_state_broadcaster: +# ros__parameters: +# robot_name: lbr + +# ROS 2 control controllers +/**/joint_trajectory_controller: + ros__parameters: + joints: + # robot one + - lbr_one_A1 + - lbr_one_A2 + - lbr_one_A3 + - lbr_one_A4 + - lbr_one_A5 + - lbr_one_A6 + - lbr_one_A7 + # robot two + - lbr_two_A1 + - lbr_two_A2 + - lbr_two_A3 + - lbr_two_A4 + - lbr_two_A5 + - lbr_two_A6 + - lbr_two_A7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 50.0 + action_monitor_rate: 20.0 diff --git a/lbr_demos/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml b/lbr_demos/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml new file mode 100644 index 00000000..c88faf26 --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml @@ -0,0 +1,33 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 + client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups + remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection + rt_prio: 80 # real-time priority for the control loop + pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) + pid_i: 0.0 # I gain for the joint position command + pid_d: 0.0 # D gain for the joint position command + pid_i_max: 0.0 # max integral value for the joint position command + pid_i_min: 0.0 # min integral value for the joint position command + pid_antiwindup: false # enable antiwindup for the joint position command + command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] + external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] + measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] + open_loop: true # KUKA works the best in open_loop control mode + +estimated_ft_sensor: # estimates the external force-torque from the external joint torque values + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation + chain_root: lbr_one_link_0 + chain_tip: lbr_one_link_ee + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered + force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered + force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered + torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml b/lbr_demos/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml new file mode 100644 index 00000000..ec08e89b --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml @@ -0,0 +1,33 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 + client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + port_id: 30201 # port id for the UDP communication. Useful in multi-robot setups + remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection + rt_prio: 80 # real-time priority for the control loop + pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) + pid_i: 0.0 # I gain for the joint position command + pid_d: 0.0 # D gain for the joint position command + pid_i_max: 0.0 # max integral value for the joint position command + pid_i_min: 0.0 # min integral value for the joint position command + pid_antiwindup: false # enable antiwindup for the joint position command + command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] + external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] + measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] + open_loop: true # KUKA works the best in open_loop control mode + +estimated_ft_sensor: # estimates the external force-torque from the external joint torque values + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation + chain_root: lbr_two_link_0 + chain_tip: lbr_two_link_ee + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered + force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered + force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered + torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro b/lbr_demos/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro new file mode 100644 index 00000000..c79f5d9b --- /dev/null +++ b/lbr_demos/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file