updated and fixed ompl planning template

templates/moveit/ompl_planning.yaml

@@ -1,23 +1,25 @@
 # Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
-# 
+#
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
-# 
+#
 #     http://www.apache.org/licenses/LICENSE-2.0
-# 
+#
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-# 
+#
 # Source of this file are templates in https://github.com/StoglRobotics/ros_team_workspace repository.
 
 /**:
   ros__parameters:
-    move_group:
-      planning_plugins: 
+    planning_pipelines:
+      - ompl
+    ompl:
+      planning_plugins:
         - ompl_interface/OMPLPlanner
       request_adapters:
         - default_planning_request_adapters/ResolveConstraintFrames
@@ -28,61 +30,163 @@
         - default_planning_response_adapters/AddTimeOptimalParameterization
         - default_planning_response_adapters/ValidateSolution
         - default_planning_response_adapters/DisplayMotionPath
+      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()
+          longest_valid_segment_fraction: 0.5
+        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
       $ROBOT_NAME$_manipulator:
+        # projection_evaluator: joints(joint_s,joint_l)
+        longest_valid_segment_fraction: 0.005
+        default_planner_config: RRTConnect
+        default_workspace_bounds: 100
         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
-        ##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
+          - AnytimePathShortening
+          - SBL
+          - EST
+          - LBKPIECE
+          - BKPIECE
+          - KPIECE
+          - RRT
+          - RRTConnect
+          - RRTstar
+          - TRRT
+          - PRM
+          - PRMstar
+          - FMT
+          - BFMT
+          - PDST
+          - STRIDE
+          - BiTRRT
+          - LBTRRT
+          - BiEST
+          - ProjEST
+          - LazyPRM
+          - LazyPRMstar
+          - SPARS
+          - SPARStwo