diff --git a/doc/examples/motion_planning_python_api/CMakeLists.txt b/doc/examples/motion_planning_python_api/CMakeLists.txt index c36feb47a9..0158fe90cd 100644 --- a/doc/examples/motion_planning_python_api/CMakeLists.txt +++ b/doc/examples/motion_planning_python_api/CMakeLists.txt @@ -1,5 +1,6 @@ install(PROGRAMS scripts/motion_planning_python_api_tutorial.py + scripts/motion_planning_python_api_tutorial_ur.py scripts/motion_planning_python_api_planning_scene.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/doc/examples/motion_planning_python_api/config/pilz_cartesian_limits.yaml b/doc/examples/motion_planning_python_api/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000000..37e42f6783 --- /dev/null +++ b/doc/examples/motion_planning_python_api/config/pilz_cartesian_limits.yaml @@ -0,0 +1,7 @@ +# Source: https://github.com/ros-planning/moveit_resources/blob/3d6a737f95fb7e5f3cfdc306d12b0bb6cbdadfb2/panda_moveit_config/config/pilz_cartesian_limits.yaml +# Cartesian 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/doc/examples/motion_planning_python_api/config/ur_kinematics.yaml b/doc/examples/motion_planning_python_api/config/ur_kinematics.yaml new file mode 100644 index 0000000000..63ed93d70d --- /dev/null +++ b/doc/examples/motion_planning_python_api/config/ur_kinematics.yaml @@ -0,0 +1,5 @@ + ur_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/doc/examples/motion_planning_python_api/docs/motion_planning_python_tutorial_UR.gif b/doc/examples/motion_planning_python_api/docs/motion_planning_python_tutorial_UR.gif new file mode 100644 index 0000000000..aa04a5f6b6 Binary files /dev/null and b/doc/examples/motion_planning_python_api/docs/motion_planning_python_tutorial_UR.gif differ diff --git a/doc/examples/motion_planning_python_api/launch/motion_planning_python_api_tutorial_ur.launch.py b/doc/examples/motion_planning_python_api/launch/motion_planning_python_api_tutorial_ur.launch.py new file mode 100644 index 0000000000..356d29b8a4 --- /dev/null +++ b/doc/examples/motion_planning_python_api/launch/motion_planning_python_api_tutorial_ur.launch.py @@ -0,0 +1,441 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Denis Stogl + +import os + +from pathlib import Path +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ur_moveit_config.launch_common import load_yaml + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + + +def launch_setup(context, *args, **kwargs): + # Initialize Arguments + ur_type = LaunchConfiguration("ur_type") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + safety_limits = LaunchConfiguration("safety_limits") + safety_pos_margin = LaunchConfiguration("safety_pos_margin") + safety_k_position = LaunchConfiguration("safety_k_position") + # General arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file") + moveit_config_file = LaunchConfiguration("moveit_config_file") + warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path") + prefix = LaunchConfiguration("prefix") + use_sim_time = LaunchConfiguration("use_sim_time") + launch_rviz = LaunchConfiguration("launch_rviz") + launch_servo = LaunchConfiguration("launch_servo") + + joint_limit_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]), + " ", + "robot_ip:=xxx.yyy.zzz.www", + " ", + "joint_limit_params:=", + joint_limit_params, + " ", + "kinematics_params:=", + kinematics_params, + " ", + "physical_params:=", + physical_params, + " ", + "visual_params:=", + visual_params, + " ", + "safety_limits:=", + safety_limits, + " ", + "safety_pos_margin:=", + safety_pos_margin, + " ", + "safety_k_position:=", + safety_k_position, + " ", + "name:=", + "ur", + " ", + "ur_type:=", + ur_type, + " ", + "script_filename:=ros_control.urscript", + " ", + "input_recipe_filename:=rtde_input_recipe.txt", + " ", + "output_recipe_filename:=rtde_output_recipe.txt", + " ", + "prefix:=", + prefix, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "srdf", moveit_config_file] + ), + " ", + "name:=", + # Also ur_type parameter could be used but then the planning group names in yaml + # configs has to be updated! + "ur", + " ", + "prefix:=", + prefix, + " ", + ] + ) + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + + robot_description_kinematics = PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] + ) + + robot_description_planning = { + "robot_description_planning": load_yaml( + str(moveit_config_package.perform(context)), + os.path.join("config", str(moveit_joint_limits_file.perform(context))), + ) + } + + # Planning Configuration + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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, + } + } + ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Trajectory Execution Configuration + controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml") + # the scaled_joint_trajectory_controller does not work on fake hardware + change_controllers = context.perform_substitution(use_mock_hardware) + if change_controllers == "true": + controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False + controllers_yaml["joint_trajectory_controller"]["default"] = True + + moveit_controllers = { + "moveit_simple_controller_manager": controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse_host": warehouse_sqlite_path, + } + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + robot_description_planning, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {"use_sim_time": use_sim_time}, + warehouse_ros_config, + + #Publish the descriptions for moveitpy + {"publish_robot_description": True}, + {"publish_robot_description_semantic": True}, + ], + ) + + # rviz with moveit configuration + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"] + ) + rviz_node = Node( + package="rviz2", + condition=IfCondition(launch_rviz), + executable="rviz2", + name="rviz2_moveit", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + robot_description_kinematics, + robot_description_planning, + warehouse_ros_config, + ], + ) + + # Servo node for realtime control + servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml") + servo_params = {"moveit_servo": servo_yaml} + servo_node = Node( + package="moveit_servo", + condition=IfCondition(launch_servo), + executable="servo_node_main", + parameters=[ + servo_params, + robot_description, + robot_description_semantic, + robot_description_kinematics, + ], + output="screen", + ) + + # Start setting up the required parameters for the moveit_py node + moveit_py_yaml = load_yaml("moveit2_tutorials", "config/motion_planning_python_api_tutorial.yaml") + ur_ompl_planning_pipeline_config = { + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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_id": "RRTConnectkConfigDefault" + } + } + ur_ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + + # Not tested with CHOMP and Pilz, just added this to be able to use the original moveit2_tutorials yaml file. + ur_chomp_planning_pipeline_config = { + "chomp": { + "planning_plugin": "chomp_interface/CHOMPPlanner", + "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization 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, + } + } + + ur_pilz_planning_pipeline_config = { + "pilz_industrial_motion_planner": { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", + "request_adapters": "default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", + "default_planner_config": "PTP", + "planner_id": "PTP", + } + } + + pilz_cartesian_limits_yaml = load_yaml("moveit2_tutorials", "config/pilz_cartesian_limits.yaml") + robot_description_planning["robot_description_planning"].update(pilz_cartesian_limits_yaml) + moveit_py_yaml["plan_request_params"].update({"planner_id": "RRTConnectkConfigDefault", "planning_time": 1.0, "planning_pipeline": "ompl"}) + # To initialize the planning with Pilz comment upper line and uncomment following line: + # moveit_py_yaml["plan_request_params"].update({"planner_id": "PTP", "planning_time": 1.0, "planning_pipeline": "pilz_industrial_motion_planner"}) + + moveit_args_not_concatenated = [ + moveit_py_yaml, + robot_description, + robot_description_semantic, + {"robot_description_kinematics": load_yaml("moveit2_tutorials", "config/ur_kinematics.yaml")}, + ur_ompl_planning_pipeline_config, + ur_chomp_planning_pipeline_config, + ur_pilz_planning_pipeline_config, + robot_description_planning, + moveit_controllers, + planning_scene_monitor_parameters, + trajectory_execution, + { + "publish_robot_description": True, + "publish_robot_description_semantic": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + }, + ] + + # Concatenate all dictionaries together, else moveitpy won't read all parameters + moveit_args = dict() + for d in moveit_args_not_concatenated: + moveit_args.update(d) + + moveit_py_node = Node( + name="moveit_py", + package="moveit2_tutorials", + executable="motion_planning_python_api_tutorial_ur.py", + output="both", + parameters=[moveit_args] + ) + + nodes_to_start = [move_group_node, rviz_node, servo_node, moveit_py_node] + + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Type/series of used UR robot.", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Indicate whether robot is running with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_limits", + default_value="true", + description="Enables the safety limits controller if true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_pos_margin", + default_value="0.15", + description="The margin to lower and upper limits in the safety controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_k_position", + default_value="20", + description="k-position factor in the safety controller.", + ) + ) + # General arguments + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ur_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="ur.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + default_value="ur_moveit_config", + description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom moveit config.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_file", + default_value="ur.srdf.xacro", + description="MoveIt SRDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_joint_limits_file", + default_value="joint_limits.yaml", + description="MoveIt joint limits that augment or override the values from the URDF robot_description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "warehouse_sqlite_path", + default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"), + description="Path where the warehouse database should be stored", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?") + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/doc/examples/motion_planning_python_api/readme.md b/doc/examples/motion_planning_python_api/readme.md new file mode 100644 index 0000000000..338f022260 --- /dev/null +++ b/doc/examples/motion_planning_python_api/readme.md @@ -0,0 +1,20 @@ +# Motion planning python API tutorial example for Universal Robot +The functionality of these files should be similar as the original tutorial with the Panda robot. +The main reason to change this tutorial specifically for a UR robot is that the `ur_moveit_config` package is slightly different from the available packages for robots in `moveit_resources`, which made it not so straightforward to use `MoveItConfigsBuilder` and launch the tutorial with the UR. With a proposed updated version of `ur_moveit.launch.py`, renamed to `motion_planning_python_api_tutorial.launch.py` the correct parameters are added to the `moveit_py` node. By keeping the structure of the original `ur_moveit.launch.py` it should be fairly easy to use this with any UR robot. Instructions are worked out in more detail for a simulated UR5. + + +## Dependencies +This tutorial example requires [Universal Robots ROS2 Driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main) +and [Universal Robots ROS2 Description](https://github.com/UniversalRobots/Universal_Robots_ROS2_Description) + +## Simulated UR +A summary of instructions to set up a simulated UR5 is added in the folder `ur_sim` + +## UR motion planning pyton api tutorial +Launch the tutorial to see the moveit_py interface control the simulated UR robot: + +Warning: Do NOT run this directly on a real robot yet, the planned trajectories result in big movements of the robot, which will likely cause collisions with a table. + +``` +ros2 launch moveit2_tutorials motion_planning_python_api_tutorial_ur.launch.py ur_type:=ur5 +``` \ No newline at end of file diff --git a/doc/examples/motion_planning_python_api/scripts/motion_planning_python_api_tutorial_ur.py b/doc/examples/motion_planning_python_api/scripts/motion_planning_python_api_tutorial_ur.py new file mode 100644 index 0000000000..871bdb5602 --- /dev/null +++ b/doc/examples/motion_planning_python_api/scripts/motion_planning_python_api_tutorial_ur.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 +""" +A script to outline the fundamentals of the moveit_py motion planning API. +""" + +import time + +# generic ros libraries +import rclpy +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + MultiPipelinePlanRequestParameters, +) + + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + # plan to goal + logger.info("Planning trajectory") + if multi_plan_parameters is not None: + plan_result = planning_component.plan( + multi_plan_parameters=multi_plan_parameters + ) + elif single_plan_parameters is not None: + plan_result = planning_component.plan( + single_plan_parameters=single_plan_parameters + ) + else: + plan_result = planning_component.plan() + + # execute the plan + if plan_result: + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute(robot_trajectory, controllers=[]) #, blocking=True + else: + logger.error("Planning failed") + + time.sleep(sleep_time) + + +def main(): + + ################################################################### + # MoveItPy Setup + ################################################################### + rclpy.init() + logger = get_logger("moveit_py.pose_goal") + + # instantiate MoveItPy instance and get planning component + ur = MoveItPy(node_name="moveit_py") + ur_manipulator = ur.get_planning_component("ur_manipulator") + logger.info("MoveItPy instance created") + + ########################################################################### + # Plan 1 - set states with predefined string + ########################################################################### + + # set plan start state using predefined state + # ur_manipulator.set_start_state(configuration_name="home") + + # set plan start state to current state of robot, command above fails planning if robot is not in home state. + ur_manipulator.set_start_state_to_current_state() + + # set pose goal using predefined state + ur_manipulator.set_goal_state(configuration_name="up") + + # plan to goal + plan_and_execute(ur, ur_manipulator, logger, sleep_time=3.0) + + ########################################################################### + # Plan 2 - set goal state with RobotState object + ########################################################################### + + # instantiate a RobotState instance using the current robot model + robot_model = ur.get_robot_model() + robot_state = RobotState(robot_model) + + # randomize the robot state + robot_state.set_to_random_positions() + + # set plan start state to current state + ur_manipulator.set_start_state_to_current_state() + + # set goal state to the initialized robot state + logger.info("Set goal state to the initialized robot state") + ur_manipulator.set_goal_state(robot_state=robot_state) + + # plan to goal + plan_and_execute(ur, ur_manipulator, logger, sleep_time=3.0) + + ########################################################################### + # Plan 3 - set goal state with PoseStamped message + ########################################################################### + + # set plan start state to current state + ur_manipulator.set_start_state_to_current_state() + + # set pose goal with PoseStamped message + from geometry_msgs.msg import PoseStamped + + pose_goal = PoseStamped() + pose_goal.header.frame_id = "base_link" + pose_goal.pose.orientation.w = -1.0 + pose_goal.pose.position.x = 0.6 + pose_goal.pose.position.y = -0.3 + pose_goal.pose.position.z = 0.5 + ur_manipulator.set_goal_state(pose_stamped_msg=pose_goal, pose_link="tool0") + + # plan to goal + plan_and_execute(ur, ur_manipulator, logger, sleep_time=3.0) + + ########################################################################### + # Plan 4 - set goal state with constraints + ########################################################################### + + # set plan start state to current state + ur_manipulator.set_start_state_to_current_state() + + # set constraints message + from moveit.core.kinematic_constraints import construct_joint_constraint + + joint_values = { + "shoulder_pan_joint": -1.0, + "shoulder_lift_joint": 0.7, + "elbow_joint": 0.7, + "wrist_1_joint": -1.5, + "wrist_2_joint": -0.7, + "wrist_3_joint": 2.0, + } + robot_state.joint_positions = joint_values + joint_constraint = construct_joint_constraint( + robot_state=robot_state, + joint_model_group=ur.get_robot_model().get_joint_model_group("ur_manipulator"), + ) + ur_manipulator.set_goal_state(motion_plan_constraints=[joint_constraint]) + + # plan to goal + plan_and_execute(ur, ur_manipulator, logger, sleep_time=3.0) + + ########################################################################### + # Plan 5 - Planning with Multiple Pipelines simultaneously + ########################################################################### + + # set plan start state to current state + ur_manipulator.set_start_state_to_current_state() + + # set pose goal with PoseStamped message + ur_manipulator.set_goal_state(configuration_name="test_configuration") + + # initialise multi-pipeline plan request parameters + multi_pipeline_plan_request_params = MultiPipelinePlanRequestParameters( + ur, ["ompl_rrtc", "chomp_planner"] #, "chomp", "pilz_lin", "ompl_rrt_star"] + ) + + # plan to goal + plan_and_execute( + ur, + ur_manipulator, + logger, + multi_plan_parameters=multi_pipeline_plan_request_params, + sleep_time=3.0, + ) + + +if __name__ == "__main__": + main() diff --git a/doc/examples/motion_planning_python_api/ur_sim/readme_starting_UR_sim.md b/doc/examples/motion_planning_python_api/ur_sim/readme_starting_UR_sim.md new file mode 100644 index 0000000000..3d7743d51a --- /dev/null +++ b/doc/examples/motion_planning_python_api/ur_sim/readme_starting_UR_sim.md @@ -0,0 +1,119 @@ +# Workspace installation/startup instruction + +This installation instruction assumes you have ROS2 installed on a Ubuntu 22 system, and is just a short summary that links the user to relevant other readme files and sometimes has specific instruction for this situation. +To start create a folder in the home directory for the workspace and go there + ``` + mkdir -p ~/ros2_ws/src + cd ~/ros2_ws/src + ``` +In this folder we will clone this repository to make relevant packages for the RMM trials + +Source this workspace by adding the following lines in `.bashrc` +``` +source ~/ros2_ws/install/setup.bash +source ~/ros2_ws/install/local_setup.bash +``` + +## Install the Universal robot ROS2 driver +Follow the installation instructions for an installation from source in this workspace on the [Universal Robots ROS2 Driver humble GIT](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/humble) + +## Install the Universal robot description package +Follow the installation instructions for an installation from source in this workspace on the [Universal Robots ROS2 Description](https://github.com/UniversalRobots/Universal_Robots_ROS2_Description) + + +### Installing Docker for Ubuntu +1. Update the `apt` package index and install packages to allow `apt` to use a repository over HTTPS: + +``` +sudo apt-get update +sudo apt-get install ca-certificates curl gnupg +``` + +2. Add Docker’s official GPG key: + +``` +sudo install -m 0755 -d /etc/apt/keyrings +curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg +sudo chmod a+r /etc/apt/keyrings/docker.gpg +``` + +3. Use the following command to set up the repository: + +``` +echo \ + "deb [arch="$(dpkg --print-architecture)" signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \ + "$(. /etc/os-release && echo "$VERSION_CODENAME")" stable" | \ + sudo tee /etc/apt/sources.list.d/docker.list > /dev/null +``` + +4. Update the apt package index: + +``` +sudo apt-get update +``` + +5. Install Docker Engine, containerd, and Docker Compose. + +``` +sudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin +``` + +6. Verify that the Docker Engine installation is successful by running the `hello-world` image. + +``` +sudo docker run hello-world +``` + +7. Create the `docker` group. + +``` +sudo groupadd docker +``` + +8. Add your user to the `docker` group. + +``` +sudo usermod -aG docker $USER +``` + +9. Run the following command to activate the changes to groups: + +``` +newgrp docker +``` + +10. Verify that you can run `docker` commands without `sudo`. + +``` +docker run hello-world +``` + +## Get the URSIM docker +The CB3 series simulation docker is available installation instruction and more information on how to run can be found [via this website](https://hub.docker.com/r/universalrobots/ursim_cb3) + +Simply running the robot simulation can be done using: +``` +docker run --rm -it -e ROBOT_MODEL=UR5 -p 5900:5900 -p 6080:6080 universalrobots/ursim_cb3 +``` + +To control the robot via ROS2 (also in simulation) we require to also install and run an external control program on the robot. + +To install and use the URCap external control with a real robot, follow the instructions from the ROS Driver for [e-Series](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/install_urcap_e_series.md) +or for [cb3-Series](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/install_urcap_cb3.md). + +For the robot simulation thes steps have been performed already and result in the files that are included in "ursim_init_folder_structure.zip". Place the contents of this .zip in the home directory, such that there is a new folder `.ursim` + +The next step is to run the URsim docker and mount the /urcaps folder to a folder containing the urcaps jar files, at start time: +``` +docker run --rm -it -p 5900:5900 -p 6080:6080 -v ${HOME}/.ursim/urcaps:/urcaps -v ${HOME}/.ursim/programs:/ursim/programs --name ursim universalrobots/ursim_cb3 +``` + +With the Simulated robot running and the workspaces sourced it is finally possible to start controlling the robot trough ros +First, launch the robot control which sets up the connection with the simulated robot (This assumes the IP's are al set correctly, the docker takes `172.17.0.2` as default) + +``` +ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5 robot_ip:=172.17.0.2 launch_rviz:=false +``` + +Using the teachpendant of the simulated robot, start ext_ctrl.urp, to allow the robot to be controlled by a remote interface. + diff --git a/doc/examples/motion_planning_python_api/ur_sim/ursim_init_folder_structure.zip b/doc/examples/motion_planning_python_api/ur_sim/ursim_init_folder_structure.zip new file mode 100644 index 0000000000..7d80745222 Binary files /dev/null and b/doc/examples/motion_planning_python_api/ur_sim/ursim_init_folder_structure.zip differ