Add tutorial for How to Use OMPL Constrained Planning (#386)

* Add tutorial for How to Use OMPL Constrained Planning

* Reorganize code, port comments from original, use equality constraints

* Update tutorial text and media, reorganize files, and clean up

* Update .repos file for RViz Visual Tools

* Remove outdated video link

* Add description and links for projection evaluator

Co-authored-by: AndyZe <[email protected]>
(cherry picked from commit 2db5773)

Author: 2 people

.github/upstream.repos

@@ -9,7 +9,7 @@ repositories:
     version: humble
   moveit_visual_tools:
     type: git
-    url: https://github.com/ros-planning/moveit_visual_tools
+    url: https://github.com/PickNikRobotics/rviz_visual_tools.git
     version: ros2
   rosparam_shortcuts:
     type: git

CMakeLists.txt

@@ -82,6 +82,7 @@ add_subdirectory(doc/examples/moveit_cpp)
 # add_subdirectory(doc/examples/bullet_collision_checker)
 add_subdirectory(doc/examples/realtime_servo)
 add_subdirectory(doc/how_to_guides/isaac_panda)
+add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
 
 ament_export_dependencies(
   ${THIS_PACKAGE_INCLUDE_DEPENDS}

_static/videos/ompl_constraints_box.webm

@@ -63,7 +63,6 @@ Configuration
    kinematics_configuration/kinematics_configuration_tutorial
    custom_constraint_samplers/custom_constraint_samplers_tutorial
    ompl_interface/ompl_interface_tutorial
-   ompl_constrained_planning/ompl_constrained_planning
    chomp_planner/chomp_planner_tutorial
    stomp_planner/stomp_planner_tutorial
    trajopt_planner/trajopt_planner_tutorial

_static/videos/ompl_constraints_line.webm

@@ -10,4 +10,8 @@ These how-to guides will help you quickly solve specific problems using MoveIt.
    how_to_generate_api_doxygen_locally
    how_to_setup_docker_containers_in_ubuntu
    how_to_write_doxygen
+<<<<<<< HEAD
    isaac_panda/isaac_panda_tutorial
+=======
+   using_ompl_constrained_planning/ompl_constrained_planning
+>>>>>>> 110141618... Add tutorial for How to Use OMPL Constrained Planning (#386)

_static/videos/ompl_constraints_orientation.webm

@@ -0,0 +1,13 @@
+add_executable(ompl_constrained_planning
+    src/ompl_constrained_planning_tutorial.cpp)
+target_include_directories(ompl_constrained_planning
+    PUBLIC include)
+ament_target_dependencies(ompl_constrained_planning
+    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+
+install(TARGETS ompl_constrained_planning
+  DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)

_static/videos/ompl_constraints_plane.webm

@@ -0,0 +1,216 @@
+planning_plugin: ompl_interface/OMPLPlanner
+request_adapters: >-
+  default_planner_request_adapters/AddTimeOptimalParameterization
+  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
+start_state_max_bounds_error: 0.1
+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
+  FMTkConfigDefault:
+    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
+  BFMTkConfigDefault:
+    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
+  PDSTkConfigDefault:
+    type: geometric::PDST
+  STRIDEkConfigDefault:
+    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
+  BiTRRTkConfigDefault:
+    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
+  LBTRRTkConfigDefault:
+    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
+  BiESTkConfigDefault:
+    type: geometric::BiEST
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ProjESTkConfigDefault:
+    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
+  LazyPRMkConfigDefault:
+    type: geometric::LazyPRM
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  LazyPRMstarkConfigDefault:
+    type: geometric::LazyPRMstar
+  SPARSkConfigDefault:
+    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
+  SPARStwokConfigDefault:
+    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
+  TrajOptDefault:
+    type: geometric::TrajOpt
+
+panda_arm:
+  enforce_constrained_state_space: true
+  projection_evaluator: joints(panda_joint1,panda_joint2)
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault
+panda_arm_hand:
+  enforce_constrained_state_space: true
+  projection_evaluator: joints(panda_joint1,panda_joint2)
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault
+hand:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault