diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml deleted file mode 100644 index 94c7c66..0000000 --- a/.github/workflows/build.yaml +++ /dev/null @@ -1,65 +0,0 @@ -name: Build with ROS-Industrial CI - -on: push - -jobs: - industrial_ci: - strategy: - matrix: - env: - - IMAGE: humble - CLANG_TIDY: false - env: - DOCKER_IMAGE: ros:${{ matrix.env.IMAGE }} - UPSTREAM_WORKSPACE: dependencies_rolling.repos - AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src - TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE='Release' - CCACHE_DIR: ${{ github.workspace }}/.ccache - BASEDIR: ${{ github.workspace }}/.work - CACHE_PREFIX: ${{ matrix.env.IMAGE }} - CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} - - name: ${{ matrix.env.IMAGE }}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || '' }} - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - name: cache upstream_ws - uses: pat-s/always-upload-cache@v2.1.3 - with: - path: ${{ env.BASEDIR }}/upstream_ws - key: upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('dependencies_rolling.repos') }}-${{ github.run_id }} - restore-keys: | - upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('dependencies_rolling.repos') }} - # The target directory cache doesn't include the source directory because - # that comes from the checkout. See "prepare target_ws for cache" task below - - name: cache target_ws - uses: pat-s/always-upload-cache@v2.1.3 - with: - path: ${{ env.BASEDIR }}/target_ws - key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} - restore-keys: | - target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - - name: cache ccache - uses: pat-s/always-upload-cache@v2.1.3 - with: - path: ${{ env.CCACHE_DIR }} - key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} - restore-keys: | - ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} - ccache-${{ env.CACHE_PREFIX }} - - name: industrial_ci - uses: 'ros-industrial/industrial_ci@master' - env: ${{ matrix.env }} - - name: upload test artifacts (on failure) - uses: actions/upload-artifact@v2 - if: failure() - with: - name: test-results - path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - - name: prepare target_ws for cache - if: ${{ always() }} - run: | - du -sh ${{ env.BASEDIR }}/target_ws - sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete - sudo rm -rf ${{ env.BASEDIR }}/target_ws/src - du -sh ${{ env.BASEDIR }}/target_ws \ No newline at end of file diff --git a/.github/workflows/docker_build_pipeline.yaml b/.github/workflows/docker_build_pipeline.yaml deleted file mode 100644 index cefb9b1..0000000 --- a/.github/workflows/docker_build_pipeline.yaml +++ /dev/null @@ -1,70 +0,0 @@ -name: Docker Image Builder CI - -on: - push: - branches: [ main ] - pull_request: - branches: [ main ] - -jobs: - - build: - - runs-on: ubuntu-latest - permissions: - packages: write - contents: read - defaults: - run: - working-directory: ./docker - - steps: - - name: Check out the repo - uses: actions/checkout@v2 - - - name: Set up QEMU - id: qemu - uses: docker/setup-qemu-action@v1 - with: - image: tonistiigi/binfmt:latest - platforms: all - - - name: Set up Docker Buildx - id: buildx - uses: docker/setup-buildx-action@v1.0.4 - with: - buildx-version: latest - qemu-version: latest - - - name: Log in to Docker Hub - uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9 - with: - username: ${{ secrets.DOCKER_USERNAME }} - password: ${{ secrets.DOCKER_PASSWORD }} - - - name: Log in to the Container registry - uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9 - with: - registry: ghcr.io - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Extract metadata (tags, labels) for Docker - id: meta - uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 - with: - images: | - ragesh18/ur5e_cell - ghcr.io/${{ github.repository }} - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} - - - name: Build and push Docker images - uses: docker/build-push-action@v2 - with: - file: docker/Dockerfile - context: ./docker - platforms: linux/arm64, linux/amd64 - push: true - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} \ No newline at end of file diff --git a/.gitignore b/.gitignore index e69de29..7e99e36 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/Readme.md b/Readme.md index d21b684..cc88424 100644 --- a/Readme.md +++ b/Readme.md @@ -1,52 +1 @@ -# Robot workcell description package -This package provides the workcell description of the ur5e robot, Shunk egp50 gripper, and the robot cell from ipa_326. - -```` -vcs import src --skip-existing --input src/ur5e_cell/dependencies_rolling.repos -```` - -Build & source your workspace -```` -colcon build --symlink-install -source install/setup.bash -```` -Launch the workcell -```` -ros2 launch ur5e_cell_description view_ur_cell.launch.py - -```` - -**Tip:** To check xacro syntax during development -```` -check_urdf <(xacro workcell.urdf.xacro) -```` -# Real Robot Bringup - -``` -vcs import src --skip-existing --input src/ur5e_cell/dependencies_rolling.repos -``` -This will pull necessary drivers and moveit_config packages into the workspace - -Build & source your workspace -```` -colcon build --symlink-install -source install/setup.bash -```` -Launch your robot driver and moveit -``` -ros2 launch ur5e_cell_bringup ur5e_cell_bringup.launch.py -``` ------------------------------------------------------------ - -# Docker deployment on Real robot - -Necessary for RVIZ2 -```` -xhost local:root -```` - -Run docker container -```` -docker-compose --file ~/ur_ws/src/ur5e_cell/docker/docker-compose.yaml up -```` ------------------------------------------------------------ \ No newline at end of file +# Robot workcell description package \ No newline at end of file diff --git a/dependencies_rolling.repos b/dependencies_rolling.repos deleted file mode 100644 index 423713b..0000000 --- a/dependencies_rolling.repos +++ /dev/null @@ -1,15 +0,0 @@ -repositories: - Universal_Robots_ROS2_Description: - type: git - url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: 2.0.0 - - Universal_Robots_ROS2_Driver: - type: git - url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git - version: 2.2.4 - - ros2_controllers: - type: git - url: https://github.com/ros-controls/ros2_controllers - version: 2.13.0 \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile deleted file mode 100644 index 42d27b0..0000000 --- a/docker/Dockerfile +++ /dev/null @@ -1,30 +0,0 @@ -FROM ros:humble - -RUN apt-get update && apt-get install -y \ - && apt install python3-vcstool \ - && apt-get install -y git - -ENV ros2_ws=/root/ur_ws -RUN mkdir -p $ros2_ws/src -WORKDIR $ros2_ws/src - -RUN git clone -b ur5e-working-with-msa https://github.com/ipa-rar/ur5e_cell.git - -WORKDIR $ros2_ws -RUN vcs import src --skip-existing --input src/ur5e_cell/dependencies_rolling.repos - -RUN rosdep update \ - && rosdep install --from-paths src --ignore-src -r -y \ - && echo "Dependecies are installed in the workspace" - -RUN . /opt/ros/humble/setup.sh \ - && colcon build \ - && rm -rf /var/lib/apt/lists/* \ - && echo "Building packages completed" - - -COPY /entrypoint.sh /entrypoint.sh -RUN chmod +x /entrypoint.sh - -ENTRYPOINT ["/entrypoint.sh"] -CMD ["bash"] \ No newline at end of file diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml deleted file mode 100644 index c9a9b88..0000000 --- a/docker/docker-compose.yaml +++ /dev/null @@ -1,21 +0,0 @@ -version: '2.3' - -services: - ur_robot: - image: ragesh18/ur5e_cell:main - container_name: ur5e_cell - platform: linux/amd64 - restart: always - pull_policy: always - # Uncomment this for development - # build: - # context: . - # dockerfile: Dockerfile - network_mode: "host" - ipc: host - environment: - - ROS_DOMAIN_ID=2 - - DISPLAY - volumes: - - /tmp/.X11-unix:/tmp/.X11-unix:rw - command: ros2 launch ur5_cell_moveit_config robot.launch.py \ No newline at end of file diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh deleted file mode 100644 index b73b512..0000000 --- a/docker/entrypoint.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash -set -e - -source /opt/ros/humble/setup.sh -source ~/ur_ws/install/setup.sh - -exec "$@" \ No newline at end of file diff --git a/ur5_cell_moveit_config/README.md b/ur5_cell_moveit_config/README.md deleted file mode 100644 index b694378..0000000 --- a/ur5_cell_moveit_config/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Dependencies - -Universal_Robots_ROS2_Description: 2.0.0 -Universal_Robots_ROS2_Driver: 2.2.4 -ros2_controllers: 2.13.0 \ No newline at end of file diff --git a/ur5_cell_moveit_config/config/initial_positions.yaml b/ur5_cell_moveit_config/config/initial_positions.yaml deleted file mode 100644 index 0fd53b9..0000000 --- a/ur5_cell_moveit_config/config/initial_positions.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# Default initial positions for ur5_msa'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 \ No newline at end of file diff --git a/ur5_cell_moveit_config/config/ur5_msa.urdf.xacro b/ur5_cell_moveit_config/config/ur5_msa.urdf.xacro deleted file mode 100644 index caaeb0f..0000000 --- a/ur5_cell_moveit_config/config/ur5_msa.urdf.xacro +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/ur5_cell_moveit_config/launch/demo.launch.py b/ur5_cell_moveit_config/launch/demo.launch.py deleted file mode 100644 index 080899a..0000000 --- a/ur5_cell_moveit_config/launch/demo.launch.py +++ /dev/null @@ -1,10 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_demo_launch -from launch_ros.actions import Node -from launch.conditions import UnlessCondition, IfCondition - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa_fake", package_name="ur5_cell_moveit_config").to_moveit_configs() - ld = generate_demo_launch(moveit_config) - return ld diff --git a/ur5_cell_moveit_config/launch/robot.launch.py b/ur5_cell_moveit_config/launch/robot.launch.py deleted file mode 100644 index 4883d1d..0000000 --- a/ur5_cell_moveit_config/launch/robot.launch.py +++ /dev/null @@ -1,139 +0,0 @@ -from os.path import join -import pprint -from launch import LaunchDescription -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_demo_launch -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.conditions import UnlessCondition, IfCondition -from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() - #pprint.pprint(moveit_config.to_dict()) - ld = LaunchDescription() - ld.add_action( - DeclareBooleanLaunchArg( - "db", - default_value=False, - description="By default, we do not start a database (it can be large)", - ) - ) - ld.add_action( - DeclareBooleanLaunchArg( - "debug", - default_value=False, - description="By default, we are not in debug mode", - ) - ) - ld.add_action( - DeclareBooleanLaunchArg( - "use_rviz", - default_value=True) - ) - - - virtual_joints_launch = ( - moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" - ) - - if virtual_joints_launch.exists(): - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource(str(virtual_joints_launch)), - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/rsp.launch.py") - ), - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/move_group.launch.py") - ), - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/moveit_rviz.launch.py") - ), - condition=IfCondition(LaunchConfiguration("use_rviz")), - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/warehouse_db.launch.py") - ), - condition=IfCondition(LaunchConfiguration("db")), - ) - ) - # UR hardware specific things - ld.add_action( - Node( - package="ur_robot_driver", - executable="ur_ros2_control_node", - parameters=[ - moveit_config.robot_description, - str(moveit_config.package_path / "config/ros2_controllers.yaml")], - output="screen" - ) - ) - - ld.add_action( - Node( - package="ur_robot_driver", - condition=IfCondition("True") and UnlessCondition("False"), - executable="dashboard_client", - name="dashboard_client", - output="screen", - emulate_tty=True, - parameters=[{"robot_ip": "192.168.56.2"}], - ) - ) - - ld.add_action( - Node( - package="ur_robot_driver", - executable="controller_stopper_node", - name="controller_stopper", - output="screen", - emulate_tty=True, - condition=UnlessCondition("False"), - parameters=[ - {"headless_mode": True}, - {"joint_controller_active": False}, - { - "consistent_controllers": [ - "io_and_status_controller", - "force_torque_sensor_broadcaster", - "joint_state_broadcaster", - "speed_scaling_state_broadcaster", - ] - }, - ], - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/spawn_controllers.launch.py") - ), - ) - ) - - return ld \ No newline at end of file diff --git a/ur5_cell_moveit_config/launch/ros2_control.launch.py b/ur5_cell_moveit_config/launch/ros2_control.launch.py deleted file mode 100644 index e69de29..0000000 diff --git a/ur5e_cell_bringup/launch/ur5e_cell_bringup.launch.py b/ur5e_cell_bringup/launch/ur5e_cell_bringup.launch.py deleted file mode 100644 index 5957ff8..0000000 --- a/ur5e_cell_bringup/launch/ur5e_cell_bringup.launch.py +++ /dev/null @@ -1,107 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Initialize Arguments - robot_ip = LaunchConfiguration("robot_ip") - use_fake_hardware = LaunchConfiguration("use_fake_hardware") - fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") - activate_joint_controller = LaunchConfiguration("activate_joint_controller") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - - # Declare arguments - launch_files = [] - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_ip", - default_value="192.168.56.2", - description="IP address by which the robot can be reached.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_fake_hardware", - default_value="false", - description="Start robot with fake hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "fake_sensor_commands", - default_value="false", - description="Enable fake command interfaces for sensors used for simple simulations. \ - Used only if 'use_fake_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "initial_joint_controller", - default_value="scaled_joint_trajectory_controller", - description="Initially loaded robot controller.", - choices=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - "forward_velocity_controller", - "forward_position_controller", - ], - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ur5e_cell_description", - description="Description package with robot URDF/XACRO files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="workcell.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "activate_joint_controller", - default_value="true", - description="Activate loaded joint controller.", - ) - ) - - ur_control_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare("ur_robot_driver"), "/launch", "/ur_control.launch.py"]), - launch_arguments={ - "ur_type": "ur5e", - "robot_ip": robot_ip, - "use_fake_hardware": use_fake_hardware, - "fake_sensor_commands": fake_sensor_commands, - "initial_joint_controller": initial_joint_controller, - "activate_joint_controller": activate_joint_controller, - "description_package": description_package, - "description_file": description_file, - }.items(), - ) - - moveit_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare("ur5e_cell_msa_config"), "/launch", "/move_group.launch.py"]), - ) - moveit_rviz_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([FindPackageShare( - "ur5e_cell_msa_config"), "/launch", "/moveit_rviz.launch.py"]), - ) - - launch_files.append(ur_control_launch) - launch_files.append(moveit_launch) - launch_files.append(moveit_rviz_launch) - - return LaunchDescription(declared_arguments + launch_files) diff --git a/ur5e_cell_description/config/initial_positions.yaml b/ur5e_cell_description/config/initial_positions.yaml index 36286f9..77eb4e2 100644 --- a/ur5e_cell_description/config/initial_positions.yaml +++ b/ur5e_cell_description/config/initial_positions.yaml @@ -1,14 +1,14 @@ initial_positions: - shoulder_pan_joint: 0.0 + shoulder_pan_joint: -1.57 shoulder_lift_joint: -1.57 - elbow_joint: 0.0 + elbow_joint: -1.57 wrist_1_joint: -1.57 - wrist_2_joint: 0.0 + wrist_2_joint: 1.57 wrist_3_joint: 0.0 -shoulder_pan_joint: 0.0 +shoulder_pan_joint: -1.57 shoulder_lift_joint: -1.57 -elbow_joint: 0.0 +elbow_joint: -1.57 wrist_1_joint: -1.57 -wrist_2_joint: 0.0 +wrist_2_joint: 1.57 wrist_3_joint: 0.0 diff --git a/ur5e_cell_description/config/ur5e/default_kinematics.yaml b/ur5e_cell_description/config/ur5e/default_kinematics.yaml index 39cad2c..17e3b78 100644 --- a/ur5e_cell_description/config/ur5e/default_kinematics.yaml +++ b/ur5e_cell_description/config/ur5e/default_kinematics.yaml @@ -41,4 +41,4 @@ kinematics: roll: 1.5724268955944378 pitch: 3.1415926535897931 yaw: 3.141592586202488 - hash: calib_11698609823524220358 \ No newline at end of file + hash: calib_11698609823524220358 diff --git a/ur5e_cell_description/config/ur5e/joint_limits.yaml b/ur5e_cell_description/config/ur5e/joint_limits.yaml index e3c4648..83e89ca 100644 --- a/ur5e_cell_description/config/ur5e/joint_limits.yaml +++ b/ur5e_cell_description/config/ur5e/joint_limits.yaml @@ -45,7 +45,7 @@ joint_limits: # # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for # more information. - max_position: !degrees 180.0 + max_position: !degrees 0.0 max_velocity: !degrees 180.0 min_position: !degrees -180.0 wrist_1_joint: diff --git a/ur5e_cell_description/package.xml b/ur5e_cell_description/package.xml index b02a53c..4d797b6 100644 --- a/ur5e_cell_description/package.xml +++ b/ur5e_cell_description/package.xml @@ -14,6 +14,11 @@ ur_description ur_moveit_config ur_robot_driver + realsense2_description + moveit + moveit_ros + moveit_setup_assistant + ros2launch ament_lint_auto diff --git a/ur5e_cell_description/urdf/workcell.urdf.xacro b/ur5e_cell_description/urdf/workcell.urdf.xacro index f75c9c0..7005478 100644 --- a/ur5e_cell_description/urdf/workcell.urdf.xacro +++ b/ur5e_cell_description/urdf/workcell.urdf.xacro @@ -1,13 +1,14 @@ - - + - - - + + + + + @@ -39,11 +40,17 @@ - + + + + + + - + + - - - - - - $(arg simulation_controllers) - - - + + + + + + + + + - + + - + + + - - + + - - - \ No newline at end of file + diff --git a/ur5_cell_moveit_config/.setup_assistant b/ur5e_cell_moveit_config/.setup_assistant similarity index 52% rename from ur5_cell_moveit_config/.setup_assistant rename to ur5e_cell_moveit_config/.setup_assistant index bc1fa6d..f363873 100644 --- a/ur5_cell_moveit_config/.setup_assistant +++ b/ur5e_cell_moveit_config/.setup_assistant @@ -1,14 +1,13 @@ moveit_setup_assistant_config: urdf: - package: ur_description - relative_path: urdf/ur.urdf.xacro - xacro_args: name:="ur5_msa" ur_type:="ur5e" + package: ur5e_cell_description + relative_path: urdf/workcell.urdf.xacro srdf: - relative_path: config/ur5_msa.srdf + relative_path: config/ur5e_workcell.srdf package_settings: - author_name: Christoph Hellmann Santos - author_email: ipa@ipa.fraunhofer.de - generated_timestamp: 1666960052 + author_name: cmh + author_email: cmh@ipa.fraunhofer.de + generated_timestamp: 1676108726 control_xacro: command: - position diff --git a/ur5_cell_moveit_config/CMakeLists.txt b/ur5e_cell_moveit_config/CMakeLists.txt similarity index 79% rename from ur5_cell_moveit_config/CMakeLists.txt rename to ur5e_cell_moveit_config/CMakeLists.txt index 48cf0b3..e492a4c 100644 --- a/ur5_cell_moveit_config/CMakeLists.txt +++ b/ur5e_cell_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.10.2) -project(ur5_cell_moveit_config) +cmake_minimum_required(VERSION 3.22) +project(ur5e_cell_moveit_config) find_package(ament_cmake REQUIRED) diff --git a/ur5e_cell_moveit_config/config/initial_positions.yaml b/ur5e_cell_moveit_config/config/initial_positions.yaml new file mode 100644 index 0000000..96fd3d0 --- /dev/null +++ b/ur5e_cell_moveit_config/config/initial_positions.yaml @@ -0,0 +1,16 @@ +# Default initial positions for ur5e_workcell's ros2_control fake system + +initial_positions: + elbow_joint: -1.57 + shoulder_lift_joint: -1.57 + shoulder_pan_joint: -1.57 + wrist_1_joint: -1.57 + wrist_2_joint: 1.57 + wrist_3_joint: 0.0 + +elbow_joint: -1.57 +shoulder_lift_joint: -1.57 +shoulder_pan_joint: -1.57 +wrist_1_joint: -1.57 +wrist_2_joint: 1.57 +wrist_3_joint: 0.0 \ No newline at end of file diff --git a/ur5_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml similarity index 58% rename from ur5_cell_moveit_config/config/joint_limits.yaml rename to ur5e_cell_moveit_config/config/joint_limits.yaml index b68248a..5ebd579 100644 --- a/ur5_cell_moveit_config/config/joint_limits.yaml +++ b/ur5e_cell_moveit_config/config/joint_limits.yaml @@ -1,12 +1,3 @@ -# Joints limits -# -# Sources: -# -# - Universal Robots e-Series, User Manual, UR5e, Version 5.8 -# https://s3-eu-west-1.amazonaws.com/ur-support-site/69091/99404_UR5e_User_Manual_en_Global.pdf -# - Support > Articles > UR articles > Max. joint torques -# https://www.universal-robots.com/articles/ur-articles/max-joint-torques -# retrieved: 2020-06-16, last modified: 2020-06-09 joint_limits: shoulder_pan_joint: # acceleration limits are not publicly available @@ -15,10 +6,10 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 150.0 - max_position: 3.14159265359 + max_position: -0.35 max_velocity: 1.57079632679 - min_position: -3.14159265359 - max_acceleration: 3.49 + min_position: -2.0 + max_acceleration: 5.0 shoulder_lift_joint: # acceleration limits are not publicly available has_acceleration_limits: true @@ -26,10 +17,10 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 150.0 - max_position: 3.14159265359 + max_position: 0.0 max_velocity: 1.57079632679 min_position: -3.14159265359 - max_acceleration: 3.49 + max_acceleration: 5.0 elbow_joint: # acceleration limits are not publicly available has_acceleration_limits: true @@ -37,20 +28,10 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 150.0 - # we artificially limit this joint to half its actual joint position limit - # to avoid (MoveIt/OMPL) planning problems, as due to the physical - # construction of the robot, it's impossible to rotate the 'elbow_joint' - # over more than approx +- 1 pi (the shoulder lift joint gets in the way). - # - # This leads to planning problems as the search space will be divided into - # two sections, with no connections from one to the other. - # - # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for - # more information. - max_position: 1.57079632679 + max_position: 0.0 max_velocity: 1.57079632679 - min_position: -1.57079632679 - max_acceleration: 3.49 + min_position: -3.14159265359 + max_acceleration: 5.0 wrist_1_joint: # acceleration limits are not publicly available has_acceleration_limits: true @@ -61,7 +42,7 @@ joint_limits: max_position: 3.14159265359 max_velocity: 1.57079632679 min_position: -3.14159265359 - max_acceleration: 3.49 + max_acceleration: 5.0 wrist_2_joint: # acceleration limits are not publicly available has_acceleration_limits: true @@ -72,7 +53,7 @@ joint_limits: max_position: 3.14159265359 max_velocity: 1.57079632679 min_position: -3.14159265359 - max_acceleration: 3.49 + max_acceleration: 5.0 wrist_3_joint: # acceleration limits are not publicly available has_effort_limits: true @@ -83,4 +64,4 @@ joint_limits: max_velocity: 1.57079632679 min_position: -3.14159265359 has_acceleration_limits: true - max_acceleration: 3.49 + max_acceleration: 5.0 diff --git a/ur5_cell_moveit_config/config/kinematics.yaml b/ur5e_cell_moveit_config/config/kinematics.yaml similarity index 100% rename from ur5_cell_moveit_config/config/kinematics.yaml rename to ur5e_cell_moveit_config/config/kinematics.yaml diff --git a/ur5_cell_moveit_config/config/moveit.rviz b/ur5e_cell_moveit_config/config/moveit.rviz similarity index 100% rename from ur5_cell_moveit_config/config/moveit.rviz rename to ur5e_cell_moveit_config/config/moveit.rviz diff --git a/ur5_cell_moveit_config/config/moveit_controllers.yaml b/ur5e_cell_moveit_config/config/moveit_controllers.yaml similarity index 54% rename from ur5_cell_moveit_config/config/moveit_controllers.yaml rename to ur5e_cell_moveit_config/config/moveit_controllers.yaml index 2e4b7ff..6eb6ada 100644 --- a/ur5_cell_moveit_config/config/moveit_controllers.yaml +++ b/ur5e_cell_moveit_config/config/moveit_controllers.yaml @@ -1,30 +1,15 @@ # 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 - joint_trajectory_controller - #- speed_scaling_state_broadcaster - - - # scaled_joint_trajectory_controller: - # action_ns: follow_joint_trajectory - # type: FollowJointTrajectory - # default: true - # joints: - # - shoulder_pan_joint - # - shoulder_lift_joint - # - elbow_joint - # - wrist_1_joint - # - wrist_2_joint - # - wrist_3_joint - joint_trajectory_controller: - action_ns: follow_joint_trajectory type: FollowJointTrajectory + action_ns: follow_joint_trajectory default: true joints: - shoulder_pan_joint @@ -32,4 +17,6 @@ moveit_simple_controller_manager: - elbow_joint - wrist_1_joint - wrist_2_joint - - wrist_3_joint \ No newline at end of file + - wrist_3_joint + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/ur5_cell_moveit_config/config/ompl_planning.yaml b/ur5e_cell_moveit_config/config/ompl_planning.yaml similarity index 98% rename from ur5_cell_moveit_config/config/ompl_planning.yaml rename to ur5e_cell_moveit_config/config/ompl_planning.yaml index 75f8fff..4cc79a3 100644 --- a/ur5_cell_moveit_config/config/ompl_planning.yaml +++ b/ur5e_cell_moveit_config/config/ompl_planning.yaml @@ -75,5 +75,5 @@ arm: - PRMkConfigDefault - PRMstarkConfigDefault ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE - #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) longest_valid_segment_fraction: 0.01 diff --git a/ur5_cell_moveit_config/config/pilz_cartesian_limits.yaml b/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml similarity index 62% rename from ur5_cell_moveit_config/config/pilz_cartesian_limits.yaml rename to ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml index f0dd367..b2997ca 100644 --- a/ur5_cell_moveit_config/config/pilz_cartesian_limits.yaml +++ b/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml @@ -1,5 +1,6 @@ +# Limits for the Pilz planner cartesian_limits: max_trans_vel: 1.0 max_trans_acc: 2.25 max_trans_dec: -5.0 - max_rot_vel: 1.57 \ No newline at end of file + max_rot_vel: 1.57 diff --git a/ur5_cell_moveit_config/config/ros2_controllers.yaml b/ur5e_cell_moveit_config/config/ros2_controllers.yaml similarity index 100% rename from ur5_cell_moveit_config/config/ros2_controllers.yaml rename to ur5e_cell_moveit_config/config/ros2_controllers.yaml diff --git a/ur5_cell_moveit_config/config/ur5_msa.ros2_control.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro similarity index 96% rename from ur5_cell_moveit_config/config/ur5_msa.ros2_control.xacro rename to ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro index 9121460..03add06 100644 --- a/ur5_cell_moveit_config/config/ur5_msa.ros2_control.xacro +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro @@ -1,6 +1,6 @@ - + diff --git a/ur5_cell_moveit_config/config/ur5_msa.srdf b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf similarity index 91% rename from ur5_cell_moveit_config/config/ur5_msa.srdf rename to ur5e_cell_moveit_config/config/ur5e_workcell.srdf index 31a1923..a11ae58 100644 --- a/ur5_cell_moveit_config/config/ur5_msa.srdf +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf @@ -10,13 +10,13 @@ - - - - - - + + + + + + @@ -25,6 +25,13 @@ + + + + + + + diff --git a/ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro similarity index 89% rename from ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro rename to ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro index 25bd991..c77c9b7 100644 --- a/ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro @@ -1,6 +1,6 @@ - + @@ -11,6 +11,6 @@ - + - + \ No newline at end of file diff --git a/ur5e_cell_moveit_config/launch/demo.launch.py b/ur5e_cell_moveit_config/launch/demo.launch.py new file mode 100644 index 0000000..2428e2c --- /dev/null +++ b/ur5e_cell_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/ur5_cell_moveit_config/launch/move_group.launch.py b/ur5e_cell_moveit_config/launch/move_group.launch.py similarity index 64% rename from ur5_cell_moveit_config/launch/move_group.launch.py rename to ur5e_cell_moveit_config/launch/move_group.launch.py index f8c0488..380b2b9 100644 --- a/ur5_cell_moveit_config/launch/move_group.launch.py +++ b/ur5e_cell_moveit_config/launch/move_group.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_move_group_launch(moveit_config) diff --git a/ur5_cell_moveit_config/launch/moveit_rviz.launch.py b/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py similarity index 64% rename from ur5_cell_moveit_config/launch/moveit_rviz.launch.py rename to ur5e_cell_moveit_config/launch/moveit_rviz.launch.py index 8211c35..8d6ed1b 100644 --- a/ur5_cell_moveit_config/launch/moveit_rviz.launch.py +++ b/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/robot.launch.py b/ur5e_cell_moveit_config/launch/robot.launch.py new file mode 100644 index 0000000..75cbe1c --- /dev/null +++ b/ur5e_cell_moveit_config/launch/robot.launch.py @@ -0,0 +1,86 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +from launch_ros.substitutions import FindPackageShare +from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch + +from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg + +from moveit_configs_utils.launches import ( + generate_move_group_launch, + generate_moveit_rviz_launch, +) + + +def generate_launch_description(): + + database_config = DeclareBooleanLaunchArg( + "db", + default_value=False, + description="By default, we do not start a database (it can be large)", + ) + + + debug_config = DeclareBooleanLaunchArg( + "debug", + default_value=False, + description="By default, we are not in debug mode", + ) + + rviz_config = DeclareBooleanLaunchArg( + "use_rviz", + default_value=True, + description="Whether to use RVIZ or not." + ) + + robot_ip_config = DeclareLaunchArgument( + name="robot_ip", + default_value="192.168.56.2", + description="The IP address of the robot to connect to." + ) + + load_moveit_config = LoadMoveitConfig( + robot_name="ur5e_workcell_fake", + package_name="ur5e_cell_moveit_config") + + generate_move_group_ld = GenerateMoveitLaunch( + function=generate_move_group_launch + ) + + generate_moveit_rviz_ld = GenerateMoveitLaunch( + function=generate_moveit_rviz_launch, + condition=IfCondition(LaunchConfiguration("use_rviz")) + ) + + ur_control_launch = IncludeLaunchDescription( + launch_description_source=PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]) + ), + launch_arguments={ + "ur_type": "ur5e", + "use_fake_hardware": "false", + "initial_joint_controller": "joint_trajectory_controller", + "activate_joint_controller": "true", + "robot_ip": LaunchConfiguration("robot_ip"), + "description_package": "ur5e_cell_description", + "description_file": "workcell.urdf.xacro", + "launch_rviz": "false" + }.items() + ) + + ld = LaunchDescription( + [ + database_config, + debug_config, + rviz_config, + robot_ip_config, + load_moveit_config, + ur_control_launch, + generate_move_group_ld, + generate_moveit_rviz_ld, + ] + ) + return ld \ No newline at end of file diff --git a/ur5_cell_moveit_config/launch/rsp.launch.py b/ur5e_cell_moveit_config/launch/rsp.launch.py similarity index 62% rename from ur5_cell_moveit_config/launch/rsp.launch.py rename to ur5e_cell_moveit_config/launch/rsp.launch.py index 3f02f02..261a0f2 100644 --- a/ur5_cell_moveit_config/launch/rsp.launch.py +++ b/ur5e_cell_moveit_config/launch/rsp.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_rsp_launch(moveit_config) diff --git a/ur5_cell_moveit_config/launch/setup_assistant.launch.py b/ur5e_cell_moveit_config/launch/setup_assistant.launch.py similarity index 65% rename from ur5_cell_moveit_config/launch/setup_assistant.launch.py rename to ur5e_cell_moveit_config/launch/setup_assistant.launch.py index 725fa7c..4a275b8 100644 --- a/ur5_cell_moveit_config/launch/setup_assistant.launch.py +++ b/ur5e_cell_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/ur5_cell_moveit_config/launch/spawn_controllers.launch.py b/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py similarity index 65% rename from ur5_cell_moveit_config/launch/spawn_controllers.launch.py rename to ur5e_cell_moveit_config/launch/spawn_controllers.launch.py index efd088e..2b9db6c 100644 --- a/ur5_cell_moveit_config/launch/spawn_controllers.launch.py +++ b/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_spawn_controllers_launch(moveit_config) diff --git a/ur5_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py b/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py similarity index 67% rename from ur5_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py rename to ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py index 66b0e63..9334add 100644 --- a/ur5_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ b/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/ur5_cell_moveit_config/launch/warehouse_db.launch.py b/ur5e_cell_moveit_config/launch/warehouse_db.launch.py similarity index 64% rename from ur5_cell_moveit_config/launch/warehouse_db.launch.py rename to ur5e_cell_moveit_config/launch/warehouse_db.launch.py index 41ceaa2..e7aaf23 100644 --- a/ur5_cell_moveit_config/launch/warehouse_db.launch.py +++ b/ur5e_cell_moveit_config/launch/warehouse_db.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() return generate_warehouse_db_launch(moveit_config) diff --git a/ur5_cell_moveit_config/package.xml b/ur5e_cell_moveit_config/package.xml similarity index 82% rename from ur5_cell_moveit_config/package.xml rename to ur5e_cell_moveit_config/package.xml index d3746ea..4ffa06d 100644 --- a/ur5_cell_moveit_config/package.xml +++ b/ur5e_cell_moveit_config/package.xml @@ -1,12 +1,12 @@ - ur5_cell_moveit_config + ur5e_cell_moveit_config 0.3.0 - An automatically generated package with all the configuration and launch files for using the ur5_msa with the MoveIt Motion Planning Framework + An automatically generated package with all the configuration and launch files for using the ur5e_workcell with the MoveIt Motion Planning Framework - Christoph Hellmann Santos + cmh BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - Christoph Hellmann Santos + cmh ament_cmake @@ -34,14 +34,13 @@ moveit_configs_utils moveit_ros_move_group moveit_ros_visualization - moveit_ros_warehouse moveit_setup_assistant robot_state_publisher rviz2 rviz_common rviz_default_plugins tf2_ros - ur_description + ur5e_cell_description xacro diff --git a/ur5e_cell_bringup/CMakeLists.txt b/ur5e_cell_pick_n_place/CMakeLists.txt similarity index 51% rename from ur5e_cell_bringup/CMakeLists.txt rename to ur5e_cell_pick_n_place/CMakeLists.txt index ecd03e4..554945a 100644 --- a/ur5e_cell_bringup/CMakeLists.txt +++ b/ur5e_cell_pick_n_place/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(ur5e_cell_bringup) +project(ur5e_cell_pick_n_place) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -8,13 +8,31 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(ur5e_cell_description REQUIRED) -find_package(ur5_cell_moveit_config REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(ur_msgs REQUIRED) -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/) +add_executable(pick_n_place_node src/pick_n_place_node.cpp) +target_include_directories(pick_n_place_node PUBLIC + $ + $) +target_compile_features(pick_n_place_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + pick_n_place_node + "rclcpp" + "moveit_msgs" + "moveit" + "moveit_ros_planning" + "ur_msgs" +) +install(TARGETS pick_n_place_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/ur5e_cell_pick_n_place/LICENSE b/ur5e_cell_pick_n_place/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/ur5e_cell_pick_n_place/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/ur5e_cell_pick_n_place/config/moveitcpp.yaml b/ur5e_cell_pick_n_place/config/moveitcpp.yaml new file mode 100644 index 0000000..6bad3b6 --- /dev/null +++ b/ur5e_cell_pick_n_place/config/moveitcpp.yaml @@ -0,0 +1,19 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + #namespace: "moveit_cpp" # optional, default is ~ + pipeline_names: ["pilz_industrial_motion_planner"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "LIN" + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 0.5 \ No newline at end of file diff --git a/ur5e_cell_pick_n_place/launch/pick_n_place.launch.py b/ur5e_cell_pick_n_place/launch/pick_n_place.launch.py new file mode 100644 index 0000000..7daaaf4 --- /dev/null +++ b/ur5e_cell_pick_n_place/launch/pick_n_place.launch.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config") + .robot_description(file_path="config/ur5e_workcell.urdf.xacro") + .moveit_cpp( + file_path=get_package_share_directory("ur5e_cell_pick_n_place") + + "/config/moveitcpp.yaml" + ) + .to_moveit_configs() + ) + + # MoveItCpp demo executable + moveit_cpp_node = Node( + name="pick_n_place_node", + package="ur5e_cell_pick_n_place", + executable="pick_n_place_node", + output="screen", + parameters=[moveit_config.to_dict()], + ) + + return LaunchDescription( + [ + moveit_cpp_node + ] + ) \ No newline at end of file diff --git a/ur5e_cell_pick_n_place/launch/pick_n_place_driver.launch.py b/ur5e_cell_pick_n_place/launch/pick_n_place_driver.launch.py new file mode 100644 index 0000000..ad36cf4 --- /dev/null +++ b/ur5e_cell_pick_n_place/launch/pick_n_place_driver.launch.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + ur_control_launch = IncludeLaunchDescription( + launch_description_source=PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]) + ), + launch_arguments={ + "ur_type": "ur5e", + "use_fake_hardware": "true", + "initial_joint_controller": "joint_trajectory_controller", + "activate_joint_controller": "true", + "robot_ip": "xxx.yyy.zzz.vvv", + "description_package": "ur5e_cell_description", + "description_file": "workcell.urdf.xacro", + "launch_rviz": "true" + }.items() + ) + + + ld = LaunchDescription( + [ + ur_control_launch + ] + ) + + return ld \ No newline at end of file diff --git a/ur5e_cell_bringup/package.xml b/ur5e_cell_pick_n_place/package.xml similarity index 53% rename from ur5e_cell_bringup/package.xml rename to ur5e_cell_pick_n_place/package.xml index 2644ab3..8dd5615 100644 --- a/ur5e_cell_bringup/package.xml +++ b/ur5e_cell_pick_n_place/package.xml @@ -1,19 +1,22 @@ - ur5e_cell_bringup + ur5e_cell_pick_n_place 0.0.0 - TODO: Package description - ragesh - TODO: License declaration + Simple pick and place + xxx + Apache-2.0 ament_cmake rclcpp - ur5e_cell_description - ur5_cell_moveit_config + moveit_msgs + moveit + moveit_ros_planning + ur_msgs ament_lint_auto + ament_lint_common ament_cmake diff --git a/ur5e_cell_pick_n_place/src/pick_n_place_node.cpp b/ur5e_cell_pick_n_place/src/pick_n_place_node.cpp new file mode 100644 index 0000000..197669a --- /dev/null +++ b/ur5e_cell_pick_n_place/src/pick_n_place_node.cpp @@ -0,0 +1,262 @@ +#include +#include +#include +#include +#include +#include + +bool open_gripper(rclcpp::Client::SharedPtr set_io_client) +{ + ur_msgs::srv::SetIO::Request set_req1; + set_req1.fun = (float)ur_msgs::srv::SetIO::Request::FUN_SET_DIGITAL_OUT; + set_req1.pin = (float)ur_msgs::srv::SetIO::Request::PIN_DOUT0; + set_req1.state = (float)ur_msgs::srv::SetIO::Request::STATE_OFF; + RCLCPP_INFO(rclcpp::get_logger("set_io"), "%s", ur_msgs::srv::to_yaml(set_req1).c_str()); + auto fut = set_io_client->async_send_request(std::make_shared(set_req1)); + auto fut_res = fut.wait_for(std::chrono::seconds(1)); + if(fut_res == std::future_status::timeout) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO failed with timeout."); + return false; + } + + if(!fut.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO was not successful."); + return false; + } + + ur_msgs::srv::SetIO::Request reset_req2; + reset_req2.fun = (float)ur_msgs::srv::SetIO::Request::FUN_SET_DIGITAL_OUT; + reset_req2.pin = (float)ur_msgs::srv::SetIO::Request::PIN_DOUT1; + reset_req2.state = (float)ur_msgs::srv::SetIO::Request::STATE_ON; + + fut = set_io_client->async_send_request(std::make_shared(reset_req2)); + fut_res = fut.wait_for(std::chrono::seconds(1)); + if(fut_res == std::future_status::timeout) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO failed with timeout."); + return false; + } + + if(!fut.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO was not successful."); + return false; + } +} + +bool close_gripper(rclcpp::Client::SharedPtr set_io_client) +{ + ur_msgs::srv::SetIO::Request set_req2; + set_req2.fun = (float)ur_msgs::srv::SetIO::Request::FUN_SET_DIGITAL_OUT; + set_req2.pin = (float)ur_msgs::srv::SetIO::Request::PIN_DOUT1; + set_req2.state = (float)ur_msgs::srv::SetIO::Request::STATE_OFF; + + RCLCPP_INFO(rclcpp::get_logger("set_io"), "%s", ur_msgs::srv::to_yaml(set_req2).c_str()); + auto fut = set_io_client->async_send_request(std::make_shared(set_req2)); + auto fut_res = fut.wait_for(std::chrono::seconds(1)); + if(fut_res == std::future_status::timeout) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO failed with timeout."); + return false; + } + + if(!fut.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO was not successful."); + return false; + } + + ur_msgs::srv::SetIO::Request reset_req1; + reset_req1.fun = (float)ur_msgs::srv::SetIO::Request::FUN_SET_DIGITAL_OUT; + reset_req1.pin = (float)ur_msgs::srv::SetIO::Request::PIN_DOUT0; + reset_req1.state = (float)ur_msgs::srv::SetIO::Request::STATE_ON; + + fut = set_io_client->async_send_request(std::make_shared(reset_req1)); + fut_res = fut.wait_for(std::chrono::seconds(1)); + if(fut_res == std::future_status::timeout) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO failed with timeout."); + return false; + } + + if(!fut.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("set_io"), "Set IO was not successful."); + return false; + } +} + + + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options); + std::string client_name = "/io_and_status_controller/set_io"; + auto set_io_client = node->create_client(client_name); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + rclcpp::sleep_for(std::chrono::seconds(1)); + if(!set_io_client->wait_for_service(std::chrono::seconds(3))) + { + RCLCPP_INFO(node->get_logger(), "No set_io service"); + rclcpp::shutdown(); + return 0; + } + static const std::string PLANNING_GROUP = "arm"; + + auto moveit_cpp_ptr = std::make_shared(node); + //rclcpp::sleep_for(std::chrono::seconds(1)); + moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); + + auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + + // Set poses + tf2::Quaternion q; + q.setRPY(-3.14,0,1.57); + + geometry_msgs::msg::PoseStamped start_position; + start_position.header.frame_id = "world"; + start_position.pose.position.x = -0.17; + start_position.pose.position.y = 0.0; + start_position.pose.position.z = 1.5; + start_position.pose.orientation.x = q.getX(); + start_position.pose.orientation.y = q.getY(); + start_position.pose.orientation.z = q.getZ(); + start_position.pose.orientation.w = q.getW(); + + + geometry_msgs::msg::PoseStamped approach_object_1; + approach_object_1.header.frame_id = "world"; + approach_object_1.pose.position.x = -0.17; + approach_object_1.pose.position.y = 0.16; + approach_object_1.pose.position.z = 1.2; + approach_object_1.pose.orientation.x = q.getX(); + approach_object_1.pose.orientation.y = q.getY(); + approach_object_1.pose.orientation.z = q.getZ(); + approach_object_1.pose.orientation.w = q.getW(); + + geometry_msgs::msg::PoseStamped pick_object_1; + pick_object_1.header.frame_id = "world"; + pick_object_1.pose.position.x = -0.17; + pick_object_1.pose.position.y = 0.16; + pick_object_1.pose.position.z = 1.05; + pick_object_1.pose.orientation.x = q.getX(); + pick_object_1.pose.orientation.y = q.getY(); + pick_object_1.pose.orientation.z = q.getZ(); + pick_object_1.pose.orientation.w = q.getW(); + + geometry_msgs::msg::PoseStamped approach_object_2; + approach_object_2.header.frame_id = "world"; + approach_object_2.pose.position.x = -0.17; + approach_object_2.pose.position.y = -0.10; + approach_object_2.pose.position.z = 1.2; + approach_object_2.pose.orientation.x = q.getX(); + approach_object_2.pose.orientation.y = q.getY(); + approach_object_2.pose.orientation.z = q.getZ(); + approach_object_2.pose.orientation.w = q.getW(); + + geometry_msgs::msg::PoseStamped pick_object_2; + pick_object_2.header.frame_id = "world"; + pick_object_2.pose.position.x = -0.17; + pick_object_2.pose.position.y = -0.10; + pick_object_2.pose.position.z = 1.05; + pick_object_2.pose.orientation.x = q.getX(); + pick_object_2.pose.orientation.y = q.getY(); + pick_object_2.pose.orientation.z = q.getZ(); + pick_object_2.pose.orientation.w = q.getW(); + + planning_components->setStartStateToCurrentState(); + open_gripper(set_io_client); + + planning_components->setGoal(start_position, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_start_plan = planning_components->plan(); + if(!to_start_plan) + { + RCLCPP_INFO(node->get_logger(), "Planning to start failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_start_plan.trajectory); + + planning_components->setStartStateToCurrentState(); + planning_components->setGoal(approach_object_1, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_approach1 = planning_components->plan(); + if(!to_approach1) + { + RCLCPP_INFO(node->get_logger(), "Planning to approach 1 failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_approach1.trajectory); + open_gripper(set_io_client); + + planning_components->setStartStateToCurrentState(); + planning_components->setGoal(pick_object_1, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_pick1 = planning_components->plan(); + if(!to_pick1) + { + RCLCPP_INFO(node->get_logger(), "Planning to pick 1 failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_pick1.trajectory); + + close_gripper(set_io_client); + + planning_components->setStartStateToCurrentState(); + planning_components->setGoal(approach_object_1, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_approach1_1 = planning_components->plan(); + if(!to_approach1_1) + { + RCLCPP_INFO(node->get_logger(), "Planning to approach 1 failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_approach1_1.trajectory); + + planning_components->setStartStateToCurrentState(); + planning_components->setGoal(approach_object_2, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_approach2 = planning_components->plan(); + if(!to_approach2) + { + RCLCPP_INFO(node->get_logger(), "Planning to aproach 2 failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_approach2.trajectory); + + planning_components->setStartStateToCurrentState(); + planning_components->setGoal(pick_object_2, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_pick2 = planning_components->plan(); + if(!to_pick2) + { + RCLCPP_INFO(node->get_logger(), "Planning to pick 2 failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_pick2.trajectory); + + open_gripper(set_io_client); + + planning_components->setStartStateToCurrentState(); + planning_components->setGoal(start_position, "tool_tip"); + const moveit_cpp::PlanningComponent::PlanSolution to_final_pos = planning_components->plan(); + if(!to_final_pos) + { + RCLCPP_INFO(node->get_logger(), "Planning to final position failed."); + rclcpp::shutdown(); + return 0; + } + moveit_cpp_ptr->execute("arm", to_final_pos.trajectory); + + rclcpp::sleep_for(std::chrono::seconds(5)); + rclcpp::shutdown(); + return 0; +}