From 71c89200e20fd40a41b5f6c4c36983f380413b0d Mon Sep 17 00:00:00 2001 From: Jesse Li Date: Tue, 26 May 2020 16:46:00 -0700 Subject: [PATCH 1/3] Updated setup assistant to include name --- pr2_moveit_config/.setup_assistant | 5 ++++- pr2_moveit_config/launch/setup_assistant.launch | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/pr2_moveit_config/.setup_assistant b/pr2_moveit_config/.setup_assistant index b2b0946..5e59260 100644 --- a/pr2_moveit_config/.setup_assistant +++ b/pr2_moveit_config/.setup_assistant @@ -2,7 +2,10 @@ moveit_setup_assistant_config: URDF: package: pr2_description relative_path: robots/pr2.urdf.xacro + xacro_args: "" SRDF: relative_path: config/pr2.srdf CONFIG: - generated_timestamp: 1364768330 \ No newline at end of file + author_name: MoveIt Setup Assistant + author_email: assistant@moveit.ros.org + generated_timestamp: 1590536702 \ No newline at end of file diff --git a/pr2_moveit_config/launch/setup_assistant.launch b/pr2_moveit_config/launch/setup_assistant.launch index 371a28a..637e00a 100644 --- a/pr2_moveit_config/launch/setup_assistant.launch +++ b/pr2_moveit_config/launch/setup_assistant.launch @@ -7,7 +7,7 @@ - From f07351f9e86d8333e9c87c6b4684644f0f1c656e Mon Sep 17 00:00:00 2001 From: Jesse Li Date: Tue, 26 May 2020 17:31:02 -0700 Subject: [PATCH 2/3] Updated OMPL planning options --- pr2_moveit_config/config/ompl_planning.yaml | 579 +++++++++++++++----- 1 file changed, 456 insertions(+), 123 deletions(-) diff --git a/pr2_moveit_config/config/ompl_planning.yaml b/pr2_moveit_config/config/ompl_planning.yaml index a281f54..ee7f416 100644 --- a/pr2_moveit_config/config/ompl_planning.yaml +++ b/pr2_moveit_config/config/ompl_planning.yaml @@ -1,133 +1,466 @@ -## the planner configurations; each config must have a type, which specifies -## the planner to be used; other parameters can be specified as well, depending -## on the planner - -constraint_approximations_path: ~/constraints_approximation_database - +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 + 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 expresssion. 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 +base: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +left_arm: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(l_shoulder_pan_joint,l_shoulder_lift_joint) + longest_valid_segment_fraction: 0.005 +left_arm_and_torso: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(torso_lift_joint,l_shoulder_pan_joint) + longest_valid_segment_fraction: 0.005 right_arm: + default_planner_config: "" planner_configs: - - PRMkConfigDefault - - SBLkConfigDefault - - LBKPIECEkConfigDefault - - RRTkConfigDefault - - TRRTkConfigDefault - - RRTConnectkConfigDefault - - ESTkConfigDefault - - KPIECEkConfigDefault - - BKPIECEkConfigDefault - - RRTStarkConfigDefault - projection_evaluator: link(r_wrist_roll_link) - longest_valid_segment_fraction: 0.05 - + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(r_shoulder_pan_joint,r_shoulder_lift_joint) + longest_valid_segment_fraction: 0.005 right_arm_and_torso: + default_planner_config: "" planner_configs: - - SBLkConfigDefault - - LBKPIECEkConfigDefault - - RRTkConfigDefault - - TRRTkConfigDefault - - RRTConnectkConfigDefault - - ESTkConfigDefault - - KPIECEkConfigDefault - - BKPIECEkConfigDefault - - RRTStarkConfigDefault - projection_evaluator: joints(r_shoulder_pan_joint,r_shoulder_lift_joint) - longest_valid_segment_fraction: 0.05 - -left_arm: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(torso_lift_joint,r_shoulder_pan_joint) + longest_valid_segment_fraction: 0.005 +arms: + default_planner_config: "" planner_configs: - - SBLkConfigDefault - - LBKPIECEkConfigDefault - - RRTkConfigDefault - - TRRTkConfigDefault - - RRTConnectkConfigDefault - - ESTkConfigDefault - - KPIECEkConfigDefault - - BKPIECEkConfigDefault - - RRTStarkConfigDefault + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo projection_evaluator: joints(l_shoulder_pan_joint,l_shoulder_lift_joint) - longest_valid_segment_fraction: 0.05 - -left_arm_and_torso: + longest_valid_segment_fraction: 0.005 +head: + default_planner_config: "" planner_configs: - - SBLkConfigDefault - - LBKPIECEkConfigDefault - - RRTkConfigDefault - - TRRTkConfigDefault - - RRTConnectkConfigDefault - - ESTkConfigDefault - - KPIECEkConfigDefault - - BKPIECEkConfigDefault - - RRTStarkConfigDefault - projection_evaluator: joints(l_shoulder_pan_joint,l_shoulder_lift_joint) - longest_valid_segment_fraction: 0.05 - -arms: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(head_pan_joint,head_tilt_joint) + longest_valid_segment_fraction: 0.005 +torso: + default_planner_config: "" planner_configs: - - SBLkConfigDefault - - LBKPIECEkConfigDefault - - RRTkConfigDefault - - TRRTkConfigDefault - - RRTConnectkConfigDefault - - ESTkConfigDefault - - KPIECEkConfigDefault - - BKPIECEkConfigDefault - - RRTStarkConfigDefault - longest_valid_segment_fraction: 0.05 - projection_evaluator: joints(r_shoulder_pan_joint,l_shoulder_pan_joint) - + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +arms_and_torso: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(torso_lift_joint,l_shoulder_pan_joint) + longest_valid_segment_fraction: 0.005 whole_body: + default_planner_config: "" planner_configs: - - SBLkConfigDefault - - LBKPIECEkConfigDefault - - RRTkConfigDefault - - TRRTkConfigDefault - - RRTConnectkConfigDefault - - ESTkConfigDefault - - KPIECEkConfigDefault - - BKPIECEkConfigDefault - - RRTStarkConfigDefault - longest_valid_segment_fraction: 0.05 - projection_evaluator: joints(r_shoulder_pan_joint,l_shoulder_pan_joint) - - -planner_configs: - - PRMkConfigDefault: - type: geometric::PRM - - SBLkConfigDefault: - type: geometric::SBL - range: 100000000 - - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 100000000 - - RRTkConfigDefault: - type: geometric::RRT - range: 100000000 - - TRRTkConfigDefault: - type: geometric::TRRT - - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 100000000 - - LazyRRTkConfigDefault: - type: geometric::LazyRRT - - ESTkConfigDefault: - type: geometric::EST - range: 100000000 - - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 100000000 - - RRTStarkConfigDefault: - type: geometric::RRTstar - - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 100000000 + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +right_gripper: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +left_gripper: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo \ No newline at end of file From 9f02664608eeda8af2ddd288946b90f26e25a04e Mon Sep 17 00:00:00 2001 From: Jesse Li Date: Wed, 27 May 2020 12:32:02 -0700 Subject: [PATCH 3/3] Kept projection evaluators --- pr2_moveit_config/config/ompl_planning.yaml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/pr2_moveit_config/config/ompl_planning.yaml b/pr2_moveit_config/config/ompl_planning.yaml index ee7f416..3238e2f 100644 --- a/pr2_moveit_config/config/ompl_planning.yaml +++ b/pr2_moveit_config/config/ompl_planning.yaml @@ -209,7 +209,7 @@ left_arm_and_torso: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(torso_lift_joint,l_shoulder_pan_joint) + projection_evaluator: joints(l_shoulder_pan_joint,l_shoulder_lift_joint) longest_valid_segment_fraction: 0.005 right_arm: default_planner_config: "" @@ -238,7 +238,7 @@ right_arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(r_shoulder_pan_joint,r_shoulder_lift_joint) + projection_evaluator: link(r_wrist_roll_link) longest_valid_segment_fraction: 0.005 right_arm_and_torso: default_planner_config: "" @@ -267,7 +267,7 @@ right_arm_and_torso: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(torso_lift_joint,r_shoulder_pan_joint) + projection_evaluator: joints(r_shoulder_pan_joint,r_shoulder_lift_joint) longest_valid_segment_fraction: 0.005 arms: default_planner_config: "" @@ -296,7 +296,7 @@ arms: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(l_shoulder_pan_joint,l_shoulder_lift_joint) + projection_evaluator: joints(r_shoulder_pan_joint,l_shoulder_pan_joint) longest_valid_segment_fraction: 0.005 head: default_planner_config: "" @@ -386,6 +386,7 @@ arms_and_torso: whole_body: default_planner_config: "" planner_configs: + projection_evaluator: joints(r_shoulder_pan_joint,l_shoulder_pan_joint) - AnytimePathShortening - SBL - EST @@ -463,4 +464,4 @@ left_gripper: - LazyPRM - LazyPRMstar - SPARS - - SPARStwo \ No newline at end of file + - SPARStwo