Skip to content

Commit

Permalink
move_group launch for moveit_configs_utils (#1131)
Browse files Browse the repository at this point in the history
  • Loading branch information
DLu authored Apr 19, 2022
1 parent d27518d commit 7f04091
Show file tree
Hide file tree
Showing 10 changed files with 361 additions and 21 deletions.
12 changes: 12 additions & 0 deletions moveit_configs_utils/default_configs/chomp_planning.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
planning_plugin: chomp_interface/CHOMPPlanner
enable_failure_recovery: true
jiggle_fraction: 0.05
request_adapters: >-
default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
ridge_factor: 0.01
start_state_max_bounds_error: 0.1
129 changes: 129 additions & 0 deletions moveit_configs_utils/default_configs/ompl_defaults.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,129 @@
planner_configs:
AnytimePathShortening:
type: geometric::AnytimePathShortening
shortcut: true # Attempt to shortcut all new solution paths
hybridize: true # Compute hybrid solution trajectories
max_hybrid_paths: 24 # Number of hybrid paths generated per iteration
num_planners: 4 # The number of default planners to use for planning
planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
SBL:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
EST:
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
LBKPIECE:
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
BKPIECE:
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
KPIECE:
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
RRT:
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
RRTConnect:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstar:
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
TRRT:
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()
PRM:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PRMstar:
type: geometric::PRMstar
FMT:
type: geometric::FMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
nearest_k: 1 # use Knearest strategy. default: 1
cache_cc: 1 # use collision checking cache. default: 1
heuristics: 0 # activate cost to go heuristics. default: 0
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
BFMT:
type: geometric::BFMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
nearest_k: 1 # use the Knearest strategy. default: 1
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
heuristics: 1 # activates cost to go heuristics. default: 1
cache_cc: 1 # use the collision checking cache. default: 1
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
PDST:
type: geometric::PDST
STRIDE:
type: geometric::STRIDE
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
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
max_degree: 18 # max degree of a node in the GNAT. default: 12
min_degree: 12 # min degree of a node in the GNAT. default: 12
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
BiTRRT:
type: geometric::BiTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
init_temperature: 100 # initial temperature. default: 100
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
LBTRRT:
type: geometric::LBTRRT
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
epsilon: 0.4 # optimality approximation factor. default: 0.4
BiEST:
type: geometric::BiEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ProjEST:
type: geometric::ProjEST
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
LazyPRM:
type: geometric::LazyPRM
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
LazyPRMstar:
type: geometric::LazyPRMstar
SPARS:
type: geometric::SPARS
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 1000 # maximum consecutive failure limit. default: 1000
SPARStwo:
type: geometric::SPARStwo
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
10 changes: 10 additions & 0 deletions moveit_configs_utils/default_configs/ompl_planning.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
planning_plugin: ompl_interface/OMPLPlanner
start_state_max_bounds_error: 0.1
jiggle_fraction: 0.05
request_adapters: >-
default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
planning_plugin: pilz_industrial_motion_planner/CommandPlanner
request_adapters: ""
default_planner_config: PTP
capabilities: >-
pilz_industrial_motion_planner/MoveGroupSequenceAction
pilz_industrial_motion_planner/MoveGroupSequenceService
9 changes: 9 additions & 0 deletions moveit_configs_utils/default_configs/trajopt_planning.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
planning_plugin: trajopt_interface/TrajOptPlanner
start_state_max_bounds_error: 0.1
jiggle_fraction: 0.05
request_adapters: >-
default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
17 changes: 16 additions & 1 deletion moveit_configs_utils/moveit_configs_utils/launch_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,13 @@ def __init__(self, name, *, default_value=False, description=None, **kwargs):


def add_debuggable_node(
ld, package, executable, condition_name="debug", commands_file=None, **kwargs
ld,
package,
executable,
condition_name="debug",
commands_file=None,
extra_debug_args=None,
**kwargs,
):
"""Adds two versions of a Node to the launch description, one with gdb debugging, controlled by a launch config"""
standard_node = Node(
Expand All @@ -36,11 +42,20 @@ def add_debuggable_node(
dash_x_arg = ""
prefix = [f"gdb {dash_x_arg}--ex run --args"]

if "arguments" in kwargs:
arguments = kwargs["arguments"]
if extra_debug_args:
arguments += extra_debug_args
del kwargs["arguments"]
else:
arguments = None

debug_node = Node(
package=package,
executable=executable,
condition=IfCondition(LaunchConfiguration(condition_name)),
prefix=prefix,
arguments=arguments,
**kwargs,
)
ld.add_action(debug_node)
60 changes: 57 additions & 3 deletions moveit_configs_utils/moveit_configs_utils/launches.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue

from srdfdom.srdf import SRDF

Expand Down Expand Up @@ -37,7 +38,7 @@ def generate_rsp_launch(moveit_config):
return ld


def generate_rviz_launch(moveit_config):
def generate_moveit_rviz_launch(moveit_config):
"""Launch file for rviz"""
ld = LaunchDescription()

Expand Down Expand Up @@ -67,7 +68,7 @@ def generate_rviz_launch(moveit_config):
return ld


def generate_assistant_launch(moveit_config):
def generate_setup_assistant_launch(moveit_config):
"""Launch file for MoveIt Setup Assistant"""
ld = LaunchDescription()

Expand Down Expand Up @@ -125,7 +126,7 @@ def generate_spawn_controllers_launch(moveit_config):
return ld


def generate_warehouse_launch(moveit_config):
def generate_warehouse_db_launch(moveit_config):
"""Launch file for warehouse database"""
ld = LaunchDescription()
ld.add_action(
Expand Down Expand Up @@ -177,3 +178,56 @@ def generate_warehouse_launch(moveit_config):
ld.add_action(reset_node)

return ld


def generate_move_group_launch(moveit_config):
ld = LaunchDescription()

ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
ld.add_action(
DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
)
ld.add_action(
DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
)
# load non-default MoveGroup capabilities (space separated)
ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
# inhibit these default MoveGroup capabilities (space separated)
ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))

should_publish = LaunchConfiguration("publish_monitored_planning_scene")

move_group_configuration = {
"publish_robot_description_semantic": True,
"allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
# Note: Wrapping the following values is necessary so that the parameter value can be the empty string
"capabilities": ParameterValue(
LaunchConfiguration("capabilities"), value_type=str
),
"disable_capabilities": ParameterValue(
LaunchConfiguration("disable_capabilities"), value_type=str
),
# Publish the planning scene of the physical robot so that rviz plugin can know actual robot
"publish_planning_scene": should_publish,
"publish_geometry_updates": should_publish,
"publish_state_updates": should_publish,
"publish_transforms_updates": should_publish,
}

move_group_params = [
move_group_configuration,
moveit_config.to_dict(),
]

add_debuggable_node(
ld,
package="moveit_ros_move_group",
executable="move_group",
commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
output="screen",
parameters=move_group_params,
extra_debug_args=["--debug"],
# Set the display variable, in case OpenGL code is used internally
additional_env={"DISPLAY": ":0"},
)
return ld
Loading

0 comments on commit 7f04091

Please sign in to comment.