Add Benchmarks for RRT

.github/workflows/build_and_test.yml

@@ -120,3 +120,7 @@ jobs:
           pixi run install_all
           pixi run build_tests
           pixi run test_all
+      # TODO: Decide on a method for automated benchmark performance testing.
+      - name: Run benchmarks
+        run: |
+          pixi run test_benchmarks

.gitignore

@@ -7,3 +7,6 @@
 # pixi environments
 .pixi
 *.egg-info
+
+# Benchmarking results
+pytest_benchmarks.json

benchmarks/test_benchmark_rrt.py

@@ -0,0 +1,95 @@
+import pytest
+import sys
+import xacro
+
+from roboplan import (
+    JointConfiguration,
+    Scene,
+    RRTOptions,
+    RRT,
+)
+
+# We don't build the bindings examples, so we just include the relative
+# directory manually.
+from pathlib import Path
+
+examples_dir = Path(__file__).parent.parent / "bindings" / "examples"
+sys.path.insert(0, str(examples_dir))
+
+from common import MODELS, ROBOPLAN_EXAMPLES_DIR
+
+
+def solve(scene: Scene, rrt: RRT, seed: int = 1234):
+    """
+    Runs an RRT test by sampling random, collision-free joint configurations
+    then attempting to plan a path between them.
+
+    Returns 1 if planning was successful, 0 otherwise.
+    """
+    scene.setRngSeed(seed)
+
+    start = JointConfiguration()
+    start.positions = scene.randomCollisionFreePositions()
+    assert start.positions is not None
+
+    goal = JointConfiguration()
+    goal.positions = scene.randomCollisionFreePositions()
+    assert goal.positions is not None
+
+    path = rrt.plan(start, goal)
+    return 0 if path is None else 1
+
+
+def solve_many(scene: Scene, rrt: RRT, iterations: int = 10):
+    """
+    Runs the specified number of iterations of RRT with a random seed.
+
+    Returns the number of successful solves.
+    """
+    successes = 0
+    for i in range(iterations):
+        successes += solve(scene, rrt, 11235 + i)
+    return successes
+
+
+def create_scene(model_name: str) -> Scene:
+    model_data = MODELS[model_name]
+    package_paths = [ROBOPLAN_EXAMPLES_DIR]
+
+    urdf_xml = xacro.process_file(model_data.urdf_path).toxml()
+    srdf_xml = xacro.process_file(model_data.srdf_path).toxml()
+
+    scene = Scene(
+        f"{model_name}_benchmark_scene",
+        urdf=urdf_xml,
+        srdf=srdf_xml,
+        package_paths=package_paths,
+        yaml_config_path=model_data.yaml_config_path,
+    )
+    return scene
+
+
+@pytest.fixture(scope="session", params=["kinova", "ur5", "franka", "dual"])
+def scene(request):
+    return create_scene(request.param)
+
+
+def test_benchmark_rrt(benchmark, scene):
+    options = RRTOptions()
+    options.max_nodes = 100000
+    options.max_planning_time = 10.0
+    rrt = RRT(scene, options)
+
+    success_rate = benchmark(solve_many, scene, rrt, 10)
+    assert success_rate >= 0.95
+
+
+def test_benchmark_rrt_connect(benchmark, scene):
+    options = RRTOptions()
+    options.max_nodes = 100000
+    options.rrt_connect = True
+    options.max_planning_time = 10.0
+    rrt = RRT(scene, options)
+
+    success_rate = benchmark(solve_many, scene, rrt, 10)
+    assert success_rate >= 0.95

pixi.toml

@@ -180,7 +180,7 @@ test_cpp = { depends-on = [
   { task = "test", args = ["roboplan_toppra"] },
 ] }
 test_all = { depends-on = ["test_py", "test_cpp"] }
-
+test_benchmarks = { cmd = "pytest benchmarks/ --benchmark-json pytest_benchmarks.json" }
 
 [dependencies]
 
@@ -227,6 +227,7 @@ compiler-rt = ">=19.1.7,<20"
 # Python test dependencies
 pytest = ">=6"
 matplotlib = ">=3.10.5,<4"
+pytest-benchmark = ">=5.1.0,<6"
 
 [pypi-dependencies]
 pycollada = ">=0.9.2, <0.10"