Add dual arm configuration (#231)

---------

Co-authored-by: Sebastian Castro <[email protected]>

README.md

@@ -31,4 +31,5 @@ Specifically:
 * `picknik_ur_site_config` extends the base configuration with capabilities for robots with physics and perception.
 * `picknik_ur_gazebo_config` extends the site configuration with support for the Gazebo simulator.
 * `picknik_ur_gazebo_scan_and_plan_config` extends the Gazebo configuration with an alternate environment for 3D object scanning.
+* `picknik_ur_multi_arm_config` is an example configuration for using multiple arms.
 * Other hardware-specific configurations, such as the ones used on PickNik's UR arms, inherit from `picknik_ur_site_config`.

src/picknik_ur_base_config/description/picknik_ur_attachments_macro.xacro

@@ -1,33 +1,36 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-    <xacro:macro name="picknik_ur_attachments" params="parent child has_tool_changer">
+    <xacro:include filename="$(find picknik_accessories)/descriptions/brackets/ur_realsense_camera_adapter/picknik_ur_camera_adapter.urdf.xacro"/>
+    <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro"/>
+    <xacro:include filename="$(find robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
+    <xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
+    <xacro:include filename="$(find picknik_accessories)/descriptions/brackets/mtc_ur_tool_changer/mtc_ur_tool_changer.urdf.xacro"/>
+
+    <xacro:macro name="picknik_ur_attachments" params="prefix:='' parent child has_tool_changer">
+        <!-- prefix property -->
+        <xacro:property name="prefix_" value='${prefix + "_" if prefix else ""}' />
         <!-- parameters -->
         <xacro:arg name="simulation" default=""/>
 
-        <xacro:include filename="$(find picknik_accessories)/descriptions/brackets/ur_realsense_camera_adapter/picknik_ur_camera_adapter.urdf.xacro"/>
-        <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro"/>
-        <xacro:include filename="$(find robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
-        <xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
-        <xacro:include filename="$(find picknik_accessories)/descriptions/brackets/mtc_ur_tool_changer/mtc_ur_tool_changer.urdf.xacro"/>
-
         <!-- tool changer-->
         <xacro:if value="$(arg has_tool_changer)">
-            <xacro:mtc_ur_tool_changer prefix="" parent="${parent}" child="tool_changer_tool0" rotation="0"/>
-            <xacro:property name="ur_realsense_camera_adapter_parent" value="tool_changer_tool0" />
+            <xacro:mtc_ur_tool_changer prefix="${prefix_}" parent="${parent}" child="${prefix_}tool_changer_tool0" rotation="0"/>
+            <xacro:property name="ur_realsense_camera_adapter_parent" value="${prefix_}tool_changer_tool0" />
         </xacro:if>
         <xacro:unless value="$(arg has_tool_changer)">
             <xacro:property name="ur_realsense_camera_adapter_parent" value="${parent}" />
         </xacro:unless>
 
         <!-- wrist camera adapter and camera-->
-        <xacro:ur_realsense_camera_adapter prefix="" parent="${ur_realsense_camera_adapter_parent}" child_tool="realsense_camera_adapter_tool0" child_camera="d415_mount_link" rotation="0"/>
+        <xacro:ur_realsense_camera_adapter prefix="${prefix_}" parent="${ur_realsense_camera_adapter_parent}" child_tool="${prefix_}realsense_camera_adapter_tool0" child_camera="${prefix_}d415_mount_link" rotation="0"/>
 
         <xacro:property name="simulation" default="$(arg simulation)"/>
-        <xacro:realsense_d415 parent="d415_mount_link" name="wrist_mounted_camera" use_mesh="${not simulation == 'gazebo'}">
+        <xacro:realsense_d415 parent="${prefix_}d415_mount_link" name="${prefix_}wrist_mounted_camera" use_mesh="${not simulation == 'gazebo'}">
             <origin xyz="0 0 0" rpy="0 0 0"/>
         </xacro:realsense_d415>
 
-        <xacro:ur_to_robotiq prefix="" parent="realsense_camera_adapter_tool0" child="gripper_mount_link" rotation="0"/>
+        <!-- gripper and UR adapter-->
+        <xacro:ur_to_robotiq prefix="${prefix_}" parent="${prefix_}realsense_camera_adapter_tool0" child="${prefix_}gripper_mount_link" rotation="0"/>
 
         <xacro:if value="${simulation == 'gazebo'}">
             <xacro:property name="use_fake_hardware" default="false"/>
@@ -37,16 +40,16 @@
             <xacro:property name="use_fake_hardware" default="$(arg use_fake_hardware)"/>
             <xacro:property name="sim_ignition" default="false"/>
         </xacro:if>
-        <xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix=""
-                               parent="gripper_mount_link" use_fake_hardware="${use_fake_hardware}" sim_ignition="${sim_ignition}"
+        <xacro:robotiq_gripper name="${prefix_}RobotiqGripperHardwareInterface" prefix="${prefix_}"
+                               parent="${prefix_}gripper_mount_link" use_fake_hardware="${use_fake_hardware}" sim_ignition="${sim_ignition}"
                                com_port="$(arg tool_device_name)">
             <origin xyz="0 0 0" rpy="0 0 0"/>
         </xacro:robotiq_gripper>
 
         <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
         <link name="${child}"/>
         <joint name="${child}_joint" type="fixed">
-            <parent link="robotiq_85_base_link"/>
+            <parent link="${prefix_}robotiq_85_base_link"/>
             <child link="${child}"/>
             <origin xyz="0.0 0.0 0.134" rpy="0.0 0.0 ${pi}"/>
         </joint>

src/picknik_ur_multi_arm_config/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.22)
+project(picknik_ur_multi_arm_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+    objectives
+    waypoints
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/picknik_ur_multi_arm_config/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

src/picknik_ur_multi_arm_config/README.md

@@ -0,0 +1,5 @@
+# picknik_ur_multi_arm_config
+
+A MoveIt Pro example configuration package for using multiple UR5e arms using mock hardware.
+
+For further documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/picknik_ur_multi_arm_config/config/cameras.yaml

@@ -0,0 +1,68 @@
+# Placeholders for camera configuration.
+# Cameras are not a requirement for running MoveIt Pro, but this is provided as an
+# example for plugging in Realsense D415 camera topics to the application.
+
+- scene_camera:
+    camera_name: "scene_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: False
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/scene_camera/color/camera_info"
+    rgb_image: "/scene_camera/color/image_raw"
+
+    registered_rgb_depth_pair:
+      depth_info: "/scene_camera/color/camera_info"
+      depth_image: "/scene_camera/depth/image_rect_raw"
+      registered_info: "/scene_camera/color/camera_info"
+      registered_image: "/scene_camera/depth/image_rect_raw"
+
+- left_wrist_mounted_camera:
+    camera_name: "left_wrist_mounted_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: True
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/left_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/left_wrist_mounted_camera/color/image_raw"
+
+    # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
+    registered_rgb_depth_pair:
+      depth_info: "/left_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/left_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/left_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/left_wrist_mounted_camera/depth_registered/image_rect"
+
+- right_wrist_mounted_camera:
+    camera_name: "right_wrist_mounted_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: True
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/right_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/right_wrist_mounted_camera/color/image_raw"
+
+    # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
+    registered_rgb_depth_pair:
+      depth_info: "/right_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/right_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/right_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/right_wrist_mounted_camera/depth_registered/image_rect"