Gazebo config for multi UR arm (#251)

Dual UR in space station with blocks

src/picknik_ur_multi_arm_gazebo_config/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.22)
+project(picknik_ur_multi_arm_gazebo_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+    launch
+    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_gazebo_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_gazebo_config/README.md

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

src/picknik_ur_multi_arm_gazebo_config/config/cameras.yaml

@@ -0,0 +1,67 @@
+# 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
+
+    # 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/depth_registered/camera_info"
+      registered_image: "/scene_camera/depth_registered/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/color/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/color/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"

src/picknik_ur_multi_arm_gazebo_config/config/config.yaml

@@ -0,0 +1,214 @@
+#
+#  This contains information for a unique instance of a robot.
+#
+
+# Optional parameters that can be read in your launch files for specific functionality
+optional_feature_params:
+  gazebo_world_name: "space_station_blocks_world.sdf"
+  gazebo_gui: False
+  gazebo_verbose: True
+
+hardware:
+  # Used by the ur_description package to set kinematics and geometry for a specific robot type.
+  # You can change this to another UR model but you must update any configuration affected by the different arm size.
+  # UR models in the ur_description package are ur3, ur3e, ur5, ur5e, ur10, ur10e, and ur16e.
+  type: "ur5e"
+
+  # This is the only option for this site_config
+  simulated: True
+
+  # Gazebo starts its own controller manager through the ros2_control plugin, so set this to False
+  launch_control_node: False
+
+  # If the MoveIt Pro Agent should launch the robot state publisher
+  # This should be false if you are launching the robot state publisher as part of drivers.
+  # [Optional, default=True]
+  launch_robot_state_publisher: True
+
+  # If the MoveIt Pro Agent should launch cameras when simulated.
+  launch_cameras_when_simulated: True
+
+  # The robot's IP address.
+  # [Required]
+  ip: "0.0.0.0"
+
+  # Specify any additional launch files for running the robot in simulation mode.
+  # Used when hardware.simulated is True.
+  # [Optional, defaults to a blank launch file if not specified]
+
+
+  simulated_hardware_launch_file:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "launch/sim/hardware_sim.launch.py"
+
+  # Configuration details for cameras and scene planning.
+  # [Required]
+  camera_config_file:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/cameras.yaml"
+
+  # Parameters used to configure the robot description through XACRO.
+  # A URDF and SRDF are both required.
+  # [Required]
+  robot_description:
+    urdf:
+      package: "picknik_ur_multi_arm_gazebo_config"
+      path: "description/dual_arm_ur.xacro"
+    srdf:
+      package: "picknik_ur_multi_arm_gazebo_config"
+      path: "config/moveit/dual_arm_ur.srdf"
+    # Specify any additional parameters required for the URDF.
+    # Many of these are specific to the UR descriptions packages, and can be customized as needed.
+    # [Optional]
+    urdf_params:
+      - name: "dual_arm_ur"
+      - prefix: ""
+      - use_fake_hardware: "false"
+      - gripper_name: "robotiq_85"
+      - use_pinch_links: "true"
+      - simulation: "gazebo"
+      # Using the ogre renderer as it is compatible with most host hardware.
+      # ogre2 (which is the Gazebo default) is also an option if your system supports it.
+      - gazebo_renderer: ${GAZEBO_RENDERER:-ogre}
+      - fake_sensor_commands: "false"
+      - headless_mode: "true"
+      - robot_ip: "%>> hardware.ip"
+      - joint_limits_parameters_file:
+          package: "picknik_ur_base_config"
+          path: "config/moveit/joint_limits.yaml"
+      # The following files are loaded based on the ur_description package layout.
+      # To use parameters from a different package, place them in a config/ROBOT_NAME/ directory,
+      # replace ROBOT_NAME with the value used for hardware.type in this file.
+      - kinematics_parameters_file:
+      # Load default_kinematics.yaml from ur_description/config/<ur_type>
+          package: "ur_description"
+          path: "config/%>> hardware.type <<%/default_kinematics.yaml"
+      - physical_parameters_file:
+      # Load physical_parameters.yaml from ur_description/config/<ur_type>
+          package: "ur_description"
+          path: "config/%>> hardware.type <<%/physical_parameters.yaml"
+      - visual_parameters_file:
+      # Load visual_parameters.yaml from ur_description/config/<ur_type>
+          package: "ur_description"
+          path: "config/%>> hardware.type <<%/visual_parameters.yaml"
+
+# Sets ROS global params for launch.
+# [Optional]
+ros_global_params:
+  # Whether or not to use simulated time.
+  # [Optional, default=False]
+  use_sim_time: True
+
+# Configuration files for MoveIt.
+# For more information, refer to https://moveit.picknik.ai/main/doc/how_to_guides/moveit_configuration/moveit_configuration_tutorial.html
+# [Required]
+moveit_params:
+  # Used by the Waypoint Manager to save joint states from this joint group.
+  joint_group_name: "manipulator_right"
+
+  ompl_planning:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/moveit/ompl_planning.yaml"
+  pilz_planning:
+    package: "picknik_ur_base_config"
+    path: "config/moveit/pilz_industrial_motion_planner_planning.yaml"
+  stomp_planning:
+    package: "picknik_ur_base_config"
+    path: "config/moveit/stomp_planning.yaml"
+  kinematics:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/moveit/trac_ik_kinematics_distance.yaml"
+  servo:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/moveit/ur_servo.yaml"
+  sensors_3d:
+    package: "picknik_ur_base_config"
+    path: "config/moveit/sensors_3d.yaml"
+  servo_kinematics:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/moveit/trac_ik_kinematics_speed.yaml"
+  joint_limits:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/moveit/dual_arm_joint_limits.yaml"
+  pilz_cartesian_limits:
+    package: "picknik_ur_base_config"
+    path: "config/moveit/pilz_cartesian_limits.yaml"
+
+  publish:
+    planning_scene: True
+    geometry_updates: True
+    state_updates: True
+    transforms_updates: True
+
+  trajectory_execution:
+    manage_controllers: True
+    allowed_execution_duration_scaling: 2.0
+    allowed_goal_duration_margin: 5.0
+    allowed_start_tolerance: 0.01
+
+# Additional configurable parameters for the MoveIt Pro user interface.
+# [Required]
+ui_params:
+  # By default, MoveIt Pro uses a frame called "grasp_link" for tool grasp pose rendering
+  # and planning.
+  # [Required]
+  servo_endpoint_frame_id: "right_grasp_link"
+
+
+# This configures what controllers gets run at startup
+ros2_control:
+  config:
+    package: "picknik_ur_multi_arm_gazebo_config"
+    path: "config/control/picknik_dual_ur.ros2_control.yaml"
+  # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
+  # If not specified, it is up to the user to ensure the appropriate controllers are active and available
+  # for running the application.
+  # [Optional, default=[]]
+  controllers_active_at_startup:
+    - "joint_state_broadcaster"
+    - "servo_controller"
+    - "left_robotiq_gripper_controller"
+    - "right_robotiq_gripper_controller"
+  # Load but do not start these controllers so they can be activated later if needed.
+  # [Optional, default=[]]
+  controllers_inactive_at_startup:
+    - "left_joint_trajectory_controller"
+    - "right_joint_trajectory_controller"
+    - "dual_arm_joint_trajectory_controller"
+  # Any controllers here will not be spawned by MoveIt Pro.
+  # [Optional, default=[]]
+  controllers_not_managed: []
+  # Optionally configure remapping rules to let multiple controllers receive commands on the same topic.
+  # [Optional, default=[]]
+  controller_shared_topics: []
+
+# Octomap manager configuration parameters
+octomap_manager:
+  # Input point cloud topic name. The *output* point cloud topic published by
+  # the Octomap manager node is defined in sensors_3d.yaml.
+  input_point_cloud_topic: "/left_wrist_mounted_camera/depth/color/points"
+
+# Configuration for loading behaviors and objectives.
+# [Required]
+objectives:
+  # List of plugins for loading custom behaviors.
+  # [Required]
+  behavior_loader_plugins:
+    # This plugin will load the core MoveIt Pro behaviors.
+    # Add additional plugin loaders as needed.
+    core:
+      - "moveit_studio::behaviors::CoreBehaviorsLoader"
+      - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
+      - "moveit_studio::behaviors::ServoBehaviorsLoader"
+      - "moveit_studio::behaviors::VisionBehaviorsLoader"
+  # Specify source folder for objectives
+  # [Required]
+  objective_library_paths:
+    core:
+      package_name: "picknik_ur_multi_arm_gazebo_config"
+      relative_path: "objectives"
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "picknik_ur_multi_arm_gazebo_config"
+    relative_path: "waypoints/waypoints.yaml"