Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Port simplified benchmarking tutorial #772

Merged
merged 6 commits into from
Sep 27, 2023
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
177 changes: 0 additions & 177 deletions doc/examples/benchmarking/benchmarking_tutorial.rst

This file was deleted.

1 change: 0 additions & 1 deletion doc/examples/examples.rst
Original file line number Diff line number Diff line change
@@ -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
6 changes: 6 additions & 0 deletions doc/how_to_guides/benchmarking/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
install(DIRECTORY config
DESTINATION share/${PROJECT_NAME}
)
61 changes: 61 additions & 0 deletions doc/how_to_guides/benchmarking/benchmarking_tutorial.rst
Original file line number Diff line number Diff line change
@@ -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 </doc/tutorials/getting_started/getting_started>`.

The :moveit_codedir:`benchmarking package <moveit_ros/benchmarks>` 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 <http://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 <moveit_ros/benchmarks/include/moveit/benchmarks/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.
sjahr marked this conversation as resolved.
Show resolved Hide resolved

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.
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
planning_pipelines:
# MoveIt cpp
pipeline_names: ["ompl", "ompl_rrtc", "stomp", "pilz_industrial_motion_planner"]
32 changes: 32 additions & 0 deletions doc/how_to_guides/benchmarking/config/benchmarks.yaml
Original file line number Diff line number Diff line change
@@ -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]
97 changes: 97 additions & 0 deletions doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py
Original file line number Diff line number Diff line change
@@ -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])
1 change: 1 addition & 0 deletions doc/how_to_guides/how_to_guides.rst
Original file line number Diff line number Diff line change
@@ -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
---------------------------------