Make moveit_config compatible to moveit_configs_builder (#998)

* Make the config moveit_configs_builder compatible

- Make sure it can be used in conjunction with the configs builder
- Still use a parametrized srdf
- Re-use the description from the robot state publisher

* Wait for robot_description in moveit startup

Since we built our moveit config around receiving the description via topic, we also add a script that waits for the robot description to be published.
This helps when components support subscribing to the description, but crash if it is not present.

* Added servo node

* Add servo node config to disable advertising /get_planning_scene

* Apply suggestions from code review

Co-authored-by: Vincenzo Di Pentima <[email protected]>

* Fix corrections

* Added missing dependency for chomp planners

* Added use_sim_time argument and defaulted launch_servo to false

* Launchfile formatting

---------

Co-authored-by: Ruddick Lawrence <[email protected]>
Co-authored-by: Vincenzo Di Pentima <[email protected]>

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()

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

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

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

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

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

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

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]