Add UR5e robot descriptions and MoveIt configurations for multi - end effector setups at CMS

src/custom-ur-descriptions/ur5e_moveit_configs/README.md

@@ -0,0 +1,132 @@
+# MoveIt Configurations
+
+This directory contains MoveIt configuration packages for UR5e robotic systems with various end-effector configurations. Each package provides complete MoveIt planning and execution capabilities for specific robot setups.
+
+## Available Configurations
+
+### ur_standalone_moveit_config
+
+- **Robot**: UR5e arm with Zivid camera and tool exchanger robotside
+- **Chain**: UR5e → Zivid Camera → TE_RobotSide
+
+### ur_zivid_hande_moveit_config
+
+- **Robot**: UR5e arm with Zivid camera, tool block, and Robotiq Hand-E gripper
+- **Chain**: UR5e → Zivid Camera → Tool Block → Hand-E Gripper
+
+### ur_zivid_epick_moveit_config
+
+- **Robot**: UR5e arm with Zivid camera, tool block, and ePick vacuum gripper
+- **Chain**: UR5e → Zivid Camera → Tool Block → ePick Gripper
+
+## Package Structure
+
+Each MoveIt configuration package follows a standard structure:
+
+```text
+ur_<config_name>_moveit_config/
+├── CMakeLists.txt                    # Build configuration
+├── package.xml                       # Package metadata
+├── config/                           # MoveIt configuration files
+│   ├── ur.srdf                      # Semantic robot description
+│   ├── kinematics.yaml              # Kinematics solver configuration
+│   ├── joint_limits.yaml            # Joint velocity/acceleration limits
+│   ├── initial_positions.yaml       # Default joint positions
+│   ├── ompl_planning.yaml          # OMPL planner configuration
+│   ├── moveit_controllers.yaml     # MoveIt controller configuration
+│   ├── ur_<gripper>_controllers.yaml # Hardware controller configuration
+│   └── ur.urdf.xacro               # Robot description for MoveIt
+├── launch/
+│   └── robot_bringup.launch.py      # Main launch file
+├── rviz/
+│   └── view_robot_mtc.rviz         # RViz configuration
+└── .setup_assistant                 # MoveIt Setup Assistant metadata
+```
+
+## Configuration Files Explained
+
+### Core MoveIt Files
+
+- **ur.srdf**: Semantic Robot Description Format file defining planning groups, disabled collisions, and poses
+- **kinematics.yaml**: Inverse kinematics solver configuration (uses KDL plugin)
+- **joint_limits.yaml**: Velocity/acceleration scaling and joint-specific limits
+- **ompl_planning.yaml**: Open Motion Planning Library planner configurations
+- **moveit_controllers.yaml**: Maps MoveIt planning groups to ROS2 controllers
+
+### Hardware Integration
+
+- **ur_&lt;gripper&gt;_controllers.yaml**: ROS2 control configuration for specific grippers
+- **ur.urdf.xacro**: Robot description file that MoveIt uses internally
+
+### Launch Configuration
+
+- **robot_bringup.launch.py**:
+  - Launches UR robot driver with hardware interface
+  - Starts MoveIt move_group node with planning capabilities
+  - Spawns gripper controllers
+  - Launches RViz for visualization
+  - Configures robot state publisher for TF transforms
+
+## Usage
+
+### Launch MoveIt Planning Environment
+
+```bash
+# For standalone configuration (no gripper)
+ros2 launch ur_standalone_moveit_config robot_bringup.launch.py
+
+# For Hand-E gripper configuration
+ros2 launch ur_zivid_hande_moveit_config robot_bringup.launch.py
+
+# For ePick vacuum gripper configuration
+ros2 launch ur_zivid_epick_moveit_config robot_bringup.launch.py
+```
+
+### Launch Parameters
+
+Each launch file accepts the following parameters:
+
+- `robot_ip`: IP address of the UR robot (default: '192.168.1.10')
+- `ur_type`: UR robot model (default: 'ur5e')
+- `description_package`: Package for robot config files (default: 'ur_description')
+- `description_file`: Custom URDF file path (points to ur5e_robot_description)
+- `controllers_file`: Hardware controller configuration file
+- `rviz_config`: RViz configuration file to load
+
+Example with custom parameters:
+
+```bash
+ros2 launch ur_zivid_hande_moveit_config robot_bringup.launch.py robot_ip:=192.168.1.100 ur_type:=ur5e
+```
+
+## Creating New MoveIt Configurations
+
+For detailed instructions on creating new MoveIt configurations, please refer to the official MoveIt documentation:
+
+**[MoveIt Setup Assistant Tutorial](https://moveit.picknik.ai/main/doc/examples/setup_assistant/setup_assistant_tutorial.html)**
+
+## Configuration Consistency
+
+All configurations maintain consistency in:
+
+- **Kinematics**: Same solver settings across all configs
+- **Joint Limits**: Identical UR arm limits, gripper-specific limits vary
+- **OMPL Planning**: Consistent planner configurations
+- **Launch Structure**: Standardized launch file patterns
+
+### Robot Naming Convention
+
+Robot name is set dynamically using the `ur_type` parameter (e.g., ur3e, ur5e, ur10e) to match ur_control's behavior. The launch files pass `name:=LaunchConfiguration("ur_type")` to ensure consistency between the `/robot_description` topic (used by action servers and ur_control) and MoveIt's move_group. This enables using the same launch files for any UR robot type.
+
+## Integration with Robot Description
+
+These MoveIt configurations work with robot descriptions from:
+
+- **ur5e_robot_description**: Custom URDF files for complete robot systems
+- **ur_description**: Standard UR robot configuration files
+- **robotiq_hande_description**: Hand-E gripper descriptions
+- **epick_config**: ePick vacuum gripper descriptions
+
+## TODO
+
+- Have consistent "moveit_home" values across SRDFs

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/.setup_assistant

@@ -0,0 +1,25 @@
+moveit_setup_assistant_config:
+  urdf:
+    package: ur5e_robot_description
+    relative_path: urdf/ur_standalone.urdf
+  srdf:
+    relative_path: config/ur.srdf
+  package_settings:
+    author_name: abondada
+    author_email: abondada@bnl.gov
+    generated_timestamp: 1753716818
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
+  modified_urdf:
+    xacros:
+      - control_xacro
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.22)
+project(ur_standalone_moveit_config)
+
+find_package(ament_cmake REQUIRED)
+
+ament_package()
+
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
+  install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+    PATTERN "setup_assistant.launch" EXCLUDE)
+endif()
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
+install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/initial_positions.yaml

@@ -0,0 +1,9 @@
+# Default initial positions for ur's ros2_control fake system
+
+initial_positions:
+  elbow_joint: 0
+  shoulder_lift_joint: 0
+  shoulder_pan_joint: 0
+  wrist_1_joint: 0
+  wrist_2_joint: 0
+  wrist_3_joint: 0

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/joint_limits.yaml

@@ -0,0 +1,40 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  elbow_joint:
+    has_velocity_limits: true
+    max_velocity: 3.1415926535897931
+    has_acceleration_limits: false
+    max_acceleration: 0
+  shoulder_lift_joint:
+    has_velocity_limits: true
+    max_velocity: 3.1415926535897931
+    has_acceleration_limits: false
+    max_acceleration: 0
+  shoulder_pan_joint:
+    has_velocity_limits: true
+    max_velocity: 3.1415926535897931
+    has_acceleration_limits: false
+    max_acceleration: 0
+  wrist_1_joint:
+    has_velocity_limits: true
+    max_velocity: 3.1415926535897931
+    has_acceleration_limits: false
+    max_acceleration: 0
+  wrist_2_joint:
+    has_velocity_limits: true
+    max_velocity: 3.1415926535897931
+    has_acceleration_limits: false
+    max_acceleration: 0
+  wrist_3_joint:
+    has_velocity_limits: true
+    max_velocity: 3.1415926535897931
+    has_acceleration_limits: false
+    max_acceleration: 0

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/kinematics.yaml

@@ -0,0 +1,5 @@
+ur_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.001
+  kinematics_solver_timeout: 0.1
+  kinematics_solver_attempts: 3

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/moveit_controllers.yaml

@@ -0,0 +1,19 @@
+# MoveIt uses this configuration for controller management
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - scaled_joint_trajectory_controller
+
+  scaled_joint_trajectory_controller:
+    type: FollowJointTrajectory
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    action_ns: follow_joint_trajectory
+    default: true

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ompl_planning.yaml

@@ -0,0 +1,61 @@
+planning_plugin: ompl_interface/OMPLPlanner
+request_adapters: 
+  default_planner_request_adapters/AddTimeOptimalParameterization
+planner_configs:
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+    range: 0.0
+    # goal_tolerance: 0.01
+    # num_planning_attempts: 10
+    # planning_time: 10.0
+  RRTstar:
+    type: geometric::RRTstar
+    range: 1.0
+  PRMkConfigDefault:
+    type: geometric::PRM
+    range: 0.0
+  RRTkConfigDefault:
+    type: geometric::RRT
+    range: 0.0
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    range: 0.0
+  BiTRRTkConfigDefault:
+    type: geometric::BiTRRT
+    range: 0.0
+  LazyPRMkConfigDefault:
+    type: geometric::LazyPRM
+    range: 0.0
+  SBLkConfigDefault:
+    type: geometric::SBL
+    range: 0.0
+
+ur_arm:
+  default_planner_config: RRTConnectkConfigDefault
+  planner_configs:
+    - RRTConnectkConfigDefault
+    - RRTstar
+    - PRMkConfigDefault
+    - RRTkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LazyPRMkConfigDefault
+    - SBLkConfigDefault
+
+# Optionally, if you have a gripper group:
+# gripper:
+#   default_planner_config: RRTConnectkConfigDefault
+#   planner_configs:
+#     - RRTConnectkConfigDefault

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/pilz_cartesian_limits.yaml

@@ -0,0 +1,7 @@
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57
+  max_rot_acc: 3.15
+  max_rot_dec: -5.0

src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ur.ros2_control.xacro

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="ur_ros2_control" params="name initial_positions_file">
+        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
+
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+                <plugin>mock_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="shoulder_pan_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['shoulder_pan_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="shoulder_lift_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['shoulder_lift_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="elbow_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['elbow_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="wrist_1_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['wrist_1_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="wrist_2_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['wrist_2_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="wrist_3_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['wrist_3_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>