Skip to content

Commit 34f4577

Browse files
zkingstonChamzasKonstantinos
zkingston
and
ChamzasKonstantinos
authored
Benchmarking Overhaul + GNUPlotHelper (#216)
* Working on planning profiler * simple profiler example + GNUPlot helper * Added example box domain + more plotting options * basic live plotting example, instance based plotting * documentation + additional example * live rviz visualization example, script options * doc * remove cond * Result -> PlanData * revamp of benchmarker to experiment * updates for OMPL * documentation + API changes * kinetic fix * starting benchmarking documentation * addressing comments * various metric improvements * resources instead of fetch * fixes to OMPL pipeline refresh for benchmarking * change order of prerun + public refreshContext * doc update * documentation * version bump Co-authored-by: Constantinos Chamzas <chamzask@gmail.com>
1 parent b6334e5 commit 34f4577

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

57 files changed

+3615
-570
lines changed

‎.docs/markdown/benchmarking.md‎

Lines changed: 224 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,224 @@
1+
# Benchmarking Planners in Robowflex {#benchmarking}
2+
3+
Robowflex makes it easy to profile and benchmark motion planners.
4+
The primary tool used is `robowflex::Profiler`, which profiles and collects metric data on a motion planning run, producing `robowflex::PlanData`.
5+
`robowflex::Experiment` uses `robowflex::Profiler` on a collection of motion planning queries (`robowflex::PlanningQuery`) to generate a `robowflex::PlanDataSet`.
6+
All relevant classes are found in `benchmarking.h`.
7+
8+
For the following documentation, assume we have loaded a Fetch robot and a basic scene and planner, such as this:
9+
```cpp
10+
// ...
11+
#include <robowflex_library/benchmarking.h>
12+
#include <robowflex_ompl/ompl_interface.h>
13+
// ...
14+
15+
auto fetch = std::make_shared<FetchRobot>();
16+
fetch->initialize();
17+
18+
auto scene = std::make_shared<Scene>(fetch);
19+
20+
// Create the OMPL interface planner (from robowflex_ompl), as it has access to planner progress properties
21+
auto planner = std::make_shared<OMPL::OMPLInterfacePlanner>(fetch);
22+
planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml");
23+
```
24+
25+
## Profiler
26+
27+
`robowflex::Profiler` is used with `robowflex::Experiment`, but can be used standalone to manually profile a single run.
28+
Here is a high-level example of creating a profiler and then profiling a plan:
29+
```cpp
30+
// Create the profiler
31+
Profiler profiler;
32+
33+
// ... customize profiler here ...
34+
35+
// Setup options for profiling run, look at documentation for full list
36+
Profiler::Options options;
37+
38+
// What built-in metrics to compute at the end of the run
39+
options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::SMOOTHNESS;
40+
41+
// Profiler results are stored in this PlanData.
42+
PlanData result;
43+
44+
// Run and profile the plan.
45+
if (profiler.profilePlan(planner, scene, request->getRequest(), options, result))
46+
RBX_INFO("Planning success!");
47+
else
48+
RBX_ERROR("Planning failure!");
49+
50+
// ... look at results here ...
51+
```
52+
Take a look at the documentation for `robowflex::Profiler` for further options.
53+
Additionally, look at `robowflex::PlanData` for what information is computed by default by the profiler, such as planning time.
54+
For an in-depth example of how to use `robowflex::Profiler`, look at `robowflex_ompl/scripts/fetch_profile.cpp`.
55+
56+
### Planning Metrics
57+
58+
An important feature of the profiler is that you can add custom metrics to compute at the end of a run.
59+
After adding a custom metric, these are automatically computed and added to the `robowflex::PlanData`.
60+
These metrics can also be added to the profiler used inside of a `robowflex::Experiment` (using `robowflex::Experiment::getProfiler()`), to add metrics in a benchmark.
61+
62+
Here is an example of a metric that returns distance-to-go to the goal (using `robowflex::OMPL::OMPLInterfacePlanner`, as above).
63+
The resulting metric will be stored in `robowflex::PlanData::metrics` under the name "goal_distance".
64+
65+
```cpp
66+
// ... create profiler ...
67+
68+
profiler.addMetricCallback("goal_distance", //
69+
[](const PlannerPtr &planner, //
70+
const SceneConstPtr &scene, //
71+
const planning_interface::MotionPlanRequest &request, //
72+
const PlanData &run) -> PlannerMetric {
73+
const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);
74+
const auto &pdef = ompl_planner->getLastSimpleSetup()->getProblemDefinition();
75+
return pdef->getSolutionDifference();
76+
};
77+
);
78+
79+
// ... profile plan ....
80+
```
81+
82+
Note that planning metrics are stored as a boost variant type.
83+
In order to extract the underlying data, you will have to do something like the following:
84+
```cpp
85+
PlanData result;
86+
87+
// ... profile goes here ...
88+
89+
double goal_distance = boost::get<double>(result.metrics["goal_distance"]);
90+
```
91+
92+
### Planner Progress Properties
93+
94+
Some planners also expose _progress properties_, which are planner metrics that change over the course of a planning run.
95+
A common use-case for these properties is to profile asymptotically optimal planners (such as RRT*) with properties such as the best path cost so far or number of iterations.
96+
The profiler automatically includes whatever progress properties are exposed through the planner itself, that is, through the `robowflex::Planner::getProgressProperties()` function.
97+
If you are using `robowflex_ompl` `robowflex::OMPL::OMPLInterfacePlanner`, this will return the underlying OMPL planner's progress properties.
98+
The profiler captures progress properties by spinning up a separate thread that queries the planner for each property at a specified update rate.
99+
100+
There are a number of options associated with the progress properties, look at the documentation for more information:
101+
```cpp
102+
Profiler::Options options;
103+
options.progress = true;
104+
options.progress_at_least_once = true;
105+
options.progress_update_rate = 0.1;
106+
```
107+
108+
You can also specify custom progress properties for the profiler to use.
109+
Progress properties are specified through progress property allocator functions, which generate the query at the start of the profiling run, so that the function can be customized to the specifics of the query.
110+
Here is an example of a progress property that counts the number of vertices in the OMPL planning graph:
111+
```cpp
112+
// ... create profiler ...
113+
114+
profiler.addProgressAllocator("num vertices INTEGER", //
115+
[](const PlannerPtr &planner, //
116+
const SceneConstPtr &scene, //
117+
const planning_interface::MotionPlanRequest &request) -> Planner::ProgressProperty {
118+
const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);
119+
const auto &ss = ompl_planner->getLastSimpleSetup();
120+
const auto &op = ss->getPlanner();
121+
122+
return [op] {
123+
ompl::base::PlannerData pd(op->getSpaceInformation());
124+
op->getPlannerData(pd);
125+
126+
return std::to_string(pd.numVertices());
127+
};
128+
});
129+
130+
// ... profile plan ....
131+
```
132+
133+
There are a few things to note.
134+
First, if there are any progress properties captured, there is also a progress property called "time REAL" that is captured in tandem, which is the time that the progress property was captured in seconds since planning started.
135+
Second, all progress properties are strings.
136+
Third, all progress property names must include their datatype at the end of their name.
137+
These are datatypes supported by SQL, e.g., "INTEGER", "VARCHAR(128)", etc.
138+
This is to tell the benchmark experiment output functions how to encode the data.
139+
140+
Say you wanted to extract the "num vertices INTEGER" in a usable format, such as a time series.
141+
You could do something like the following:
142+
```cpp
143+
PlanData result;
144+
145+
// ... profile goes here ...
146+
147+
std::vector<double> times;
148+
std::vector<int> verts;
149+
for (const auto &sample : result.progress)
150+
{
151+
times.emplace_back(boost::lexical_cast<double>(sample["time REAL"]));
152+
verts.emplace_back(boost::lexical_cast<int>(sample["num vertices INTEGER"]));
153+
}
154+
```
155+
156+
You can also add simple callback functions to the profiler that are called at the same rate as the progress properties, e.g.,
157+
```cpp
158+
profiler.addProgressCallback([](const PlannerPtr &planner, //
159+
const SceneConstPtr &scene, //
160+
const planning_interface::MotionPlanRequest &request, //
161+
const PlanData &result) {
162+
std::cout << "Wow! A callback!" << std::endl;
163+
});
164+
```
165+
166+
## Experiment
167+
168+
`robowflex::Experiment` is a wrapper that profiles a number of motion planning queries and collects all data into a `robowflex::PlanDataSet`.
169+
It is simple to setup and run an experiment:
170+
```cpp
171+
Profiler::Options options;
172+
// ... setup options ...
173+
174+
Experiment experiment( //
175+
"demo", // Name of the experiment. This is encoded in output files so experiments can be told apart.
176+
options, // Options used by the underlying profiler. Can be modifed later by using Experiment::getOptions().
177+
60.0, // Total time allotted to each query. This overrides whatever is in the request, unless you call
178+
// Experiment::overridePlanningTime().
179+
100, // Total number of trials that each query should be profiled for.
180+
false // (Optional, defaults to false) If true, the benchmarker will re-run each query until the *total*
181+
// time has exceeded the allotted time. That is, say 5 seconds are allotted to a query. The
182+
// planner solves the problem in 3 seconds. The benchmarker will then re-run the same query but
183+
// only with a time budget of 2 seconds.
184+
);
185+
186+
// Add a named query to the experiment.
187+
// These consist of the planning scene, the motion planner, and the request.
188+
// Note that you can add *multiple* queries under the same name - the experiment will collect the data under
189+
// the same name if this is the case.
190+
// This is useful if you have variations on a planning scene.
191+
experiment.addQuery("basic", scene, planner, request);
192+
193+
// ... add pre & post run callbacks to experiment ...
194+
experiment.setPreRunCallback([&](const PlannerQuery &query) {
195+
std::cout << "Running query " << query.name << std::endl;
196+
});
197+
198+
Profiler &profiler = experiment.getProfiler();
199+
// ... modify profiler, e.g., add custom metrics ...
200+
201+
// Run Benchmark!
202+
PlanDataSetPtr dataset = experiment.benchmark(1);
203+
```
204+
205+
Note that you can add pre- and post-run callbacks to the experiment that run for every trial.
206+
There is also post-query callback that is called after a run's data has been entered into the dataset.
207+
208+
Finally, you can output `robowflex::PlanDataSet` to a number of output file types, all of which inherit from `robowflex::PlanDataSetOutputter`, e.g.,
209+
```cpp
210+
OMPLPlanDataSetOutputter output("results");
211+
output.dump(*dataset);
212+
```
213+
The resulting log files from `robowflex::OMPLPlanDataSetOutputter` can be used with OMPL's `ompl_benchmark_statistics.py` script to generate benchmarking databases.
214+
See [the OMPL documentation for benchmarking](http://ompl.kavrakilab.org/benchmark.html) for more information.
215+
The resulting database can be viewed online at [plannerarena.org](http://plannerarena.org/).
216+
217+
For an in-depth example of how to use `robowflex::Experiment`, look at:
218+
- `robowflex_library/scripts/fetch_benchmark.cpp`.
219+
- `robowflex_library/scripts/fetch_scenes_benchmark.cpp`.
220+
- `robowflex_ompl/scripts/fetch_ompl_benchmark.cpp`.
221+
- `robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp`.
222+
223+
**IMPORTANT** If you are using the `robowflex_ompl` `robowflex::OMPL::OMPLInterfacePlanner` you cannot benchmark with multiple threads, as the underlying planner is context sensitive.
224+
Use only one thread if profiling queries that contain this planner.

‎robowflex/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>robowflex</name>
4-
<version>1.2.0</version>
4+
<version>1.3.0</version>
55
<description>Robowflex: Making MoveIt Easy!</description>
66

77
<maintainer email="zak@rice.edu">Zachary Kingston</maintainer>

‎robowflex_dart/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>robowflex_dart</name>
4-
<version>1.2.0</version>
4+
<version>1.3.0</version>
55
<description>Robowflex: DART Planning Tools</description>
66

77
<maintainer email="zak@rice.edu">Zachary Kingston</maintainer>

‎robowflex_library/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,7 @@ list(APPEND SOURCES
7777
src/io/colormap.cpp
7878
src/io/broadcaster.cpp
7979
src/io/hdf5.cpp
80+
src/io/gnuplot.cpp
8081
src/pool.cpp
8182
src/tf.cpp
8283
src/random.cpp

0 commit comments

Comments
 (0)