From 3c8dc20f3fc8f2db060cd0efb292a11a26ee45f7 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sun, 12 Feb 2023 20:52:21 +0000 Subject: [PATCH 01/23] Make workcell.urdf.xacro more nice Signed-off-by: Christoph Hellmann Santos --- .../urdf/workcell.urdf.xacro | 66 ++++++++++++------- 1 file changed, 43 insertions(+), 23 deletions(-) diff --git a/ur5e_cell_description/urdf/workcell.urdf.xacro b/ur5e_cell_description/urdf/workcell.urdf.xacro index f75c9c0..693319d 100644 --- a/ur5e_cell_description/urdf/workcell.urdf.xacro +++ b/ur5e_cell_description/urdf/workcell.urdf.xacro @@ -1,13 +1,14 @@ - - + - - - + + + + + @@ -39,11 +40,18 @@ - + + + + + + + - + + - - - - - - $(arg simulation_controllers) - - - + + + + + + + + + - + + - + + + - - + + - - \ No newline at end of file From 79018b1392c4e08f91605545f42eef687bbb55d3 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 2 Feb 2023 13:53:07 +0000 Subject: [PATCH 02/23] restructuring 1 Signed-off-by: Christoph Hellmann Santos --- ur5_cell_moveit_config/launch/demo.launch.py | 10 - ur5_cell_moveit_config/launch/robot.launch.py | 139 ------------- ur5e_cell_bringup/CMakeLists.txt | 31 --- .../launch/ur5e_cell_bringup.launch.py | 107 ---------- ur5e_cell_bringup/package.xml | 21 -- .../.setup_assistant | 4 +- .../CMakeLists.txt | 2 +- .../README.md | 0 .../config/initial_positions.yaml | 2 +- .../config/joint_limits.yaml | 0 .../config/kinematics.yaml | 0 .../config/moveit.rviz | 0 .../config/moveit_controllers.yaml | 0 .../config/ompl_planning.yaml | 0 .../config/pilz_cartesian_limits.yaml | 0 .../config/ros2_controllers.yaml | 0 .../config/ur5e_workcell.ros2_control.xacro | 2 +- .../config/ur5e_workcell.srdf | 0 .../config/ur5e_workcell.urdf.xacro | 2 +- .../config/ur5e_workcell_fake.urdf.xacro | 2 +- ur5e_cell_moveit_config/launch/demo.launch.py | 20 ++ .../launch/move_group.launch.py | 2 +- .../launch/moveit_rviz.launch.py | 2 +- .../launch/robot.launch.py | 183 ++++++++++++++++++ .../launch/ros2_control.launch.py | 0 .../launch/rsp.launch.py | 2 +- .../launch/setup_assistant.launch.py | 2 +- .../launch/spawn_controllers.launch.py | 2 +- .../launch/static_virtual_joint_tfs.launch.py | 2 +- .../launch/warehouse_db.launch.py | 2 +- .../package.xml | 4 +- 31 files changed, 219 insertions(+), 324 deletions(-) delete mode 100644 ur5_cell_moveit_config/launch/demo.launch.py delete mode 100644 ur5_cell_moveit_config/launch/robot.launch.py delete mode 100644 ur5e_cell_bringup/CMakeLists.txt delete mode 100644 ur5e_cell_bringup/launch/ur5e_cell_bringup.launch.py delete mode 100644 ur5e_cell_bringup/package.xml rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/.setup_assistant (83%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/CMakeLists.txt (90%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/README.md (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/initial_positions.yaml (66%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/joint_limits.yaml (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/kinematics.yaml (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/moveit.rviz (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/moveit_controllers.yaml (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/ompl_planning.yaml (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/pilz_cartesian_limits.yaml (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/config/ros2_controllers.yaml (100%) rename ur5_cell_moveit_config/config/ur5_msa.ros2_control.xacro => ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro (96%) rename ur5_cell_moveit_config/config/ur5_msa.srdf => ur5e_cell_moveit_config/config/ur5e_workcell.srdf (100%) rename ur5_cell_moveit_config/config/ur5_msa.urdf.xacro => ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro (95%) rename ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro => ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro (95%) create mode 100644 ur5e_cell_moveit_config/launch/demo.launch.py rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/move_group.launch.py (64%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/moveit_rviz.launch.py (64%) create mode 100644 ur5e_cell_moveit_config/launch/robot.launch.py rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/ros2_control.launch.py (100%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/rsp.launch.py (62%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/setup_assistant.launch.py (65%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/spawn_controllers.launch.py (65%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/static_virtual_joint_tfs.launch.py (67%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/launch/warehouse_db.launch.py (64%) rename {ur5_cell_moveit_config => ur5e_cell_moveit_config}/package.xml (94%) 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/ur5e_cell_bringup/CMakeLists.txt b/ur5e_cell_bringup/CMakeLists.txt deleted file mode 100644 index ecd03e4..0000000 --- a/ur5e_cell_bringup/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ur5e_cell_bringup) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(ur5e_cell_description REQUIRED) -find_package(ur5_cell_moveit_config REQUIRED) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/) - - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() 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_bringup/package.xml b/ur5e_cell_bringup/package.xml deleted file mode 100644 index 2644ab3..0000000 --- a/ur5e_cell_bringup/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ur5e_cell_bringup - 0.0.0 - TODO: Package description - ragesh - TODO: License declaration - - ament_cmake - - rclcpp - ur5e_cell_description - ur5_cell_moveit_config - - ament_lint_auto - - - ament_cmake - - diff --git a/ur5_cell_moveit_config/.setup_assistant b/ur5e_cell_moveit_config/.setup_assistant similarity index 83% rename from ur5_cell_moveit_config/.setup_assistant rename to ur5e_cell_moveit_config/.setup_assistant index bc1fa6d..07ef60f 100644 --- a/ur5_cell_moveit_config/.setup_assistant +++ b/ur5e_cell_moveit_config/.setup_assistant @@ -2,9 +2,9 @@ moveit_setup_assistant_config: urdf: package: ur_description relative_path: urdf/ur.urdf.xacro - xacro_args: name:="ur5_msa" ur_type:="ur5e" + xacro_args: name:="ur5e_workcell" ur_type:="ur5e" 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 diff --git a/ur5_cell_moveit_config/CMakeLists.txt b/ur5e_cell_moveit_config/CMakeLists.txt similarity index 90% rename from ur5_cell_moveit_config/CMakeLists.txt rename to ur5e_cell_moveit_config/CMakeLists.txt index 48cf0b3..aa6a063 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) +project(ur5e_cell_moveit_config) find_package(ament_cmake REQUIRED) diff --git a/ur5_cell_moveit_config/README.md b/ur5e_cell_moveit_config/README.md similarity index 100% rename from ur5_cell_moveit_config/README.md rename to ur5e_cell_moveit_config/README.md diff --git a/ur5_cell_moveit_config/config/initial_positions.yaml b/ur5e_cell_moveit_config/config/initial_positions.yaml similarity index 66% rename from ur5_cell_moveit_config/config/initial_positions.yaml rename to ur5e_cell_moveit_config/config/initial_positions.yaml index 0fd53b9..bc9131c 100644 --- a/ur5_cell_moveit_config/config/initial_positions.yaml +++ b/ur5e_cell_moveit_config/config/initial_positions.yaml @@ -1,4 +1,4 @@ -# Default initial positions for ur5_msa's ros2_control fake system +# Default initial positions for ur5e_workcell's ros2_control fake system initial_positions: elbow_joint: 0 diff --git a/ur5_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml similarity index 100% rename from ur5_cell_moveit_config/config/joint_limits.yaml rename to ur5e_cell_moveit_config/config/joint_limits.yaml 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 100% rename from ur5_cell_moveit_config/config/moveit_controllers.yaml rename to ur5e_cell_moveit_config/config/moveit_controllers.yaml diff --git a/ur5_cell_moveit_config/config/ompl_planning.yaml b/ur5e_cell_moveit_config/config/ompl_planning.yaml similarity index 100% rename from ur5_cell_moveit_config/config/ompl_planning.yaml rename to ur5e_cell_moveit_config/config/ompl_planning.yaml diff --git a/ur5_cell_moveit_config/config/pilz_cartesian_limits.yaml b/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml similarity index 100% rename from ur5_cell_moveit_config/config/pilz_cartesian_limits.yaml rename to ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml 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 100% rename from ur5_cell_moveit_config/config/ur5_msa.srdf rename to ur5e_cell_moveit_config/config/ur5e_workcell.srdf diff --git a/ur5_cell_moveit_config/config/ur5_msa.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro similarity index 95% rename from ur5_cell_moveit_config/config/ur5_msa.urdf.xacro rename to ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro index caaeb0f..3b668e5 100644 --- a/ur5_cell_moveit_config/config/ur5_msa.urdf.xacro +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro @@ -11,6 +11,6 @@ - + diff --git a/ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro similarity index 95% rename from ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro rename to ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro index 25bd991..2fd5669 100644 --- a/ur5_cell_moveit_config/config/ur5_msa_fake.urdf.xacro +++ b/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro @@ -11,6 +11,6 @@ - + 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..d3a8584 --- /dev/null +++ b/ur5e_cell_moveit_config/launch/demo.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from moveit_configs_utils.launches import generate_demo_launch +from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch + +def generate_launch_description(): + load_moveit_config = LoadMoveitConfig( + robot_name="ur5e_workcell_fake", + package_name="ur5e_cell_moveit_config") + + generate_demo_ld = GenerateMoveitLaunch( + function= generate_demo_launch) + + ld = LaunchDescription( + [ + load_moveit_config, + generate_demo_ld + ] + ) + + return ld 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..ec6abed 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="ur5_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..bc90dd1 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="ur5_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..f8e09eb --- /dev/null +++ b/ur5e_cell_moveit_config/launch/robot.launch.py @@ -0,0 +1,183 @@ +from os.path import join +import pprint +from launch import LaunchDescription +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import ( + generate_move_group_launch, + generate_moveit_rviz_launch, + generate_rsp_launch, + generate_spawn_controllers_launch, + generate_static_virtual_joint_tfs_launch, + generate_warehouse_db_launch +) +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +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 +from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch, LoadYaml +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +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_servo_config = LoadYaml( + package="ur5e_cell_moveit_config", + file="config/moveit_servo.yaml", + configuration_variable="moveit_servo" + ) + + load_moveit_config = LoadMoveitConfig( + robot_name="ur5e_workcell_fake", + package_name="ur5e_cell_moveit_config") + + generate_virtual_joint_ld = GenerateMoveitLaunch( + function=generate_static_virtual_joint_tfs_launch + ) + + generate_rsp_ld = GenerateMoveitLaunch( + function=generate_rsp_launch + ) + + 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")) + ) + + generate_warehouse_db_ld = GenerateMoveitLaunch( + function=generate_warehouse_db_launch, + condition=IfCondition(LaunchConfiguration("db")) + ) + + # UR hardware specific things + ur_ros2_control_node = Node( + package="ur_robot_driver", + executable="ur_ros2_control_node", + parameters=[ + {"robot_description": LaunchConfiguration("moveit_robot_description")}, + PathJoinSubstitution([LaunchConfiguration('moveit_package_path'), "config", "ros2_controllers.yaml"]) + ], + output="screen" + ) + + ur_dashboard_client = 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"}], + ) + + ur_controller_stopper_node = 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", + ] + }, + ], + ) + + # moveit_servo_node = Node( + # package="moveit_servo", + # executable="servo_node_main", + # parameters=[ + # {"moveit_servo": LaunchConfiguration("moveit_servo")}, + # {"robot_description": LaunchConfiguration("moveit_robot_description")}, + # {"robot_description_semantic": LaunchConfiguration("moveit_robot_description_semantic")}, + # ], + # output="screen", + # ) + + # container = ComposableNodeContainer( + # name="servo_joy_container", + # namespace="/", + # package="rclcpp_components", + # executable="component_container_mt", + # composable_node_descriptions=[ + # ComposableNode( + # package="moveit_servo", + # plugin="moveit_servo::JoyToServoPub", + # name="controller_to_servo_node", + # ), + # ComposableNode( + # package="joy", + # plugin="joy::Joy", + # name="joy_node", + # ), + # ], + # output="screen", + # ) + + generate_controllers_ld = GenerateMoveitLaunch( + function=generate_spawn_controllers_launch + ) + + ld = LaunchDescription( + [ + database_config, + debug_config, + rviz_config, + robot_ip_config, + load_moveit_config, + load_servo_config, + generate_virtual_joint_ld, + generate_rsp_ld, + generate_move_group_ld, + generate_moveit_rviz_ld, + generate_warehouse_db_ld, + ur_ros2_control_node, + ur_dashboard_client, + ur_controller_stopper_node, + #container, + #moveit_servo_node, + generate_controllers_ld + ] + ) + return ld \ No newline at end of file diff --git a/ur5_cell_moveit_config/launch/ros2_control.launch.py b/ur5e_cell_moveit_config/launch/ros2_control.launch.py similarity index 100% rename from ur5_cell_moveit_config/launch/ros2_control.launch.py rename to ur5e_cell_moveit_config/launch/ros2_control.launch.py 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..3505f49 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="ur5_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..ae7308b 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="ur5_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..1c74557 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="ur5_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..b2a5d67 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="ur5_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..63ccb3f 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="ur5_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 94% rename from ur5_cell_moveit_config/package.xml rename to ur5e_cell_moveit_config/package.xml index d3746ea..bd8baed 100644 --- a/ur5_cell_moveit_config/package.xml +++ b/ur5e_cell_moveit_config/package.xml @@ -1,10 +1,10 @@ - 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 From f12d5c905fa173d0faedbb28000cf0870e0a4d3a Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 3 Feb 2023 07:29:57 +0000 Subject: [PATCH 03/23] Add io controller to launch files. Signed-off-by: Christoph Hellmann Santos --- .gitignore | 1 + ur5e_cell_moveit_config/launch/demo.launch.py | 10 +++++++++- ur5e_cell_moveit_config/launch/robot.launch.py | 8 ++++++++ 3 files changed, 18 insertions(+), 1 deletion(-) 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/ur5e_cell_moveit_config/launch/demo.launch.py b/ur5e_cell_moveit_config/launch/demo.launch.py index d3a8584..84530cb 100644 --- a/ur5e_cell_moveit_config/launch/demo.launch.py +++ b/ur5e_cell_moveit_config/launch/demo.launch.py @@ -1,6 +1,7 @@ from launch import LaunchDescription from moveit_configs_utils.launches import generate_demo_launch from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch +from launch_ros.actions import Node def generate_launch_description(): load_moveit_config = LoadMoveitConfig( @@ -9,11 +10,18 @@ def generate_launch_description(): generate_demo_ld = GenerateMoveitLaunch( function= generate_demo_launch) + + io_and_status_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["io_and_status_controller", "-c", "/controller_manager"], + ) ld = LaunchDescription( [ load_moveit_config, - generate_demo_ld + generate_demo_ld, + io_and_status_controller_spawner ] ) diff --git a/ur5e_cell_moveit_config/launch/robot.launch.py b/ur5e_cell_moveit_config/launch/robot.launch.py index f8e09eb..720da0a 100644 --- a/ur5e_cell_moveit_config/launch/robot.launch.py +++ b/ur5e_cell_moveit_config/launch/robot.launch.py @@ -159,6 +159,13 @@ def generate_launch_description(): function=generate_spawn_controllers_launch ) + + io_and_status_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["io_and_status_controller", "-c", "/controller_manager"], + ) + ld = LaunchDescription( [ database_config, @@ -177,6 +184,7 @@ def generate_launch_description(): ur_controller_stopper_node, #container, #moveit_servo_node, + io_and_status_controller_spawner, generate_controllers_ld ] ) From 9b410735bca10814d904f0ffd99afd0b35e6bb19 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 8 Feb 2023 12:59:56 +0000 Subject: [PATCH 04/23] good limits values Signed-off-by: Christoph Hellmann Santos --- .../config/initial_positions.yaml | 12 +- .../config/ur5e/joint_limits.yaml | 2 +- .../config/initial_positions.yaml | 19 +- .../config/joint_limits.yaml | 12 +- ur5e_cell_moveit_config/launch/demo.launch.py | 38 +++- .../launch/robot.launch.py | 168 ++++++++++-------- 6 files changed, 148 insertions(+), 103 deletions(-) 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/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_moveit_config/config/initial_positions.yaml b/ur5e_cell_moveit_config/config/initial_positions.yaml index bc9131c..8019d8b 100644 --- a/ur5e_cell_moveit_config/config/initial_positions.yaml +++ b/ur5e_cell_moveit_config/config/initial_positions.yaml @@ -1,9 +1,16 @@ # Default initial positions for ur5e_workcell'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 + shoulder_pan_joint: -1.57 + shoulder_lift_joint: -1.57 + elbow_joint: -1.57 + wrist_1_joint: -1.57 + wrist_2_joint: 1.57 + wrist_3_joint: 0.0 + +shoulder_pan_joint: -1.57 +shoulder_lift_joint: -1.57 +elbow_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/ur5e_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml index b68248a..0990a9a 100644 --- a/ur5e_cell_moveit_config/config/joint_limits.yaml +++ b/ur5e_cell_moveit_config/config/joint_limits.yaml @@ -15,9 +15,9 @@ 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 + min_position: -2.0 max_acceleration: 3.49 shoulder_lift_joint: # acceleration limits are not publicly available @@ -26,9 +26,9 @@ 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 + min_position: -2.0 max_acceleration: 3.49 elbow_joint: # acceleration limits are not publicly available @@ -47,9 +47,9 @@ joint_limits: # # 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 + min_position: -3.14159265359 max_acceleration: 3.49 wrist_1_joint: # acceleration limits are not publicly available diff --git a/ur5e_cell_moveit_config/launch/demo.launch.py b/ur5e_cell_moveit_config/launch/demo.launch.py index 84530cb..e34aeea 100644 --- a/ur5e_cell_moveit_config/launch/demo.launch.py +++ b/ur5e_cell_moveit_config/launch/demo.launch.py @@ -1,27 +1,47 @@ from launch import LaunchDescription -from moveit_configs_utils.launches import generate_demo_launch +from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch from launch_ros.actions import Node +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(): load_moveit_config = LoadMoveitConfig( robot_name="ur5e_workcell_fake", package_name="ur5e_cell_moveit_config") - generate_demo_ld = GenerateMoveitLaunch( - function= generate_demo_launch) + move_group_launch = GenerateMoveitLaunch( + function= generate_move_group_launch) + + moveit_rviz_launch = GenerateMoveitLaunch( + function= generate_moveit_rviz_launch) - io_and_status_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["io_and_status_controller", "-c", "/controller_manager"], + 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( [ load_moveit_config, - generate_demo_ld, - io_and_status_controller_spawner + move_group_launch, + moveit_rviz_launch, + ur_control_launch ] ) diff --git a/ur5e_cell_moveit_config/launch/robot.launch.py b/ur5e_cell_moveit_config/launch/robot.launch.py index 720da0a..8dfd84c 100644 --- a/ur5e_cell_moveit_config/launch/robot.launch.py +++ b/ur5e_cell_moveit_config/launch/robot.launch.py @@ -11,7 +11,7 @@ generate_warehouse_db_launch ) from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import UnlessCondition, IfCondition from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg @@ -50,79 +50,96 @@ def generate_launch_description(): description="The IP address of the robot to connect to." ) - load_servo_config = LoadYaml( - package="ur5e_cell_moveit_config", - file="config/moveit_servo.yaml", - configuration_variable="moveit_servo" - ) + # load_servo_config = LoadYaml( + # package="ur5e_cell_moveit_config", + # file="config/moveit_servo.yaml", + # configuration_variable="moveit_servo" + # ) load_moveit_config = LoadMoveitConfig( robot_name="ur5e_workcell_fake", package_name="ur5e_cell_moveit_config") - generate_virtual_joint_ld = GenerateMoveitLaunch( - function=generate_static_virtual_joint_tfs_launch - ) + # generate_virtual_joint_ld = GenerateMoveitLaunch( + # function=generate_static_virtual_joint_tfs_launch + # ) - generate_rsp_ld = GenerateMoveitLaunch( - function=generate_rsp_launch - ) + # generate_rsp_ld = GenerateMoveitLaunch( + # function=generate_rsp_launch + # ) - generate_move_group_ld = GenerateMoveitLaunch( - function=generate_move_group_launch - ) + # 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")) - ) + # generate_moveit_rviz_ld = GenerateMoveitLaunch( + # function=generate_moveit_rviz_launch, + # condition=IfCondition(LaunchConfiguration("use_rviz")) + # ) - generate_warehouse_db_ld = GenerateMoveitLaunch( - function=generate_warehouse_db_launch, - condition=IfCondition(LaunchConfiguration("db")) + # generate_warehouse_db_ld = GenerateMoveitLaunch( + # function=generate_warehouse_db_launch, + # condition=IfCondition(LaunchConfiguration("db")) + # ) + + 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": "192.168.56.2", + "description_package": "ur5e_cell_description", + "description_file": "workcell.urdf.xacro", + "launch_rviz": "false" + }.items() ) + # UR hardware specific things - ur_ros2_control_node = Node( - package="ur_robot_driver", - executable="ur_ros2_control_node", - parameters=[ - {"robot_description": LaunchConfiguration("moveit_robot_description")}, - PathJoinSubstitution([LaunchConfiguration('moveit_package_path'), "config", "ros2_controllers.yaml"]) - ], - output="screen" - ) - - ur_dashboard_client = 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"}], - ) - - ur_controller_stopper_node = 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", - ] - }, - ], - ) + # ur_ros2_control_node = Node( + # package="ur_robot_driver", + # executable="ur_ros2_control_node", + # parameters=[ + # {"robot_description": LaunchConfiguration("moveit_robot_description")}, + # PathJoinSubstitution([LaunchConfiguration('moveit_package_path'), "config", "ros2_controllers.yaml"]) + # ], + # output="screen" + # ) + + # ur_dashboard_client = 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"}], + # ) + + # ur_controller_stopper_node = 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", + # ] + # }, + # ], + # ) # moveit_servo_node = Node( # package="moveit_servo", @@ -155,9 +172,9 @@ def generate_launch_description(): # output="screen", # ) - generate_controllers_ld = GenerateMoveitLaunch( - function=generate_spawn_controllers_launch - ) + # generate_controllers_ld = GenerateMoveitLaunch( + # function=generate_spawn_controllers_launch + # ) io_and_status_controller_spawner = Node( @@ -173,19 +190,20 @@ def generate_launch_description(): rviz_config, robot_ip_config, load_moveit_config, - load_servo_config, - generate_virtual_joint_ld, - generate_rsp_ld, - generate_move_group_ld, - generate_moveit_rviz_ld, - generate_warehouse_db_ld, - ur_ros2_control_node, - ur_dashboard_client, - ur_controller_stopper_node, + # load_servo_config, + # generate_virtual_joint_ld, + # generate_rsp_ld, + # generate_move_group_ld, + # generate_moveit_rviz_ld, + # generate_warehouse_db_ld, + ur_control_launch, + #ur_ros2_control_node, + #ur_dashboard_client, + #ur_controller_stopper_node, #container, #moveit_servo_node, io_and_status_controller_spawner, - generate_controllers_ld + # generate_controllers_ld ] ) return ld \ No newline at end of file From 70046abe95049d60ffb7cc2ebc1169c3a971043f Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 8 Feb 2023 13:35:40 +0000 Subject: [PATCH 05/23] small updates Signed-off-by: Christoph Hellmann Santos --- .../config/joint_limits.yaml | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/ur5e_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml index 0990a9a..eefe1fc 100644 --- a/ur5e_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 @@ -28,7 +19,7 @@ joint_limits: max_effort: 150.0 max_position: 0.0 max_velocity: 1.57079632679 - min_position: -2.0 + min_position: -3.14159265359 max_acceleration: 3.49 elbow_joint: # acceleration limits are not publicly available @@ -37,16 +28,6 @@ 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: 0.0 max_velocity: 1.57079632679 min_position: -3.14159265359 From 36f8a0773ffcc30d83952d06ace871a32a761693 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 8 Feb 2023 16:31:18 +0100 Subject: [PATCH 06/23] add ur calibration Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_description/config/ur5e/default_kinematics.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From d1d062c3397a5023c3e2c3ea86e0f77c26ad2619 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 8 Feb 2023 16:31:38 +0100 Subject: [PATCH 07/23] Adjust robot.launch.py Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_moveit_config/launch/robot.launch.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ur5e_cell_moveit_config/launch/robot.launch.py b/ur5e_cell_moveit_config/launch/robot.launch.py index 8dfd84c..c98683e 100644 --- a/ur5e_cell_moveit_config/launch/robot.launch.py +++ b/ur5e_cell_moveit_config/launch/robot.launch.py @@ -68,14 +68,14 @@ def generate_launch_description(): # function=generate_rsp_launch # ) - # generate_move_group_ld = GenerateMoveitLaunch( - # function=generate_move_group_launch - # ) + 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")) - # ) + generate_moveit_rviz_ld = GenerateMoveitLaunch( + function=generate_moveit_rviz_launch, + condition=IfCondition(LaunchConfiguration("use_rviz")) + ) # generate_warehouse_db_ld = GenerateMoveitLaunch( # function=generate_warehouse_db_launch, @@ -193,8 +193,6 @@ def generate_launch_description(): # load_servo_config, # generate_virtual_joint_ld, # generate_rsp_ld, - # generate_move_group_ld, - # generate_moveit_rviz_ld, # generate_warehouse_db_ld, ur_control_launch, #ur_ros2_control_node, @@ -202,6 +200,8 @@ def generate_launch_description(): #ur_controller_stopper_node, #container, #moveit_servo_node, + generate_move_group_ld, + generate_moveit_rviz_ld, io_and_status_controller_spawner, # generate_controllers_ld ] From 589d9f2fd8018f5790b4646274823a87a68e7d43 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 8 Feb 2023 16:32:04 +0100 Subject: [PATCH 08/23] remove ros2_controllers from moveit_config Signed-off-by: Christoph Hellmann Santos --- .../config/ur5e_workcell.ros2_control.xacro | 56 ------------------- 1 file changed, 56 deletions(-) delete mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro deleted file mode 100644 index 03add06..0000000 --- a/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - mock_components/GenericSystem - - - - - ${initial_positions['shoulder_pan_joint']} - - - - - - - ${initial_positions['shoulder_lift_joint']} - - - - - - - ${initial_positions['elbow_joint']} - - - - - - - ${initial_positions['wrist_1_joint']} - - - - - - - ${initial_positions['wrist_2_joint']} - - - - - - - ${initial_positions['wrist_3_joint']} - - - - - - - From b57df0b3b994bab31955bec7cb7f9719912b1c6b Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 8 Feb 2023 16:32:27 +0100 Subject: [PATCH 09/23] Add point to point movement example with move_group_interface Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_point_to_point/CMakeLists.txt | 49 +++++ ur5e_cell_point_to_point/LICENSE | 202 ++++++++++++++++++ .../launch/point_to_point.launch.py | 23 ++ ur5e_cell_point_to_point/package.xml | 23 ++ .../src/point_to_point_node.cpp | 126 +++++++++++ 5 files changed, 423 insertions(+) create mode 100644 ur5e_cell_point_to_point/CMakeLists.txt create mode 100644 ur5e_cell_point_to_point/LICENSE create mode 100644 ur5e_cell_point_to_point/launch/point_to_point.launch.py create mode 100644 ur5e_cell_point_to_point/package.xml create mode 100644 ur5e_cell_point_to_point/src/point_to_point_node.cpp diff --git a/ur5e_cell_point_to_point/CMakeLists.txt b/ur5e_cell_point_to_point/CMakeLists.txt new file mode 100644 index 0000000..89f24b4 --- /dev/null +++ b/ur5e_cell_point_to_point/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.8) +project(ur5e_cell_point_to_point) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) + +add_executable(point_to_point_node src/point_to_point_node.cpp) +target_include_directories(point_to_point_node PUBLIC + $ + $) +target_compile_features(point_to_point_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + point_to_point_node + "Boost" + "rclcpp" + "moveit_msgs" + "moveit_core" + "moveit_ros_planning_interface" +) + +install(TARGETS point_to_point_node + DESTINATION lib/${PROJECT_NAME}) + + install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ur5e_cell_point_to_point/LICENSE b/ur5e_cell_point_to_point/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/ur5e_cell_point_to_point/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_point_to_point/launch/point_to_point.launch.py b/ur5e_cell_point_to_point/launch/point_to_point.launch.py new file mode 100644 index 0000000..1c6dae8 --- /dev/null +++ b/ur5e_cell_point_to_point/launch/point_to_point.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder(robot_name="ur5e_workcell_fake", package_name="ur5e_cell_moveit_config").to_moveit_configs() + + # MoveGroupInterface demo executable + point_to_point_demo = Node( + name="p2p_node", + package="ur5e_cell_point_to_point", + executable="point_to_point_node", + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + ], + ) + + return LaunchDescription([point_to_point_demo]) \ No newline at end of file diff --git a/ur5e_cell_point_to_point/package.xml b/ur5e_cell_point_to_point/package.xml new file mode 100644 index 0000000..530688d --- /dev/null +++ b/ur5e_cell_point_to_point/package.xml @@ -0,0 +1,23 @@ + + + + ur5e_cell_point_to_point + 0.0.0 + Use C++ Moveit2 to go to some points. + Christoph + Apache-2.0 + + ament_cmake + + Boost + moveit_msgs + moveit_core + moveit_ros_planning_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ur5e_cell_point_to_point/src/point_to_point_node.cpp b/ur5e_cell_point_to_point/src/point_to_point_node.cpp new file mode 100644 index 0000000..5f9627a --- /dev/null +++ b/ur5e_cell_point_to_point/src/point_to_point_node.cpp @@ -0,0 +1,126 @@ +#include +#include +#include +#include +#include + + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("point_to_point_planner"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("point_to_point_planner", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + + + + static const std::string PLANNING_GROUP = "arm"; + moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + + const moveit::core::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + geometry_msgs::msg::PoseStamped cpose = move_group.getCurrentPose(); + RCLCPP_INFO(LOGGER, geometry_msgs::msg::to_yaml(cpose).c_str()); + + + tf2::Quaternion myQuaternion; + myQuaternion.setRPY(-3.14,0,1.57); + geometry_msgs::msg::Pose target_pose1; + geometry_msgs::msg::Pose target_pose2; + geometry_msgs::msg::Pose target_pose3; + geometry_msgs::msg::Pose target_pose4; + + std::vector waypoints; + + // Aproach area 1 + target_pose1.orientation.x = myQuaternion.getX(); + target_pose1.orientation.y = myQuaternion.getY(); + target_pose1.orientation.z = myQuaternion.getZ(); + target_pose1.orientation.w = myQuaternion.getW(); + + //target_pose1.position.x = -0.171; + target_pose1.position.x = -0.17; + target_pose1.position.y = 0.16; + target_pose1.position.z = 1.4; + + + // Pick area 1 + target_pose2.orientation.x = myQuaternion.getX(); + target_pose2.orientation.y = myQuaternion.getY(); + target_pose2.orientation.z = myQuaternion.getZ(); + target_pose2.orientation.w = myQuaternion.getW(); + + //target_pose1.position.x = -0.171; + target_pose2.position.x = -0.17; + target_pose2.position.y = 0.16; + target_pose2.position.z = 1.25; + + + // Aproach area 2 + target_pose3.orientation.x = myQuaternion.getX(); + target_pose3.orientation.y = myQuaternion.getY(); + target_pose3.orientation.z = myQuaternion.getZ(); + target_pose3.orientation.w = myQuaternion.getW(); + + //target_pose1.position.x = -0.171; + target_pose3.position.x = -0.17; + target_pose3.position.y = -0.10; + target_pose3.position.z = 1.4; + + + + // Pick area 2 + target_pose4.orientation.x = myQuaternion.getX(); + target_pose4.orientation.y = myQuaternion.getY(); + target_pose4.orientation.z = myQuaternion.getZ(); + target_pose4.orientation.w = myQuaternion.getW(); + + //target_pose1.position.x = -0.171; + target_pose4.position.x = -0.17; + target_pose4.position.y = -0.10; + target_pose4.position.z = 1.25; + + waypoints.push_back(target_pose1); + waypoints.push_back(target_pose2); + waypoints.push_back(target_pose1); + waypoints.push_back(target_pose3); + waypoints.push_back(target_pose4); + waypoints.push_back(target_pose3); + + + for (int i = 0; i < waypoints.size(); i++) + { + move_group.setPoseTarget(waypoints[i]); + move_group.setMaxVelocityScalingFactor(1.3); + move_group.setPlanningPipelineId("pilz_industrial_motion_planner"); + move_group.setPlannerId("LIN"); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + + bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); + if(!success) break; + + success = (move_group.execute(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Executing plan 1(pose goal) %s", success ? "" : "FAILED"); + if(!success) break; + + //sleep(1.0); + + } + + + + + + rclcpp::shutdown(); + return 0; +} From 5a307d7db064d45a62acc9cb33fc3a0e9339ccd0 Mon Sep 17 00:00:00 2001 From: Ruichao Wu Date: Tue, 7 Feb 2023 11:28:48 +0100 Subject: [PATCH 10/23] add states and end_effector --- .../config/ur5e_workcell.srdf | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf index 31a1923..7c32660 100644 --- a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf @@ -18,6 +18,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + From 73e91281be9e02497ec3ee75e837ae814261eca2 Mon Sep 17 00:00:00 2001 From: Ruichao Wu Date: Wed, 8 Feb 2023 17:49:58 +0100 Subject: [PATCH 11/23] add camera --- ur5e_cell_description/package.xml | 1 + ur5e_cell_description/urdf/workcell.urdf.xacro | 3 +-- ur5e_cell_moveit_config/config/ur5e_workcell.srdf | 8 ++++++++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/ur5e_cell_description/package.xml b/ur5e_cell_description/package.xml index b02a53c..caa2d68 100644 --- a/ur5e_cell_description/package.xml +++ b/ur5e_cell_description/package.xml @@ -14,6 +14,7 @@ ur_description ur_moveit_config ur_robot_driver + realsense2_description ros2launch ament_lint_auto diff --git a/ur5e_cell_description/urdf/workcell.urdf.xacro b/ur5e_cell_description/urdf/workcell.urdf.xacro index 693319d..7005478 100644 --- a/ur5e_cell_description/urdf/workcell.urdf.xacro +++ b/ur5e_cell_description/urdf/workcell.urdf.xacro @@ -44,7 +44,6 @@ - @@ -123,4 +122,4 @@ - \ No newline at end of file + diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf index 7c32660..3802d19 100644 --- a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf @@ -50,6 +50,14 @@ + + + + + + + + From c44f0e5ebcd7343d7f0be273bb0541dd84895c18 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 9 Feb 2023 06:53:57 +0100 Subject: [PATCH 12/23] Update to ur5e Signed-off-by: Christoph Hellmann Santos --- docker/docker-compose.yaml | 2 +- ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro | 2 +- ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro | 2 +- ur5e_cell_moveit_config/launch/move_group.launch.py | 2 +- ur5e_cell_moveit_config/launch/moveit_rviz.launch.py | 2 +- ur5e_cell_moveit_config/launch/rsp.launch.py | 2 +- ur5e_cell_moveit_config/launch/setup_assistant.launch.py | 2 +- ur5e_cell_moveit_config/launch/spawn_controllers.launch.py | 2 +- .../launch/static_virtual_joint_tfs.launch.py | 2 +- ur5e_cell_moveit_config/launch/warehouse_db.launch.py | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index c9a9b88..36de3eb 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -18,4 +18,4 @@ services: - 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 + command: ros2 launch ur5e_cell_moveit_config robot.launch.py \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro index 3b668e5..e4d80d1 100644 --- a/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro index 2fd5669..f974235 100644 --- a/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro +++ b/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/ur5e_cell_moveit_config/launch/move_group.launch.py b/ur5e_cell_moveit_config/launch/move_group.launch.py index ec6abed..380b2b9 100644 --- a/ur5e_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("ur5e_workcell", 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/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py b/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py index bc90dd1..8d6ed1b 100644 --- a/ur5e_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("ur5e_workcell", 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/rsp.launch.py b/ur5e_cell_moveit_config/launch/rsp.launch.py index 3505f49..261a0f2 100644 --- a/ur5e_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("ur5e_workcell", 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/ur5e_cell_moveit_config/launch/setup_assistant.launch.py b/ur5e_cell_moveit_config/launch/setup_assistant.launch.py index ae7308b..4a275b8 100644 --- a/ur5e_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("ur5e_workcell", 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/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py b/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py index 1c74557..2b9db6c 100644 --- a/ur5e_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("ur5e_workcell", 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/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py b/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py index b2a5d67..9334add 100644 --- a/ur5e_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("ur5e_workcell", 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/ur5e_cell_moveit_config/launch/warehouse_db.launch.py b/ur5e_cell_moveit_config/launch/warehouse_db.launch.py index 63ccb3f..e7aaf23 100644 --- a/ur5e_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("ur5e_workcell", 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) From b6fe899165d4c5cdc9290dac94ce0bf4a5531066 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 10 Feb 2023 13:21:09 +0100 Subject: [PATCH 13/23] intialise start of training. Signed-off-by: Christoph Hellmann Santos --- .github/workflows/build.yaml | 65 ------ .github/workflows/docker_build_pipeline.yaml | 70 ------ dependencies_rolling.repos | 15 -- docker/Dockerfile | 30 --- docker/docker-compose.yaml | 21 -- docker/entrypoint.sh | 7 - ur5e_cell_moveit_config/.setup_assistant | 26 --- ur5e_cell_moveit_config/CMakeLists.txt | 11 - ur5e_cell_moveit_config/README.md | 5 - .../config/initial_positions.yaml | 16 -- .../config/joint_limits.yaml | 67 ------ .../config/kinematics.yaml | 4 - ur5e_cell_moveit_config/config/moveit.rviz | 51 ----- .../config/moveit_controllers.yaml | 35 --- .../config/ompl_planning.yaml | 79 ------- .../config/pilz_cartesian_limits.yaml | 5 - .../config/ros2_controllers.yaml | 123 ----------- .../config/ur5e_workcell.srdf | 112 ---------- .../config/ur5e_workcell.urdf.xacro | 16 -- .../config/ur5e_workcell_fake.urdf.xacro | 16 -- ur5e_cell_moveit_config/launch/demo.launch.py | 48 ---- .../launch/move_group.launch.py | 7 - .../launch/moveit_rviz.launch.py | 7 - .../launch/robot.launch.py | 209 ------------------ .../launch/ros2_control.launch.py | 0 ur5e_cell_moveit_config/launch/rsp.launch.py | 7 - .../launch/setup_assistant.launch.py | 7 - .../launch/spawn_controllers.launch.py | 7 - .../launch/static_virtual_joint_tfs.launch.py | 7 - .../launch/warehouse_db.launch.py | 7 - ur5e_cell_moveit_config/package.xml | 51 ----- ur5e_cell_point_to_point/CMakeLists.txt | 49 ---- ur5e_cell_point_to_point/LICENSE | 202 ----------------- .../launch/point_to_point.launch.py | 23 -- ur5e_cell_point_to_point/package.xml | 23 -- .../src/point_to_point_node.cpp | 126 ----------- 36 files changed, 1554 deletions(-) delete mode 100644 .github/workflows/build.yaml delete mode 100644 .github/workflows/docker_build_pipeline.yaml delete mode 100644 dependencies_rolling.repos delete mode 100644 docker/Dockerfile delete mode 100644 docker/docker-compose.yaml delete mode 100644 docker/entrypoint.sh delete mode 100644 ur5e_cell_moveit_config/.setup_assistant delete mode 100644 ur5e_cell_moveit_config/CMakeLists.txt delete mode 100644 ur5e_cell_moveit_config/README.md delete mode 100644 ur5e_cell_moveit_config/config/initial_positions.yaml delete mode 100644 ur5e_cell_moveit_config/config/joint_limits.yaml delete mode 100644 ur5e_cell_moveit_config/config/kinematics.yaml delete mode 100644 ur5e_cell_moveit_config/config/moveit.rviz delete mode 100644 ur5e_cell_moveit_config/config/moveit_controllers.yaml delete mode 100644 ur5e_cell_moveit_config/config/ompl_planning.yaml delete mode 100644 ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml delete mode 100644 ur5e_cell_moveit_config/config/ros2_controllers.yaml delete mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell.srdf delete mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro delete mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro delete mode 100644 ur5e_cell_moveit_config/launch/demo.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/move_group.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/moveit_rviz.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/robot.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/ros2_control.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/rsp.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/setup_assistant.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/spawn_controllers.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py delete mode 100644 ur5e_cell_moveit_config/launch/warehouse_db.launch.py delete mode 100644 ur5e_cell_moveit_config/package.xml delete mode 100644 ur5e_cell_point_to_point/CMakeLists.txt delete mode 100644 ur5e_cell_point_to_point/LICENSE delete mode 100644 ur5e_cell_point_to_point/launch/point_to_point.launch.py delete mode 100644 ur5e_cell_point_to_point/package.xml delete mode 100644 ur5e_cell_point_to_point/src/point_to_point_node.cpp 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/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 36de3eb..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 ur5e_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/ur5e_cell_moveit_config/.setup_assistant b/ur5e_cell_moveit_config/.setup_assistant deleted file mode 100644 index 07ef60f..0000000 --- a/ur5e_cell_moveit_config/.setup_assistant +++ /dev/null @@ -1,26 +0,0 @@ -moveit_setup_assistant_config: - urdf: - package: ur_description - relative_path: urdf/ur.urdf.xacro - xacro_args: name:="ur5e_workcell" ur_type:="ur5e" - srdf: - relative_path: config/ur5e_workcell.srdf - package_settings: - author_name: Christoph Hellmann Santos - author_email: ipa@ipa.fraunhofer.de - generated_timestamp: 1666960052 - control_xacro: - command: - - position - state: - - position - - velocity - modified_urdf: - xacros: - - control_xacro - control_xacro: - command: - - position - state: - - position - - velocity \ No newline at end of file diff --git a/ur5e_cell_moveit_config/CMakeLists.txt b/ur5e_cell_moveit_config/CMakeLists.txt deleted file mode 100644 index aa6a063..0000000 --- a/ur5e_cell_moveit_config/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(ur5e_cell_moveit_config) - -find_package(ament_cmake REQUIRED) - -ament_package() - -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} - PATTERN "setup_assistant.launch" EXCLUDE) -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) -install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/ur5e_cell_moveit_config/README.md b/ur5e_cell_moveit_config/README.md deleted file mode 100644 index b694378..0000000 --- a/ur5e_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/ur5e_cell_moveit_config/config/initial_positions.yaml b/ur5e_cell_moveit_config/config/initial_positions.yaml deleted file mode 100644 index 8019d8b..0000000 --- a/ur5e_cell_moveit_config/config/initial_positions.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# Default initial positions for ur5e_workcell's ros2_control fake system - -initial_positions: - shoulder_pan_joint: -1.57 - shoulder_lift_joint: -1.57 - elbow_joint: -1.57 - wrist_1_joint: -1.57 - wrist_2_joint: 1.57 - wrist_3_joint: 0.0 - -shoulder_pan_joint: -1.57 -shoulder_lift_joint: -1.57 -elbow_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/ur5e_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml deleted file mode 100644 index eefe1fc..0000000 --- a/ur5e_cell_moveit_config/config/joint_limits.yaml +++ /dev/null @@ -1,67 +0,0 @@ -joint_limits: - shoulder_pan_joint: - # acceleration limits are not publicly available - has_acceleration_limits: true - has_effort_limits: true - has_position_limits: true - has_velocity_limits: true - max_effort: 150.0 - max_position: -0.35 - max_velocity: 1.57079632679 - min_position: -2.0 - max_acceleration: 3.49 - shoulder_lift_joint: - # acceleration limits are not publicly available - has_acceleration_limits: true - has_effort_limits: true - has_position_limits: true - has_velocity_limits: true - max_effort: 150.0 - max_position: 0.0 - max_velocity: 1.57079632679 - min_position: -3.14159265359 - max_acceleration: 3.49 - elbow_joint: - # acceleration limits are not publicly available - has_acceleration_limits: true - has_effort_limits: true - has_position_limits: true - has_velocity_limits: true - max_effort: 150.0 - max_position: 0.0 - max_velocity: 1.57079632679 - min_position: -3.14159265359 - max_acceleration: 3.49 - wrist_1_joint: - # acceleration limits are not publicly available - has_acceleration_limits: true - has_effort_limits: true - has_position_limits: true - has_velocity_limits: true - max_effort: 28.0 - max_position: 3.14159265359 - max_velocity: 1.57079632679 - min_position: -3.14159265359 - max_acceleration: 3.49 - wrist_2_joint: - # acceleration limits are not publicly available - has_acceleration_limits: true - has_effort_limits: true - has_position_limits: true - has_velocity_limits: true - max_effort: 28.0 - max_position: 3.14159265359 - max_velocity: 1.57079632679 - min_position: -3.14159265359 - max_acceleration: 3.49 - wrist_3_joint: - # acceleration limits are not publicly available - has_effort_limits: true - has_position_limits: true - has_velocity_limits: true - max_effort: 28.0 - max_position: 3.14159265359 - max_velocity: 1.57079632679 - min_position: -3.14159265359 - has_acceleration_limits: true - max_acceleration: 3.49 diff --git a/ur5e_cell_moveit_config/config/kinematics.yaml b/ur5e_cell_moveit_config/config/kinematics.yaml deleted file mode 100644 index 2f6bb96..0000000 --- a/ur5e_cell_moveit_config/config/kinematics.yaml +++ /dev/null @@ -1,4 +0,0 @@ -arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.0050000000000000001 - kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/moveit.rviz b/ur5e_cell_moveit_config/config/moveit.rviz deleted file mode 100644 index f31651e..0000000 --- a/ur5e_cell_moveit_config/config/moveit.rviz +++ /dev/null @@ -1,51 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - - Class: rviz_common/Help - Name: Help - - Class: rviz_common/Views - Name: Views -Visualization Manager: - Displays: - - Class: rviz_default_plugins/Grid - Name: Grid - Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Name: MotionPlanning - Planned Path: - Loop Animation: true - State Display Time: 0.05 s - Trajectory Topic: display_planned_path - Planning Scene Topic: monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Scene Robot: - Robot Alpha: 0.5 - Value: true - Global Options: - Fixed Frame: world - Tools: - - Class: rviz_default_plugins/Interact - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.0 - Focal Point: - X: -0.1 - Y: 0.25 - Z: 0.30 - Name: Current View - Pitch: 0.5 - Target Frame: world - Yaw: -0.623 -Window Geometry: - Height: 975 - QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Width: 1200 diff --git a/ur5e_cell_moveit_config/config/moveit_controllers.yaml b/ur5e_cell_moveit_config/config/moveit_controllers.yaml deleted file mode 100644 index 2e4b7ff..0000000 --- a/ur5e_cell_moveit_config/config/moveit_controllers.yaml +++ /dev/null @@ -1,35 +0,0 @@ -# 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 - default: true - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/ompl_planning.yaml b/ur5e_cell_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index 75f8fff..0000000 --- a/ur5e_cell_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,79 +0,0 @@ -planning_plugin: ompl_interface/OMPLPlanner -request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/ResolveConstraintFrames - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints -start_state_max_bounds_error: 0.1 -planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: - type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: - type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar -arm: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - 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) - longest_valid_segment_fraction: 0.01 diff --git a/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml b/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml deleted file mode 100644 index f0dd367..0000000 --- a/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml +++ /dev/null @@ -1,5 +0,0 @@ -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 diff --git a/ur5e_cell_moveit_config/config/ros2_controllers.yaml b/ur5e_cell_moveit_config/config/ros2_controllers.yaml deleted file mode 100644 index 0123733..0000000 --- a/ur5e_cell_moveit_config/config/ros2_controllers.yaml +++ /dev/null @@ -1,123 +0,0 @@ -controller_manager: - ros__parameters: - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - io_and_status_controller: - type: ur_controllers/GPIOController - - speed_scaling_state_broadcaster: - type: ur_controllers/SpeedScalingStateBroadcaster - - force_torque_sensor_broadcaster: - type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster - - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - scaled_joint_trajectory_controller: - type: ur_controllers/ScaledJointTrajectoryController - - forward_velocity_controller: - type: velocity_controllers/JointGroupVelocityController - - forward_position_controller: - type: position_controllers/JointGroupPositionController - - update_rate: 500 # Hz - - -speed_scaling_state_broadcaster: - ros__parameters: - state_publish_rate: 100.0 - - -force_torque_sensor_broadcaster: - ros__parameters: - sensor_name: tcp_fts_sensor - state_interface_names: - - force.x - - force.y - - force.z - - torque.x - - torque.y - - torque.z - frame_id: tool0 - topic_name: ft_data - - -joint_trajectory_controller: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - - -scaled_joint_trajectory_controller: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - -forward_velocity_controller: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - interface_name: velocity - -forward_position_controller: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf deleted file mode 100644 index 3802d19..0000000 --- a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro deleted file mode 100644 index e4d80d1..0000000 --- a/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro deleted file mode 100644 index f974235..0000000 --- a/ur5e_cell_moveit_config/config/ur5e_workcell_fake.urdf.xacro +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/ur5e_cell_moveit_config/launch/demo.launch.py b/ur5e_cell_moveit_config/launch/demo.launch.py deleted file mode 100644 index e34aeea..0000000 --- a/ur5e_cell_moveit_config/launch/demo.launch.py +++ /dev/null @@ -1,48 +0,0 @@ -from launch import LaunchDescription -from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch -from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch -from launch_ros.actions import Node -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(): - load_moveit_config = LoadMoveitConfig( - robot_name="ur5e_workcell_fake", - package_name="ur5e_cell_moveit_config") - - move_group_launch = GenerateMoveitLaunch( - function= generate_move_group_launch) - - moveit_rviz_launch = GenerateMoveitLaunch( - function= generate_moveit_rviz_launch) - - 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( - [ - load_moveit_config, - move_group_launch, - moveit_rviz_launch, - ur_control_launch - ] - ) - - return ld diff --git a/ur5e_cell_moveit_config/launch/move_group.launch.py b/ur5e_cell_moveit_config/launch/move_group.launch.py deleted file mode 100644 index 380b2b9..0000000 --- a/ur5e_cell_moveit_config/launch/move_group.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_move_group_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() - return generate_move_group_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py b/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py deleted file mode 100644 index 8d6ed1b..0000000 --- a/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_moveit_rviz_launch - - -def generate_launch_description(): - 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 deleted file mode 100644 index c98683e..0000000 --- a/ur5e_cell_moveit_config/launch/robot.launch.py +++ /dev/null @@ -1,209 +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_move_group_launch, - generate_moveit_rviz_launch, - generate_rsp_launch, - generate_spawn_controllers_launch, - generate_static_virtual_joint_tfs_launch, - generate_warehouse_db_launch -) -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument, 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 -from launch_ros_extras.actions import LoadMoveitConfig, GenerateMoveitLaunch, LoadYaml -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -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_servo_config = LoadYaml( - # package="ur5e_cell_moveit_config", - # file="config/moveit_servo.yaml", - # configuration_variable="moveit_servo" - # ) - - load_moveit_config = LoadMoveitConfig( - robot_name="ur5e_workcell_fake", - package_name="ur5e_cell_moveit_config") - - # generate_virtual_joint_ld = GenerateMoveitLaunch( - # function=generate_static_virtual_joint_tfs_launch - # ) - - # generate_rsp_ld = GenerateMoveitLaunch( - # function=generate_rsp_launch - # ) - - 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")) - ) - - # generate_warehouse_db_ld = GenerateMoveitLaunch( - # function=generate_warehouse_db_launch, - # condition=IfCondition(LaunchConfiguration("db")) - # ) - - 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": "192.168.56.2", - "description_package": "ur5e_cell_description", - "description_file": "workcell.urdf.xacro", - "launch_rviz": "false" - }.items() - ) - - - # UR hardware specific things - # ur_ros2_control_node = Node( - # package="ur_robot_driver", - # executable="ur_ros2_control_node", - # parameters=[ - # {"robot_description": LaunchConfiguration("moveit_robot_description")}, - # PathJoinSubstitution([LaunchConfiguration('moveit_package_path'), "config", "ros2_controllers.yaml"]) - # ], - # output="screen" - # ) - - # ur_dashboard_client = 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"}], - # ) - - # ur_controller_stopper_node = 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", - # ] - # }, - # ], - # ) - - # moveit_servo_node = Node( - # package="moveit_servo", - # executable="servo_node_main", - # parameters=[ - # {"moveit_servo": LaunchConfiguration("moveit_servo")}, - # {"robot_description": LaunchConfiguration("moveit_robot_description")}, - # {"robot_description_semantic": LaunchConfiguration("moveit_robot_description_semantic")}, - # ], - # output="screen", - # ) - - # container = ComposableNodeContainer( - # name="servo_joy_container", - # namespace="/", - # package="rclcpp_components", - # executable="component_container_mt", - # composable_node_descriptions=[ - # ComposableNode( - # package="moveit_servo", - # plugin="moveit_servo::JoyToServoPub", - # name="controller_to_servo_node", - # ), - # ComposableNode( - # package="joy", - # plugin="joy::Joy", - # name="joy_node", - # ), - # ], - # output="screen", - # ) - - # generate_controllers_ld = GenerateMoveitLaunch( - # function=generate_spawn_controllers_launch - # ) - - - io_and_status_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["io_and_status_controller", "-c", "/controller_manager"], - ) - - ld = LaunchDescription( - [ - database_config, - debug_config, - rviz_config, - robot_ip_config, - load_moveit_config, - # load_servo_config, - # generate_virtual_joint_ld, - # generate_rsp_ld, - # generate_warehouse_db_ld, - ur_control_launch, - #ur_ros2_control_node, - #ur_dashboard_client, - #ur_controller_stopper_node, - #container, - #moveit_servo_node, - generate_move_group_ld, - generate_moveit_rviz_ld, - io_and_status_controller_spawner, - # generate_controllers_ld - ] - ) - return ld \ No newline at end of file diff --git a/ur5e_cell_moveit_config/launch/ros2_control.launch.py b/ur5e_cell_moveit_config/launch/ros2_control.launch.py deleted file mode 100644 index e69de29..0000000 diff --git a/ur5e_cell_moveit_config/launch/rsp.launch.py b/ur5e_cell_moveit_config/launch/rsp.launch.py deleted file mode 100644 index 261a0f2..0000000 --- a/ur5e_cell_moveit_config/launch/rsp.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_rsp_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() - return generate_rsp_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/setup_assistant.launch.py b/ur5e_cell_moveit_config/launch/setup_assistant.launch.py deleted file mode 100644 index 4a275b8..0000000 --- a/ur5e_cell_moveit_config/launch/setup_assistant.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_setup_assistant_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() - return generate_setup_assistant_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py b/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py deleted file mode 100644 index 2b9db6c..0000000 --- a/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_spawn_controllers_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() - return generate_spawn_controllers_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py b/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py deleted file mode 100644 index 9334add..0000000 --- a/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch - - -def generate_launch_description(): - 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/ur5e_cell_moveit_config/launch/warehouse_db.launch.py b/ur5e_cell_moveit_config/launch/warehouse_db.launch.py deleted file mode 100644 index e7aaf23..0000000 --- a/ur5e_cell_moveit_config/launch/warehouse_db.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_warehouse_db_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() - return generate_warehouse_db_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/package.xml b/ur5e_cell_moveit_config/package.xml deleted file mode 100644 index bd8baed..0000000 --- a/ur5e_cell_moveit_config/package.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - ur5e_cell_moveit_config - 0.3.0 - - 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 - - BSD - - http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - Christoph Hellmann Santos - - ament_cmake - - moveit_ros_move_group - moveit_kinematics - moveit_planners - moveit_simple_controller_manager - joint_state_publisher - joint_state_publisher_gui - tf2_ros - xacro - - - - controller_manager - 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 - xacro - - - - ament_cmake - - diff --git a/ur5e_cell_point_to_point/CMakeLists.txt b/ur5e_cell_point_to_point/CMakeLists.txt deleted file mode 100644 index 89f24b4..0000000 --- a/ur5e_cell_point_to_point/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ur5e_cell_point_to_point) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(Boost REQUIRED) -find_package(rclcpp REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(moveit_core REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) - -add_executable(point_to_point_node src/point_to_point_node.cpp) -target_include_directories(point_to_point_node PUBLIC - $ - $) -target_compile_features(point_to_point_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - point_to_point_node - "Boost" - "rclcpp" - "moveit_msgs" - "moveit_core" - "moveit_ros_planning_interface" -) - -install(TARGETS point_to_point_node - DESTINATION lib/${PROJECT_NAME}) - - install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/ur5e_cell_point_to_point/LICENSE b/ur5e_cell_point_to_point/LICENSE deleted file mode 100644 index d645695..0000000 --- a/ur5e_cell_point_to_point/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - 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_point_to_point/launch/point_to_point.launch.py b/ur5e_cell_point_to_point/launch/point_to_point.launch.py deleted file mode 100644 index 1c6dae8..0000000 --- a/ur5e_cell_point_to_point/launch/point_to_point.launch.py +++ /dev/null @@ -1,23 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder(robot_name="ur5e_workcell_fake", package_name="ur5e_cell_moveit_config").to_moveit_configs() - - # MoveGroupInterface demo executable - point_to_point_demo = Node( - name="p2p_node", - package="ur5e_cell_point_to_point", - executable="point_to_point_node", - output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.planning_pipelines, - ], - ) - - return LaunchDescription([point_to_point_demo]) \ No newline at end of file diff --git a/ur5e_cell_point_to_point/package.xml b/ur5e_cell_point_to_point/package.xml deleted file mode 100644 index 530688d..0000000 --- a/ur5e_cell_point_to_point/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - ur5e_cell_point_to_point - 0.0.0 - Use C++ Moveit2 to go to some points. - Christoph - Apache-2.0 - - ament_cmake - - Boost - moveit_msgs - moveit_core - moveit_ros_planning_interface - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ur5e_cell_point_to_point/src/point_to_point_node.cpp b/ur5e_cell_point_to_point/src/point_to_point_node.cpp deleted file mode 100644 index 5f9627a..0000000 --- a/ur5e_cell_point_to_point/src/point_to_point_node.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#include -#include -#include -#include -#include - - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("point_to_point_planner"); - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto move_group_node = rclcpp::Node::make_shared("point_to_point_planner", node_options); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(move_group_node); - std::thread([&executor]() { executor.spin(); }).detach(); - - - - - static const std::string PLANNING_GROUP = "arm"; - moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - - const moveit::core::JointModelGroup* joint_model_group = - move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); - geometry_msgs::msg::PoseStamped cpose = move_group.getCurrentPose(); - RCLCPP_INFO(LOGGER, geometry_msgs::msg::to_yaml(cpose).c_str()); - - - tf2::Quaternion myQuaternion; - myQuaternion.setRPY(-3.14,0,1.57); - geometry_msgs::msg::Pose target_pose1; - geometry_msgs::msg::Pose target_pose2; - geometry_msgs::msg::Pose target_pose3; - geometry_msgs::msg::Pose target_pose4; - - std::vector waypoints; - - // Aproach area 1 - target_pose1.orientation.x = myQuaternion.getX(); - target_pose1.orientation.y = myQuaternion.getY(); - target_pose1.orientation.z = myQuaternion.getZ(); - target_pose1.orientation.w = myQuaternion.getW(); - - //target_pose1.position.x = -0.171; - target_pose1.position.x = -0.17; - target_pose1.position.y = 0.16; - target_pose1.position.z = 1.4; - - - // Pick area 1 - target_pose2.orientation.x = myQuaternion.getX(); - target_pose2.orientation.y = myQuaternion.getY(); - target_pose2.orientation.z = myQuaternion.getZ(); - target_pose2.orientation.w = myQuaternion.getW(); - - //target_pose1.position.x = -0.171; - target_pose2.position.x = -0.17; - target_pose2.position.y = 0.16; - target_pose2.position.z = 1.25; - - - // Aproach area 2 - target_pose3.orientation.x = myQuaternion.getX(); - target_pose3.orientation.y = myQuaternion.getY(); - target_pose3.orientation.z = myQuaternion.getZ(); - target_pose3.orientation.w = myQuaternion.getW(); - - //target_pose1.position.x = -0.171; - target_pose3.position.x = -0.17; - target_pose3.position.y = -0.10; - target_pose3.position.z = 1.4; - - - - // Pick area 2 - target_pose4.orientation.x = myQuaternion.getX(); - target_pose4.orientation.y = myQuaternion.getY(); - target_pose4.orientation.z = myQuaternion.getZ(); - target_pose4.orientation.w = myQuaternion.getW(); - - //target_pose1.position.x = -0.171; - target_pose4.position.x = -0.17; - target_pose4.position.y = -0.10; - target_pose4.position.z = 1.25; - - waypoints.push_back(target_pose1); - waypoints.push_back(target_pose2); - waypoints.push_back(target_pose1); - waypoints.push_back(target_pose3); - waypoints.push_back(target_pose4); - waypoints.push_back(target_pose3); - - - for (int i = 0; i < waypoints.size(); i++) - { - move_group.setPoseTarget(waypoints[i]); - move_group.setMaxVelocityScalingFactor(1.3); - move_group.setPlanningPipelineId("pilz_industrial_motion_planner"); - move_group.setPlannerId("LIN"); - - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - - bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); - if(!success) break; - - success = (move_group.execute(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Executing plan 1(pose goal) %s", success ? "" : "FAILED"); - if(!success) break; - - //sleep(1.0); - - } - - - - - - rclcpp::shutdown(); - return 0; -} From d80d1225fdb66ae1060726721451d993acf0d8b1 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 10 Feb 2023 12:44:35 +0000 Subject: [PATCH 14/23] add a few dependencies to make life easier after intial install Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_description/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ur5e_cell_description/package.xml b/ur5e_cell_description/package.xml index caa2d68..4d797b6 100644 --- a/ur5e_cell_description/package.xml +++ b/ur5e_cell_description/package.xml @@ -15,6 +15,10 @@ ur_moveit_config ur_robot_driver realsense2_description + moveit + moveit_ros + moveit_setup_assistant + ros2launch ament_lint_auto From 0d3c3f12490da1535003e9690f0d1f1302c8795a Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sat, 11 Feb 2023 19:09:25 +0000 Subject: [PATCH 15/23] All changes from setup assistant tutorial Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_moveit_config/.setup_assistant | 25 ++++++ ur5e_cell_moveit_config/CMakeLists.txt | 11 +++ .../config/initial_positions.yaml | 16 ++++ .../config/joint_limits.yaml | 40 +++++++++ .../config/kinematics.yaml | 4 + ur5e_cell_moveit_config/config/moveit.rviz | 51 +++++++++++ .../config/moveit_controllers.yaml | 21 +++++ .../config/ompl_planning.yaml | 79 +++++++++++++++++ .../config/pilz_cartesian_limits.yaml | 6 ++ .../config/ros2_controllers.yaml | 26 ++++++ .../config/ur5e_workcell.ros2_control.xacro | 56 ++++++++++++ .../config/ur5e_workcell.srdf | 86 +++++++++++++++++++ .../config/ur5e_workcell.urdf.xacro | 16 ++++ ur5e_cell_moveit_config/launch/demo.launch.py | 7 ++ .../launch/move_group.launch.py | 7 ++ .../launch/moveit_rviz.launch.py | 7 ++ ur5e_cell_moveit_config/launch/rsp.launch.py | 7 ++ .../launch/setup_assistant.launch.py | 7 ++ .../launch/spawn_controllers.launch.py | 7 ++ .../launch/static_virtual_joint_tfs.launch.py | 7 ++ .../launch/warehouse_db.launch.py | 7 ++ ur5e_cell_moveit_config/package.xml | 52 +++++++++++ 22 files changed, 545 insertions(+) create mode 100644 ur5e_cell_moveit_config/.setup_assistant create mode 100644 ur5e_cell_moveit_config/CMakeLists.txt create mode 100644 ur5e_cell_moveit_config/config/initial_positions.yaml create mode 100644 ur5e_cell_moveit_config/config/joint_limits.yaml create mode 100644 ur5e_cell_moveit_config/config/kinematics.yaml create mode 100644 ur5e_cell_moveit_config/config/moveit.rviz create mode 100644 ur5e_cell_moveit_config/config/moveit_controllers.yaml create mode 100644 ur5e_cell_moveit_config/config/ompl_planning.yaml create mode 100644 ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml create mode 100644 ur5e_cell_moveit_config/config/ros2_controllers.yaml create mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro create mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell.srdf create mode 100644 ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro create mode 100644 ur5e_cell_moveit_config/launch/demo.launch.py create mode 100644 ur5e_cell_moveit_config/launch/move_group.launch.py create mode 100644 ur5e_cell_moveit_config/launch/moveit_rviz.launch.py create mode 100644 ur5e_cell_moveit_config/launch/rsp.launch.py create mode 100644 ur5e_cell_moveit_config/launch/setup_assistant.launch.py create mode 100644 ur5e_cell_moveit_config/launch/spawn_controllers.launch.py create mode 100644 ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py create mode 100644 ur5e_cell_moveit_config/launch/warehouse_db.launch.py create mode 100644 ur5e_cell_moveit_config/package.xml diff --git a/ur5e_cell_moveit_config/.setup_assistant b/ur5e_cell_moveit_config/.setup_assistant new file mode 100644 index 0000000..f363873 --- /dev/null +++ b/ur5e_cell_moveit_config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: ur5e_cell_description + relative_path: urdf/workcell.urdf.xacro + srdf: + relative_path: config/ur5e_workcell.srdf + package_settings: + author_name: cmh + author_email: cmh@ipa.fraunhofer.de + generated_timestamp: 1676108726 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/ur5e_cell_moveit_config/CMakeLists.txt b/ur5e_cell_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..e492a4c --- /dev/null +++ b/ur5e_cell_moveit_config/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.22) +project(ur5e_cell_moveit_config) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 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/ur5e_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..130053c --- /dev/null +++ b/ur5e_cell_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 \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/kinematics.yaml b/ur5e_cell_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..2f6bb96 --- /dev/null +++ b/ur5e_cell_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/moveit.rviz b/ur5e_cell_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..f31651e --- /dev/null +++ b/ur5e_cell_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/ur5e_cell_moveit_config/config/moveit_controllers.yaml b/ur5e_cell_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 0000000..57ecda8 --- /dev/null +++ b/ur5e_cell_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,21 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + 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 \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/ompl_planning.yaml b/ur5e_cell_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..4cc79a3 --- /dev/null +++ b/ur5e_cell_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,79 @@ +planning_plugin: ompl_interface/OMPLPlanner +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints +start_state_max_bounds_error: 0.1 +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar +arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - 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) + longest_valid_segment_fraction: 0.01 diff --git a/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml b/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/ur5e_cell_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +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 diff --git a/ur5e_cell_moveit_config/config/ros2_controllers.yaml b/ur5e_cell_moveit_config/config/ros2_controllers.yaml new file mode 100644 index 0000000..a4a2cf3 --- /dev/null +++ b/ur5e_cell_moveit_config/config/ros2_controllers.yaml @@ -0,0 +1,26 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity \ No newline at end of file diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro new file mode 100644 index 0000000..03add06 --- /dev/null +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.ros2_control.xacro @@ -0,0 +1,56 @@ + + + + + + + + + mock_components/GenericSystem + + + + + ${initial_positions['shoulder_pan_joint']} + + + + + + + ${initial_positions['shoulder_lift_joint']} + + + + + + + ${initial_positions['elbow_joint']} + + + + + + + ${initial_positions['wrist_1_joint']} + + + + + + + ${initial_positions['wrist_2_joint']} + + + + + + + ${initial_positions['wrist_3_joint']} + + + + + + + diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.srdf b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf new file mode 100644 index 0000000..a11ae58 --- /dev/null +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.srdf @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro new file mode 100644 index 0000000..c77c9b7 --- /dev/null +++ b/ur5e_cell_moveit_config/config/ur5e_workcell.urdf.xacro @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ 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/ur5e_cell_moveit_config/launch/move_group.launch.py b/ur5e_cell_moveit_config/launch/move_group.launch.py new file mode 100644 index 0000000..380b2b9 --- /dev/null +++ b/ur5e_cell_moveit_config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py b/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..8d6ed1b --- /dev/null +++ b/ur5e_cell_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + 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/rsp.launch.py b/ur5e_cell_moveit_config/launch/rsp.launch.py new file mode 100644 index 0000000..261a0f2 --- /dev/null +++ b/ur5e_cell_moveit_config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/setup_assistant.launch.py b/ur5e_cell_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..4a275b8 --- /dev/null +++ b/ur5e_cell_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py b/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..2b9db6c --- /dev/null +++ b/ur5e_cell_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py b/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..9334add --- /dev/null +++ b/ur5e_cell_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + 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/ur5e_cell_moveit_config/launch/warehouse_db.launch.py b/ur5e_cell_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 0000000..e7aaf23 --- /dev/null +++ b/ur5e_cell_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("ur5e_workcell", package_name="ur5e_cell_moveit_config").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/ur5e_cell_moveit_config/package.xml b/ur5e_cell_moveit_config/package.xml new file mode 100644 index 0000000..bbad03c --- /dev/null +++ b/ur5e_cell_moveit_config/package.xml @@ -0,0 +1,52 @@ + + + + ur5e_cell_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the ur5e_workcell with the MoveIt Motion Planning Framework + + cmh + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + cmh + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + 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 + ur5e_cell_description + warehouse_ros_mongo + xacro + + + + ament_cmake + + From dafeda9fdb5c15dbce425d759e5dd6b65302c0d0 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sun, 12 Feb 2023 20:39:15 +0000 Subject: [PATCH 16/23] add some copies * joint_limits * moveit_controllers * ros2_controllers Signed-off-by: Christoph Hellmann Santos --- .../config/joint_limits.yaml | 85 +++++++++----- .../config/moveit_controllers.yaml | 1 + .../config/ros2_controllers.yaml | 107 +++++++++++++++++- 3 files changed, 159 insertions(+), 34 deletions(-) diff --git a/ur5e_cell_moveit_config/config/joint_limits.yaml b/ur5e_cell_moveit_config/config/joint_limits.yaml index 130053c..5ebd579 100644 --- a/ur5e_cell_moveit_config/config/joint_limits.yaml +++ b/ur5e_cell_moveit_config/config/joint_limits.yaml @@ -1,40 +1,67 @@ -# 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: + shoulder_pan_joint: + # acceleration limits are not publicly available + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 + max_effort: 150.0 + max_position: -0.35 + max_velocity: 1.57079632679 + min_position: -2.0 + max_acceleration: 5.0 shoulder_lift_joint: + # acceleration limits are not publicly available + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 - shoulder_pan_joint: + max_effort: 150.0 + max_position: 0.0 + max_velocity: 1.57079632679 + min_position: -3.14159265359 + max_acceleration: 5.0 + elbow_joint: + # acceleration limits are not publicly available + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 + max_effort: 150.0 + max_position: 0.0 + max_velocity: 1.57079632679 + min_position: -3.14159265359 + max_acceleration: 5.0 wrist_1_joint: + # acceleration limits are not publicly available + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 + max_effort: 28.0 + max_position: 3.14159265359 + max_velocity: 1.57079632679 + min_position: -3.14159265359 + max_acceleration: 5.0 wrist_2_joint: + # acceleration limits are not publicly available + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 + max_effort: 28.0 + max_position: 3.14159265359 + max_velocity: 1.57079632679 + min_position: -3.14159265359 + max_acceleration: 5.0 wrist_3_joint: + # acceleration limits are not publicly available + has_effort_limits: true + has_position_limits: true has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_effort: 28.0 + max_position: 3.14159265359 + max_velocity: 1.57079632679 + min_position: -3.14159265359 + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/ur5e_cell_moveit_config/config/moveit_controllers.yaml b/ur5e_cell_moveit_config/config/moveit_controllers.yaml index 57ecda8..6eb6ada 100644 --- a/ur5e_cell_moveit_config/config/moveit_controllers.yaml +++ b/ur5e_cell_moveit_config/config/moveit_controllers.yaml @@ -1,5 +1,6 @@ # MoveIt uses this configuration for controller management + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager moveit_simple_controller_manager: diff --git a/ur5e_cell_moveit_config/config/ros2_controllers.yaml b/ur5e_cell_moveit_config/config/ros2_controllers.yaml index a4a2cf3..0123733 100644 --- a/ur5e_cell_moveit_config/config/ros2_controllers.yaml +++ b/ur5e_cell_moveit_config/config/ros2_controllers.yaml @@ -1,14 +1,50 @@ -# This config file is used by ros2_control controller_manager: ros__parameters: - update_rate: 100 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + io_and_status_controller: + type: ur_controllers/GPIOController + + speed_scaling_state_broadcaster: + type: ur_controllers/SpeedScalingStateBroadcaster + + force_torque_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + scaled_joint_trajectory_controller: + type: ur_controllers/ScaledJointTrajectoryController + + forward_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + + update_rate: 500 # Hz + + +speed_scaling_state_broadcaster: + ros__parameters: + state_publish_rate: 100.0 + + +force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: tool0 + topic_name: ft_data - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: ros__parameters: @@ -23,4 +59,65 @@ joint_trajectory_controller: - position state_interfaces: - position - - velocity \ No newline at end of file + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + +scaled_joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + +forward_velocity_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + interface_name: velocity + +forward_position_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint From 0aa2d9a2cfa281c62a3a89d0aacafd9c6c015d22 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sat, 11 Feb 2023 19:53:17 +0000 Subject: [PATCH 17/23] Add robot.launch.py Signed-off-by: Christoph Hellmann Santos --- .../launch/robot.launch.py | 86 +++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 ur5e_cell_moveit_config/launch/robot.launch.py 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 From 40d1e22c2c8029588438f734cb67d0540d289712 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sun, 12 Feb 2023 20:40:46 +0000 Subject: [PATCH 18/23] Add pick n place packge Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_pick_n_place/CMakeLists.txt | 49 +++++++ ur5e_cell_pick_n_place/LICENSE | 202 ++++++++++++++++++++++++++ ur5e_cell_pick_n_place/package.xml | 24 +++ 3 files changed, 275 insertions(+) create mode 100644 ur5e_cell_pick_n_place/CMakeLists.txt create mode 100644 ur5e_cell_pick_n_place/LICENSE create mode 100644 ur5e_cell_pick_n_place/package.xml diff --git a/ur5e_cell_pick_n_place/CMakeLists.txt b/ur5e_cell_pick_n_place/CMakeLists.txt new file mode 100644 index 0000000..554945a --- /dev/null +++ b/ur5e_cell_pick_n_place/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.8) +project(ur5e_cell_pick_n_place) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(ur_msgs REQUIRED) + +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) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() 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/package.xml b/ur5e_cell_pick_n_place/package.xml new file mode 100644 index 0000000..8dd5615 --- /dev/null +++ b/ur5e_cell_pick_n_place/package.xml @@ -0,0 +1,24 @@ + + + + ur5e_cell_pick_n_place + 0.0.0 + Simple pick and place + xxx + Apache-2.0 + + ament_cmake + + rclcpp + moveit_msgs + moveit + moveit_ros_planning + ur_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 8bd2dc41cb0293bcfae5f41a6258968928f62019 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sun, 12 Feb 2023 20:40:59 +0000 Subject: [PATCH 19/23] Add config files Signed-off-by: Christoph Hellmann Santos --- ur5e_cell_pick_n_place/config/moveitcpp.yaml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 ur5e_cell_pick_n_place/config/moveitcpp.yaml 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 From bb1461fc96346198418a4784bb8122faa73b9ec7 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sun, 12 Feb 2023 20:43:21 +0000 Subject: [PATCH 20/23] Add launch files Signed-off-by: Christoph Hellmann Santos --- .../launch/pick_n_place.launch.py | 30 +++++++++++++++++ .../launch/pick_n_place_driver.launch.py | 33 +++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 ur5e_cell_pick_n_place/launch/pick_n_place.launch.py create mode 100644 ur5e_cell_pick_n_place/launch/pick_n_place_driver.launch.py 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 From 9e4cb19b80dbcd7b34bf19e666ebb67bf166a9e3 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sun, 12 Feb 2023 20:44:01 +0000 Subject: [PATCH 21/23] Write pick_n_place_node Signed-off-by: Christoph Hellmann Santos --- .../src/pick_n_place_node.cpp | 262 ++++++++++++++++++ 1 file changed, 262 insertions(+) create mode 100644 ur5e_cell_pick_n_place/src/pick_n_place_node.cpp 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; +} From 342a32c40eebd329df0ddeebcc2fa84bfaa54005 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Tue, 14 Feb 2023 20:52:06 +0000 Subject: [PATCH 22/23] Clean readme Signed-off-by: Christoph Hellmann Santos --- Readme.md | 53 +---------------------------------------------------- 1 file changed, 1 insertion(+), 52 deletions(-) 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 From 9dbbb2084dfc19301908d8c5f66019b1d5ec7202 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 15 Feb 2023 12:18:07 +0100 Subject: [PATCH 23/23] Update package.xml --- ur5e_cell_moveit_config/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/ur5e_cell_moveit_config/package.xml b/ur5e_cell_moveit_config/package.xml index bbad03c..4ffa06d 100644 --- a/ur5e_cell_moveit_config/package.xml +++ b/ur5e_cell_moveit_config/package.xml @@ -34,7 +34,6 @@ moveit_configs_utils moveit_ros_move_group moveit_ros_visualization - moveit_ros_warehouse moveit_setup_assistant robot_state_publisher rviz2 @@ -42,7 +41,6 @@ rviz_default_plugins tf2_ros ur5e_cell_description - warehouse_ros_mongo xacro