From fa8e7cb9baa92deabffe38275c6bea7b7507fb55 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 27 May 2024 14:28:11 +0200 Subject: [PATCH] Added servo node --- ur_moveit_config/config/ur_servo.yaml | 54 +++++++++++---------- ur_moveit_config/launch/ur_moveit.launch.py | 34 ++++++++++++- 2 files changed, 62 insertions(+), 26 deletions(-) diff --git a/ur_moveit_config/config/ur_servo.yaml b/ur_moveit_config/config/ur_servo.yaml index 172d2592..bccf49ae 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,51 +32,55 @@ 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. +# 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 5ee7b048..f8aa5d7b 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -29,6 +29,7 @@ # # Author: Felix Exner import os +import yaml from pathlib import Path @@ -46,6 +47,19 @@ from moveit_configs_utils import MoveItConfigsBuilder +from ament_index_python.packages import get_package_share_directory + + +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( @@ -71,6 +85,9 @@ def declare_arguments(): default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"), description="Path where the warehouse database should be stored", ), + DeclareLaunchArgument( + "launch_servo", default_value="true", description="Launch Servo?" + ), ] ) @@ -79,6 +96,7 @@ 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") moveit_config = ( MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config") @@ -111,6 +129,19 @@ def generate_launch_description(): ], ) + 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", + parameters=[ + moveit_config.to_dict(), + servo_params, + ], + output="screen", + ) + rviz_config_file = PathJoinSubstitution( [FindPackageShare("ur_moveit_config"), "config", "moveit.rviz"] ) @@ -134,7 +165,8 @@ def generate_launch_description(): ld.add_action( RegisterEventHandler( OnProcessExit( - target_action=wait_robot_description, on_exit=[move_group_node, rviz_node] + target_action=wait_robot_description, + on_exit=[move_group_node, rviz_node, servo_node], ) ), )