diff --git a/ur_moveit_config/CMakeLists.txt b/ur_moveit_config/CMakeLists.txt index e47ea7642..df0cc0f94 100644 --- a/ur_moveit_config/CMakeLists.txt +++ b/ur_moveit_config/CMakeLists.txt @@ -1,15 +1,11 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.22) project(ur_moveit_config) find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) -install(DIRECTORY config launch rviz srdf +ament_package() + +install( + DIRECTORY config launch DESTINATION share/${PROJECT_NAME} ) - -# Install Python modules -ament_python_install_package(${PROJECT_NAME}) -ament_python_install_module(${PROJECT_NAME}/launch_common.py) - -ament_package() diff --git a/ur_moveit_config/config/chomp_planning.yaml b/ur_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..07c1f32c4 --- /dev/null +++ b/ur_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,15 @@ +planning_plugins: + - chomp_interface/CHOMPPlanner +enable_failure_recovery: true +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + +ridge_factor: 0.01 diff --git a/ur_moveit_config/config/controllers.yaml b/ur_moveit_config/config/controllers.yaml deleted file mode 100644 index 784ebd315..000000000 --- a/ur_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,29 +0,0 @@ -controller_names: - - scaled_joint_trajectory_controller - - joint_trajectory_controller - - -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: false - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur_moveit_config/config/kinematics.yaml b/ur_moveit_config/config/kinematics.yaml index af3d30e89..bfaeef650 100644 --- a/ur_moveit_config/config/kinematics.yaml +++ b/ur_moveit_config/config/kinematics.yaml @@ -1,8 +1,4 @@ -/**: - ros__parameters: - robot_description_kinematics: - ur_manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 +ur_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 diff --git a/ur_moveit_config/rviz/view_robot.rviz b/ur_moveit_config/config/moveit.rviz similarity index 100% rename from ur_moveit_config/rviz/view_robot.rviz rename to ur_moveit_config/config/moveit.rviz diff --git a/ur_moveit_config/config/moveit_controllers.yaml b/ur_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 000000000..34e4194c7 --- /dev/null +++ b/ur_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,32 @@ +# 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 + + 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: false + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur_moveit_config/config/ompl_planning.yaml b/ur_moveit_config/config/ompl_planning.yaml index 4dcc7a773..3bb0acfc8 100644 --- a/ur_moveit_config/config/ompl_planning.yaml +++ b/ur_moveit_config/config/ompl_planning.yaml @@ -1,70 +1,12 @@ -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 -ur_manipulator: - 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 +planning_plugins: + - ompl_interface/OMPLPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath diff --git a/ur_moveit_config/config/pilz_cartesian_limits.yaml b/ur_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 000000000..b2997caf4 --- /dev/null +++ b/ur_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/ur_moveit_config/config/pilz_industrial_motion_planner_planning.yaml b/ur_moveit_config/config/pilz_industrial_motion_planner_planning.yaml new file mode 100644 index 000000000..d0a07ce4c --- /dev/null +++ b/ur_moveit_config/config/pilz_industrial_motion_planner_planning.yaml @@ -0,0 +1,14 @@ +planning_plugins: + - pilz_industrial_motion_planner/CommandPlanner +default_planner_config: PTP +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath +capabilities: >- + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService diff --git a/ur_moveit_config/config/ur_servo.yaml b/ur_moveit_config/config/ur_servo.yaml index 767481a8a..43a99185d 100644 --- a/ur_moveit_config/config/ur_servo.yaml +++ b/ur_moveit_config/config/ur_servo.yaml @@ -4,11 +4,11 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. joint: 0.01 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level @@ -32,45 +32,54 @@ publish_joint_accelerations: false ## Plugins for smoothing outgoing commands smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: false + ## Incoming Joint State properties low_pass_filter_coeff: 10.0 # Larger --> trust the filtered data more, trust the measurements less. ## MoveIt properties -move_group_name: ur_manipulator # Often 'manipulator' or 'arm' +move_group_name: ur_manipulator # Often 'manipulator' or 'arm' planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames ee_frame: tool0 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector +robot_link_command_frame: tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits -lower_singularity_threshold: 100.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +lower_singularity_threshold: 100.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this + +# added as a buffer to joint limits [radians]. If moving quickly, make this larger. +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states -status_topic: ~/status # Publish status to this topic -command_out_topic: /forward_position_controller/commands # Publish outgoing commands here +status_topic: ~/status # Publish status to this topic +command_out_topic: /forward_position_controller/commands # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. -# Two collision check algorithms are available: -# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. -# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +check_collisions: true # Check collisions? +collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: "threshold_distance" begins slowing down when +# nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the +# distance is decreasing. Requires joint acceleration limits collision_check_type: threshold_distance # Parameters for "threshold_distance"-type collision checking -self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] -scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] # Parameters for "stop_distance"-type collision checking -collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency -min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] +collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index a73bd73be..bda0c1078 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2021 PickNik, Inc. +# Copyright (c) 2024 FZI Forschungszentrum Informatik # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -27,225 +27,115 @@ # POSSIBILITY OF SUCH DAMAGE. # -# Author: Denis Stogl - +# Author: Felix Exner import os +import yaml -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from ur_moveit_config.launch_common import load_yaml +from pathlib import Path from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, RegisterEventHandler from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare -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") +from moveit_configs_utils import MoveItConfigsBuilder - 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"] - ) +from ament_index_python.packages import get_package_share_directory - 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( +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def declare_arguments(): + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "srdf", moveit_config_file] + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?"), + DeclareLaunchArgument( + "ur_type", + description="Typo/series of used UR robot.", + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], + ), + DeclareLaunchArgument( + "warehouse_sqlite_path", + default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"), + description="Path where the warehouse database should be stored", + ), + DeclareLaunchArgument( + "launch_servo", default_value="false", description="Launch Servo?" + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Using or not time from simulation", ), - " ", - "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_plugins": ["ompl_interface/OMPLPlanner"], - "request_adapters": [ - "default_planning_request_adapters/ResolveConstraintFrames", - "default_planning_request_adapters/ValidateWorkspaceBounds", - "default_planning_request_adapters/CheckStartStateBounds", - "default_planning_request_adapters/CheckStartStateCollision", - ], - "response_adapters": [ - "default_planning_response_adapters/AddTimeOptimalParameterization", - "default_planning_response_adapters/ValidateSolution", - "default_planning_response_adapters/DisplayMotionPath", - ], - } - } - 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 mock 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": False, - "trajectory_execution.allowed_execution_duration_scaling": 1.2, - "trajectory_execution.allowed_goal_duration_margin": 0.5, - "trajectory_execution.allowed_start_tolerance": 0.01, - } +def generate_launch_description(): + launch_rviz = LaunchConfiguration("launch_rviz") + ur_type = LaunchConfiguration("ur_type") + warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path") + launch_servo = LaunchConfiguration("launch_servo") + use_sim_time = LaunchConfiguration("use_sim_time") - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } + moveit_config = ( + MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config") + .robot_description_semantic(Path("srdf") / "ur.srdf.xacro", {"name": ur_type}) + .to_moveit_configs() + ) warehouse_ros_config = { "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", "warehouse_host": warehouse_sqlite_path, } - # Start the actual move_group node/action server + ld = LaunchDescription() + ld.add_entity(declare_arguments()) + + wait_robot_description = Node( + package="ur_robot_driver", + executable="wait_for_robot_description", + output="screen", + ) + ld.add_action(wait_robot_description) + 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, - ], - ) - - # 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, + moveit_config.to_dict(), warehouse_ros_config, + {"use_sim_time": use_sim_time}, ], ) - # 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( @@ -253,122 +143,39 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(launch_servo), executable="servo_node", parameters=[ + moveit_config.to_dict(), servo_params, - robot_description, - robot_description_semantic, ], output="screen", ) - nodes_to_start = [move_group_node, rviz_node, servo_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", "ur20", "ur30"], - ) - ) - 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.", - ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ur_moveit_config"), "config", "moveit.rviz"] ) - declared_arguments.append( - DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + rviz_node = Node( + package="rviz2", + condition=IfCondition(launch_rviz), + executable="rviz2", + name="rviz2_moveit", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + warehouse_ros_config, + ], ) - declared_arguments.append( - DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?") + + ld.add_action( + RegisterEventHandler( + OnProcessExit( + target_action=wait_robot_description, + on_exit=[move_group_node, rviz_node, servo_node], + ) + ), ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return ld diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index a15a90202..20c69437a 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -6,29 +6,28 @@ An example package with MoveIt2 configurations for UR robots. - Denis Stogl Felix Exner - Robert Wilbrandt + Vincenzo Di Pentima Apache2.0 + Robert Wilbrandt + Denis Stogl Lovro Ivanov Andy Zelenak ament_cmake - ament_cmake_python - launch - launch_ros + + moveit_configs_utils moveit_kinematics - moveit_planners_ompl + moveit_planners + moveit_planners_chomp moveit_ros_move_group moveit_ros_visualization moveit_servo moveit_simple_controller_manager - rviz2 ur_description - urdf warehouse_ros_sqlite xacro diff --git a/ur_moveit_config/srdf/ur.srdf.xacro b/ur_moveit_config/srdf/ur.srdf.xacro index 0ae2cca27..9cec140f2 100644 --- a/ur_moveit_config/srdf/ur.srdf.xacro +++ b/ur_moveit_config/srdf/ur.srdf.xacro @@ -3,11 +3,9 @@ - - - + diff --git a/ur_moveit_config/srdf/ur_macro.srdf.xacro b/ur_moveit_config/srdf/ur_macro.srdf.xacro index 04554f33c..15f3dcec3 100644 --- a/ur_moveit_config/srdf/ur_macro.srdf.xacro +++ b/ur_moveit_config/srdf/ur_macro.srdf.xacro @@ -4,54 +4,54 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - - + + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - + + + + + + + + + + + diff --git a/ur_moveit_config/ur_moveit_config/__init__.py b/ur_moveit_config/ur_moveit_config/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/ur_moveit_config/ur_moveit_config/launch_common.py b/ur_moveit_config/ur_moveit_config/launch_common.py deleted file mode 100644 index 1d53a1cfc..000000000 --- a/ur_moveit_config/ur_moveit_config/launch_common.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 - -# 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: Lovro Ivanov - -import math -import os -import yaml - -from ament_index_python.packages import get_package_share_directory - - -def construct_angle_radians(loader, node): - """Utility function to construct radian values from yaml.""" - value = loader.construct_scalar(node) - try: - return float(value) - except SyntaxError: - raise Exception("invalid expression: %s" % value) - - -def construct_angle_degrees(loader, node): - """Utility function for converting degrees into radians from yaml.""" - return math.radians(construct_angle_radians(loader, node)) - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - yaml.SafeLoader.add_constructor("!radians", construct_angle_radians) - yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees) - except Exception: - raise Exception("yaml support not available; install python-yaml") - - try: - with open(absolute_file_path) as file: - return yaml.safe_load(file) - except OSError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml_abs(absolute_file_path): - try: - yaml.SafeLoader.add_constructor("!radians", construct_angle_radians) - yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees) - except Exception: - raise Exception("yaml support not available; install python-yaml") - - try: - with open(absolute_file_path) as file: - return yaml.safe_load(file) - except OSError: # parent of IOError, OSError *and* WindowsError where available - return None diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 640667234..d341b911f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -142,9 +142,11 @@ ament_export_dependencies( # Install Python execs ament_python_install_package(${PROJECT_NAME}) -# Install Python executables +# Install executables install(PROGRAMS scripts/tool_communication.py + scripts/wait_for_robot_description + scripts/start_ursim.sh DESTINATION lib/${PROJECT_NAME} ) @@ -152,10 +154,6 @@ install(PROGRAMS scripts/wait_dashboard_server.sh DESTINATION bin ) -install(PROGRAMS scripts/start_ursim.sh - DESTINATION lib/${PROJECT_NAME} -) - install(DIRECTORY config launch urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/ur_robot_driver/scripts/wait_for_robot_description b/ur_robot_driver/scripts/wait_for_robot_description new file mode 100755 index 000000000..6975cd8ec --- /dev/null +++ b/ur_robot_driver/scripts/wait_for_robot_description @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024 FZI Forschungszentrum Informatik +# +# 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. + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + +from rclpy.task import Future +from rclpy.qos import QoSProfile, QoSDurabilityPolicy + + +class DescriptionSubscriber(Node): + + def __init__(self): + super().__init__("wait_for_robot_description") + self.topic = "robot_description" + qos_profile = QoSProfile( + depth=1, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self.subscription = self.create_subscription( + String, self.topic, self.desc_callback, qos_profile=qos_profile + ) + self.future = Future() + + self.get_logger().info(f"Waiting for message on {self.resolve_topic_name(self.topic)}.") + + def desc_callback(self, msg): + self.get_logger().info( + f"Received message on {self.resolve_topic_name(self.topic)}. Shutting down." + ) + self.future.set_result(True) + + +def main(args=None): + rclpy.init(args=args) + + sub = DescriptionSubscriber() + + rclpy.spin_until_future_complete(sub, sub.future) + + sub.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()