Backport 393 to jazzy

CHANGELOG.rst

@@ -11,6 +11,8 @@ Jazzy v2.x (TBD)
 
   * Replace ``ament_target_dependencies`` with ``target_link_libraries``.
   * ``lbr_moveit``: Add missing ``pynput`` dependency and fix dataclasses.
+  * ``lbr_dual_arm``: Adds dual-arm integration demo package.
+  * ``lbr_dual_arm_moveit_config``: Adds dual-arm moveit configuration package.
 * ``lbr_description``:
 
   * Removes ``lbr_description`` from this folder in favor of external descriptions:
@@ -38,6 +40,7 @@ Jazzy v2.x (TBD)
 * Related pull requests:
 
   * MoveIt fix: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/379
+  * Dual-arm: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/393 (kenichi-maeda)
   * Namespace argument: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/344, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/353
   * Effort limits: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/357
   * URDF de-coupling: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/359, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/389

lbr_demos/doc/lbr_demos.rst

@@ -32,6 +32,10 @@ MoveIt
     lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst>
     lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst>
 
+
 Integration
 -----------
-TODO system integration demos
+.. toctree::
+    :titlesonly:
+
+    lbr_dual_arm <../lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst>

lbr_demos/lbr_dual_arm/lbr_dual_arm/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.22)
+project(lbr_dual_arm)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY config launch urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml

@@ -0,0 +1,41 @@
+# 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
+
+    # ROS 2 control controllers
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+# 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

lbr_demos/lbr_dual_arm/lbr_dual_arm/config/initial_joint_positions.yaml

@@ -0,0 +1,8 @@
+# initial joint positions [degrees]
+A1: 0.0
+A2: 25.0
+A3: 0.0
+A4: 90.0
+A5: 0.0
+A6: 0.0
+A7: 0.0

lbr_demos/lbr_dual_arm/lbr_dual_arm/config/joint_limits.yaml

@@ -0,0 +1,49 @@
+A1:
+  lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits
+  upper: 169
+  velocity: 98
+  effort: 200
+  safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin
+  k_velocity: 10
+A2:
+  lower: -119
+  upper: 119
+  velocity: 98
+  effort: 200
+  safety_position_margin: 5
+  k_velocity: 10
+A3:
+  lower: -169
+  upper: 169
+  velocity: 100
+  effort: 200
+  safety_position_margin: 5
+  k_velocity: 10
+A4:
+  lower: -119
+  upper: 119
+  velocity: 130
+  effort: 200
+  safety_position_margin: 5
+  k_velocity: 10
+A5:
+  lower: -169
+  upper: 169
+  velocity: 140
+  effort: 200
+  safety_position_margin: 5
+  k_velocity: 10
+A6:
+  lower: -119
+  upper: 119
+  velocity: 180
+  effort: 200
+  safety_position_margin: 5
+  k_velocity: 10
+A7:
+  lower: -174
+  upper: 174
+  velocity: 180
+  effort: 200
+  safety_position_margin: 5
+  k_velocity: 10

lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_one_system_config.yaml

@@ -0,0 +1,32 @@
+# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface
+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
+# exponential moving average time constant for joint position commands [s]:
+#   set tau > robot sample time for more smoothing on the commands
+#   set tau << robot sample time for no smoothing on the commands
+joint_position_tau: 0.04
+command_guard:
+  # if requested position / velocities beyond limits, CommandGuard will be triggered and the command will be overriden with the current state instead
+  # available: [default, safe_stop]
+  variant: default
+state_guard:
+  # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE]
+  # in POSITION control mode, safety check is not performed
+  external_torque_safety_check: true
+  external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm]
+# exponential moving average time constant for external joint torque measurements [s]:
+#   set tau > robot sample time for more smoothing on the external torque measurements
+#   set tau << robot sample time for no smoothing on the external torque measurements
+external_torque_tau: 0.01
+# exponential moving average time constant for joint torque measurements [s]:
+#   set tau > robot sample time for more smoothing on the raw torque measurements
+#   set tau << robot sample time for no smoothing on the raw torque measurements
+measured_torque_tau: 0.01
+# for position control, LBR works best in open_loop control mode
+# note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode
+open_loop: true

lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_two_system_config.yaml

@@ -0,0 +1,32 @@
+# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface
+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
+# exponential moving average time constant for joint position commands [s]:
+#   set tau > robot sample time for more smoothing on the commands
+#   set tau << robot sample time for no smoothing on the commands
+joint_position_tau: 0.04
+command_guard:
+  # if requested position / velocities beyond limits, CommandGuard will be triggered and the command will be overriden with the current state instead
+  # available: [default, safe_stop]
+  variant: default
+state_guard:
+  # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE]
+  # in POSITION control mode, safety check is not performed
+  external_torque_safety_check: true
+  external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm]
+# exponential moving average time constant for external joint torque measurements [s]:
+#   set tau > robot sample time for more smoothing on the external torque measurements
+#   set tau << robot sample time for no smoothing on the external torque measurements
+external_torque_tau: 0.01
+# exponential moving average time constant for joint torque measurements [s]:
+#   set tau > robot sample time for more smoothing on the raw torque measurements
+#   set tau << robot sample time for no smoothing on the raw torque measurements
+measured_torque_tau: 0.01
+# for position control, LBR works best in open_loop control mode
+# note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode
+open_loop: true

lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst

@@ -0,0 +1,52 @@
+lbr_dual_arm
+============
+Dual-arm ``iiwa7`` integration demo, including: launch files, description file, and ``ros2_control`` configurations.
+
+.. contents:: Table of Contents
+   :depth: 2
+   :local:
+   :backlinks: none
+
+Quick Start
+-----------
+Open a terminal, then run:
+
+.. code-block:: bash
+
+    ros2 launch lbr_dual_arm mock.launch.py \
+        moveit:=true
+
+.. note::
+    For hardware, run ``hardware.launch.py`` (follow :doc:`Hardware Setup <../../../../lbr_fri_ros2_stack/doc/hardware_setup>` first).
+
+.. note::
+    To configure position / orientation, launch with ``lbr_one_x:=<value>`` etc. (list all arguments via ``ros2 launch lbr_dual_arm mock.launch.py -s``)
+
+Description File
+----------------
+Custom description files can be generated via ``xacro``, see  `lbr_dual_arm.xacro <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/jazzy/lbr_demos/lbr_dual_arm/lbr_dual_arm/urdf/lbr_dual_arm.xacro>`_:octicon:`link-external`.
+
+#. Include macros:
+
+    .. code-block:: xml
+
+        <xacro:include filename="$(find lbr_iiwa7_r800_description)/urdf/lbr_iiwa7_r800_macro.xacro" />
+        <xacro:include filename="$(find lbr_ros2_control)/config/lbr_system_interface.xacro" />
+
+#. Instantiate macros (``ros2_control`` plugin and robot description)
+
+    .. code-block:: xml
+
+        <xacro:lbr_system_interface ... />
+        <xacro:lbr_iiwa7_r800 ... />
+
+Done! The controller manager will load appropriate plugins when reading the robot description from the robot state publisher.
+
+Customize Hardware Network Settings
+-----------------------------------
+#. Open `lbr_one_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/jazzy/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_one_system_config.yaml>`_:octicon:`link-external` and `lbr_two_system_config.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/jazzy/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_two_system_config.yaml>`_:octicon:`link-external`.
+#. Set unique ``port_id`` values for each arm.
+#. Set ``remote_host`` per arm according to your network setup.
+
+.. note::
+    ``port_id`` must be different for both arms.