diff --git a/CMakeLists.txt b/CMakeLists.txt index 9272fd4e2a..31ef0b667e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ add_subdirectory(doc/examples/planning_scene) add_subdirectory(doc/examples/planning_scene_ros_api) add_subdirectory(doc/examples/realtime_servo) add_subdirectory(doc/examples/robot_model_and_robot_state) +add_subdirectory(doc/how_to_guides/benchmarking) add_subdirectory(doc/how_to_guides/isaac_panda) add_subdirectory(doc/how_to_guides/kinematics_cost_function) add_subdirectory(doc/how_to_guides/parallel_planning) diff --git a/doc/examples/benchmarking/benchmarking_tutorial.rst b/doc/examples/benchmarking/benchmarking_tutorial.rst deleted file mode 100644 index 762bb428e3..0000000000 --- a/doc/examples/benchmarking/benchmarking_tutorial.rst +++ /dev/null @@ -1,177 +0,0 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - -Benchmarking -============ - -Getting Started ---------------- -If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. - -The :moveit_codedir:`benchmarking package ` provides methods to benchmark motion planning algorithms and aggregate/plot statistics using the OMPL Planner Arena. -The example below demonstrates how the benchmarking can be run for a Panda robot arm. - -Example -------- -An example is provided in the ``examples`` folder. The launch file requires a MoveIt configuration package -for the Panda robot arm available from `here `_. - -To run: - -#. Launch the panda_moveit_config ``demo.launch``: :: - - roslaunch panda_moveit_config demo.launch db:=true - -#. Within the *Motion Planning* RViz plugin, connect to the database by pressing the *Connect* button in the *Context* tab. -#. Save a scene on the *Stored Scenes* tab and name it ``Kitchen1`` by double clicking the scene in the list. -#. Move the start and goal states of the Panda arm by using the interactive markers. -#. Save an associated query for the ``Kitchen1`` scene and name the query ``Pick1``. -#. Also save a start state for the robot on the *Stored States* tab and name it ``Start1``. -#. The config file ``moveit_ros/benchmarks/examples/demo1.yaml`` refers to the scenes, queries and start states used for benchmarking. Modify them appropriately. -#. Bring down your previous launch file (``ctrl+c``). -#. Change the location ``output_directory`` to export the benchmarked files:: - - rosed moveit_ros_benchmarks demo1.yaml - -#. Run the benchmarks: :: - - roslaunch moveit_ros_benchmarks demo_panda.launch - - -Viewing Results ---------------- - -The benchmarks are executed and many interesting parameters are aggregated and written to a log file. A script (``moveit_benchmark_statistics.py``) is supplied to parse this data and plot the statistics. - -Run: :: - - rosrun moveit_ros_benchmarks moveit_benchmark_statistics.py - -To generate a PDF of plots: :: - - python moveit_benchmark_statistics.py -p - -Alternatively, upload the database file generated by ``moveit_benchmark_statistics.py`` to `plannerarena.org `_ and interactively visualize the results. - - -Parameters of the BenchmarkOptions Class ----------------------------------------- - -This class reads in parameters and options for the benchmarks to run from the ROS parameter server. The format of the parameters is assumed to be in the following form: :: - - benchmark_config: - - warehouse: - host: [hostname/IP address of ROS Warehouse node] # Default localhost - port: [port number of ROS Warehouse node] # Default 33829 - scene_name: [Name of the planning scene to use for benchmarks] # REQUIRED - - parameters: - runs: [Number of runs for each planning algorithm on each request] # Default 10 - group: [The name of the group to plan] # REQUIRED - timeout: [The maximum time for a single run; seconds] # Default 10.0 - output_directory: [The directory to write the output to] # Default is current working directory - - start_states: [Regex for the stored start states in the warehouse to try] # Default "" - path_constraints: [Regex for the path constraints to benchmark] # Default "" - queries: [Regex for the motion plan queries in the warehouse to try] # Default .* - goal_constraints: [Regex for the goal constraints to benchmark] # Default "" - trajectory_constraints: [Regex for the trajectory constraints to benchmark] # Default "" - - workspace: [Bounds of the workspace the robot plans in. This is an AABB] # Optional - frame_id: [The frame the workspace parameters are specified in] - min_corner: [Coordinates of the minimum corner of the AABB] - x: [x-value] - y: [y-value] - z: [z-value] - max_corner: [Coordinates of the maximum corner of the AABB] - x: [x-value] - y: [y-value] - z: [z-value] - - planning_pipelines: - - name: [Name of the planning pipeline used as relative parameter namespace] # REQUIRED - planners: # REQUIRED - - A list of planners - - available in the - - planning pipeline - - to benchmark the - - queries in. - - name: ... - - ... - - -Parameters of the BenchmarkExecutor Class ------------------------------------------ - -This class creates a set of ``MotionPlanRequests`` that respect the parameters given in the supplied instance of ``BenchmarkOptions`` and then executes the requests on each of the planners specified. From the ``BenchmarkOptions``, queries, ``goal_constraints``, and ``trajectory_constraints`` are treated as separate queries. If a set of ``start_states`` is specified, each query, ``goal_constraint``, and ``trajectory_constraint`` is attempted with each start state (existing start states from a query are ignored). Similarly, the (optional) set of path constraints is combined combinatorially with the start query and start ``goal_constraint`` pairs (existing ``path_constraint`` from a query are ignored). The workspace, if specified, overrides any existing workspace parameters. - -The benchmarking pipeline does not utilize ``MoveGroup``. -Instead, the planning pipelines are initialized and run directly including all specified ``PlanningRequestAdapters``. -This is especially useful for benchmarking the effects of smoothing adapters. - -It is possible to customize a benchmark run by deriving a class from ``BenchmarkExecutor`` and overriding one or more of the virtual functions. -For instance, overriding the functions ``initializeBenchmarks()`` or ``loadBenchmarkQueryData()`` allows to specify the benchmark queries directly and to provide a custom planning scene without using ROS warehouse. -An example is the custom benchmark ``CombinePredefinedPosesBenchmark`` which expects a list of predefined joint states and then creates queries for all pair-wise combinations. -See the config file ``demo_panda_predefined_poses.yaml`` for how to configure the poses. -You can run this example with: :: - - roslaunch moveit_ros_benchmarks demo_panda_predefined_poses.launch - -Additionally, a set of functions exists for ease of customization in derived classes: - -- ``preRunEvent``: invoked immediately before each call to solve -- ``postRunEvent``: invoked immediately after each call to solve -- ``plannerSwitchEvent``: invoked when the planner changes during benchmarking -- ``querySwitchEvent``: invoked before a new benchmark problem begin execution - -Note, in the above, a benchmark is a concrete instance of a ``PlanningScene``, start state, goal constraints / ``trajectory_constraints``, and (optionally) ``path_constraints``. A run is one attempt by a specific planner to solve the benchmark. - -Benchmarking of Different Motion Planners: CHOMP, STOMP and OMPL ----------------------------------------------------------------- - -This section contains the instructions for benchmarking different motion planners present in MoveIt: CHOMP, STOMP and OMPL. These planners can be compared with each other for a well defined benchmark (which is for the same environment, start states, queries and goal states). Different metrics for each of the planners can be reported to get quantitative statistics which could aid in proper selection of a particular planner in a defined environment. The statistics reported for each of the planners includes: time taken to compute the path, path length, path time, whether a valid path was found or not, etc. - -Benchmarking in a scene without obstacles -+++++++++++++++++++++++++++++++++++++++++ - -To benchmark different planners in a simple environment without obstacles, open one terminal and follow the steps in the `Example section <../benchmarking/benchmarking_tutorial.html#example>`_ at the top of this page. In the last two steps instead of editing ``demo1.yaml`` and launching ``demo_panda.launch``, edit ``demo_panda_all_planners.yaml`` and launch ``demo_panda_all_planners.launch``. - -Benchmarking in a scene with obstacles -++++++++++++++++++++++++++++++++++++++ - -To benchmark motion planners in a scene filled with obstacles, open two terminals. In the first terminal start RViz and wait for everything to finish loading: :: - - roslaunch panda_moveit_config demo.launch db:=true - -In the second terminal, run either of the two commands: :: - - rosrun moveit_tutorials collision_scene_example.py sparse - -or: :: - - rosrun moveit_tutorials collision_scene_example.py cluttered - -Now follow these steps: - -#. Within the *Motion Planning* RViz plugin, connect to the database by pressing the *Connect* button in the *Context* tab. -#. Save a scene on the *Stored Scenes* tab and name it ``ObstaclesScene`` by double clicking the scene in the list. -#. Move the start and goal states of the Panda arm by using the interactive markers. -#. Save an associated query for the ``ObstaclesScene`` scene and name the query ``Pick1``. -#. Also save a start state for the robot on the *Stored States* tab and name it ``Start1``. -#. The config file ``moveit_ros/benchmarks/examples/demo_obstacles.yaml`` refers to the scenes, queries and start states used for benchmarking. Modify them appropriately. -#. Bring down your previous launch file (``ctrl+c``). -#. Change the location ``output_directory`` to export the benchmarked files:: - - rosed moveit_ros_benchmarks demo_obstacles.yaml - -#. Run the benchmarks: :: - - roslaunch moveit_ros_benchmarks demo_panda_all_planners_obstacles.launch - -To view the results follow the same steps as listed in the `Viewing Results <../benchmarking/benchmarking_tutorial.html#viewing-results>`_ section above. After loading the database into Planner arena, different statistics can be analysed about each of the planners by choosing the required benchmark attribute from the drop down list. See image below for analysis of time taken by each of the planners to compute the solution for a sample benchmark. - -.. image:: planners_benchmark.png - :width: 700px diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 357825ec78..47c8a31b03 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -81,7 +81,6 @@ Miscellaneous .. toctree:: :maxdepth: 1 - benchmarking/benchmarking_tutorial dual_arms/dual_arms_tutorial hybrid_planning/hybrid_planning_tutorial realtime_servo/realtime_servo_tutorial diff --git a/doc/how_to_guides/benchmarking/CMakeLists.txt b/doc/how_to_guides/benchmarking/CMakeLists.txt new file mode 100644 index 0000000000..c68409aa9c --- /dev/null +++ b/doc/how_to_guides/benchmarking/CMakeLists.txt @@ -0,0 +1,6 @@ +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/how_to_guides/benchmarking/benchmarking_tutorial.rst b/doc/how_to_guides/benchmarking/benchmarking_tutorial.rst new file mode 100644 index 0000000000..ef0ccb5e67 --- /dev/null +++ b/doc/how_to_guides/benchmarking/benchmarking_tutorial.rst @@ -0,0 +1,61 @@ +How to benchmark your planning pipeline +======================================= + +Getting Started +--------------- +If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. + +The :moveit_codedir:`benchmarking package ` provides methods to benchmark a MoveIt planning pipeline and aggregate/plot statistics using the OMPL Planner Arena. +The example below demonstrates how the benchmarking can be run. + +Example +------- + +To run the example you need to install git lfs by running ``git lfs install`` and clone [moveit_benchmark_resources](https://github.com/ros-planning/moveit_benchmark_resources.git) into your workspace. + +Start the benchmarks by running: :: + + ros2 launch moveit2_tutorials run_benchmarks.launch.py + + +This will take a while depending on the settings in ``benchmarks.yaml``. The benchmark results will be saved in ``/tmp/moveit_benchmarks/``. +To introspect the benchmark data, the log files need to be converted into a database. This can be done using a script provided in the moveit_ros_benchmarks package: :: + + ros2 run moveit_ros_benchmarks moveit_benchmark_statistics.py LOG_FILE_1 ... LOG_FILE_N + +This command will create a database containing the data form all benchmark log files included. An easier way to create the database is to create it with all log files from a given repository. +For example, the argument ``/tmp/moveit_benchmarks/*`` can be used to collect all log files in the given directory into a single database. This database is created in the location where the command +above is run in a file names ``benchmark.db``. + +The database can be visualized by uploading the the file to `plannerarena.org `_ and interactively visualizing the results. + + +.. image:: planners_benchmark.png + :width: 700px + +ROS 2 parameters to configure a benchmark +----------------------------------------- + +The benchmarks are configured by a set of ROS 2 parameters. You can learn more about these parameters in the :moveit_codedir:`BenchmarkOptions.h ` file. + + +The BenchmarkExecutor Class +--------------------------- + +This class creates a set of ``MotionPlanRequests`` that respect the parameters given in the supplied instance of ``BenchmarkOptions`` and then executes the requests on each of the planners specified. From the ``BenchmarkOptions``, queries, ``goal_constraints``, and ``trajectory_constraints`` are treated as separate queries. If a set of ``start_states`` is specified, each query, ``goal_constraint``, and ``trajectory_constraint`` is attempted with each start state (existing start states from a query are ignored). Similarly, the (optional) set of path constraints is combined combinatorially with the start query and start ``goal_constraint`` pairs (existing ``path_constraint`` from a query are ignored). The workspace, if specified, overrides any existing workspace parameters. + +The benchmarking pipeline does not utilize ``MoveGroup``. +Instead, the planning pipelines are initialized and run directly including all specified ``PlanningRequestAdapters``. +This is especially useful for benchmarking the effects of smoothing adapters. + +It is possible to customize a benchmark run by deriving a class from ``BenchmarkExecutor`` and overriding one or more of the virtual functions. +For instance, overriding the functions ``initializeBenchmarks()`` or ``loadBenchmarkQueryData()`` allows to specify the benchmark queries directly and to provide a custom planning scene without using ROS warehouse. + +Additionally, a set of functions exists for ease of customization in derived classes: + +- ``preRunEvent``: invoked immediately before each call to solve +- ``postRunEvent``: invoked immediately after each call to solve +- ``plannerSwitchEvent``: invoked when the planner changes during benchmarking +- ``querySwitchEvent``: invoked before a new benchmark problem begin execution + +Note, in the above, a benchmark is a concrete instance of a ``PlanningScene``, start state, goal constraints / ``trajectory_constraints``, and (optionally) ``path_constraints``. A run is one attempt by a specific planner to solve the benchmark. diff --git a/doc/how_to_guides/benchmarking/config/benchmarking_moveit_cpp.yaml b/doc/how_to_guides/benchmarking/config/benchmarking_moveit_cpp.yaml new file mode 100644 index 0000000000..d80ba987bf --- /dev/null +++ b/doc/how_to_guides/benchmarking/config/benchmarking_moveit_cpp.yaml @@ -0,0 +1,3 @@ +planning_pipelines: + # MoveIt cpp + pipeline_names: ["ompl", "ompl_rrtc", "stomp", "pilz_industrial_motion_planner"] diff --git a/doc/how_to_guides/benchmarking/config/benchmarks.yaml b/doc/how_to_guides/benchmarking/config/benchmarks.yaml new file mode 100644 index 0000000000..63e6f14315 --- /dev/null +++ b/doc/how_to_guides/benchmarking/config/benchmarks.yaml @@ -0,0 +1,32 @@ +# A detailed description for all parameters can be found in BenchmarkOptions.h + +parameters: + name: FullBenchmark + runs: 1 + group: panda_arm # Required + timeout: 1.5 + output_directory: /tmp/moveit_benchmarks/ + start_states_regex: ready +planning_pipelines: + # Benchmark tool + pipelines: [OMPL_ANY, OMPL_RRTC, CHOMP, PILZ_LIN] + OMPL_ANY: + name: ompl + planners: + - APSConfigDefault + OMPL_RRTC: + name: ompl_rrtc + planners: + - RRTConnectkConfigDefault + CHOMP: + name: stomp + planners: + - stomp + PILZ_LIN: + name: pilz_industrial_motion_planner + planners: + - LIN + parallel_pipelines: [ompl_pilz_chomp] + ompl_pilz_chomp: + pipelines: [ompl_rrtc, stomp, pilz_industrial_motion_planner] + planner_ids: [RRTConnectkConfigDefault, stomp, LIN] diff --git a/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py b/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py new file mode 100644 index 0000000000..ebf3d07ffe --- /dev/null +++ b/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py @@ -0,0 +1,97 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + moveit_ros_benchmarks_config = ( + ParameterBuilder("moveit2_tutorials") + .yaml( + parameter_namespace="benchmark_config", + file_path="config/benchmarks.yaml", + ) + .to_dict() + ) + + moveit_configs = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines("ompl", ["ompl", "stomp", "pilz_industrial_motion_planner"]) + .moveit_cpp( + os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "benchmarking_moveit_cpp.yaml", + ) + ) + .to_moveit_configs() + ) + + # Load additional OMPL pipeline + ompl_planning_pipeline_config = { + "ompl_rrtc": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """\ + default_planner_request_adapters/AddTimeOptimalParameterization \ + 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, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["ompl_rrtc"].update(ompl_planning_yaml) + + sqlite_database = ( + get_package_share_directory("moveit_benchmark_resources") + + "/databases/panda_kitchen_test_db.sqlite" + ) + + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "benchmark_config": { + "warehouse": { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "host": sqlite_database, + "port": 33828, + "scene_name": "", # If scene name is empty, all scenes will be used + "queries_regex": ".*", + }, + }, + } + + # moveit_ros_benchmark demo executable + moveit_ros_benchmarks_node = Node( + name="moveit_run_benchmark", + package="moveit_ros_benchmarks", + executable="moveit_run_benchmark", + output="screen", + parameters=[ + moveit_ros_benchmarks_config, + moveit_configs.to_dict(), + warehouse_ros_config, + ompl_planning_pipeline_config, + ], + ) + + return LaunchDescription([moveit_ros_benchmarks_node]) diff --git a/doc/examples/benchmarking/planners_benchmark.png b/doc/how_to_guides/benchmarking/planners_benchmark.png similarity index 100% rename from doc/examples/benchmarking/planners_benchmark.png rename to doc/how_to_guides/benchmarking/planners_benchmark.png diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index 1490cceb9d..7b21efdfa9 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -22,6 +22,7 @@ Configuring and Using MoveIt persistent_scenes_and_states/persistent_scenes_and_states isaac_panda/isaac_panda_tutorial pick_ik/pick_ik_tutorial + benchmarking/benchmarking_tutorial Developing and Documenting MoveIt ---------------------------------