added dual arm demo draft (#189)

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()

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 "[email protected]" \
+        --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 <https://github.com/ros/xacro/tree/ros2>`_:octicon:`link-external` is used):
+
+.. code-block:: XML
+
+    <?xml version="1.0"?>
+
+    <robot name="lbr_dual_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
+        <!-- include the lbr iiwa macro -->
+        <xacro:include filename="$(find lbr_description)/urdf/iiwa7/iiwa7_description.xacro" />
+
+        <!-- add an arguent to allow for mock / hardware / gazebo -->
+        <xacro:arg name="mode" default="mock" />
+
+        <!-- add a base / floating link -->
+        <link name="base_link" />
+
+        <!-- add robot 1 via macro, note that different lbr_one_system_config.yaml are used (to
+        configure port id) -->
+        <xacro:iiwa7
+            robot_name="lbr_one"
+            mode="$(arg mode)"
+            system_config_path="$(find lbr_dual_arm_description)/ros2_control/lbr_one_system_config.yaml" />
+
+        <!-- add robot 2 via macro -->
+        <xacro:iiwa7
+            robot_name="lbr_two"
+            mode="$(arg mode)"
+            system_config_path="$(find lbr_dual_arm_description)/ros2_control/lbr_two_system_config.yaml" />
+    </robot>
+
+#. 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

lbr_demos/lbr_dual_arm_description/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lbr_dual_arm_description</name>
+  <version>2.1.2</version>
+  <description>A dual LBR description package using lbr_description.</description>
+  <maintainer email="[email protected]">mhubii</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>lbr_description</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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

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

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

lbr_demos/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+
+<robot name="lbr_dual_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <!-- include the lbr iiwa macro -->
+    <xacro:include filename="$(find lbr_description)/urdf/iiwa7/iiwa7_description.xacro" />
+
+    <!-- add an arguent to allow for mock / hardware / gazebo -->
+    <xacro:arg name="mode" default="mock" />
+
+    <!-- add a base / floating link -->
+    <link name="base_link" />
+
+    <!-- add joints that connect the robots -->
+    <joint name="lbr_one_base_joint" type="fixed">
+        <parent link="base_link" />
+        <child link="lbr_one_link_0" />
+    </joint>
+
+    <joint name="lbr_two_base_joint" type="fixed">
+        <parent link="base_link" />
+        <child link="lbr_two_link_0" />
+    </joint>
+
+    <!-- add robot 1 via macro, note that different lbr_one_system_config.yaml are used (to
+    configure port id) -->
+    <xacro:iiwa7
+        robot_name="lbr_one"
+        mode="$(arg mode)"
+        system_config_path="$(find lbr_dual_arm_description)/ros2_control/lbr_one_system_config.yaml" />
+
+    <!-- add robot 2 via macro -->
+    <xacro:iiwa7
+        robot_name="lbr_two"
+        mode="$(arg mode)"
+        system_config_path="$(find lbr_dual_arm_description)/ros2_control/lbr_two_system_config.yaml" />
+</robot>