A deterministic, reproducible benchmark suite that compares five classical motion-planning algorithms on a Franka Panda robot in PyBullet.
- Straight-line (baseline)
- RRT (single-tree)
- RRT-Connect (bi-directional)
- PRM (eager roadmap)
- Lazy-PRM (deferred edge checks)
The benchmark measures solver-only runtime, success-rate, path smoothness, obstacle clearance, and memory overhead. Every run appends to a CSV log for longitudinal tracking.
1. Scene Build 2. Pair Generation 3. Batch Benchmark 4. Analysis
┌──────────────┐ ┌───────────────────────┐ ┌──────────────────────────┐ ┌───────────────────┐
│ simEnv.py │ ➔ │ motion_test.py │ ➔ │ benchmark_runner.py │ ➔ │ Jupyter / Excel │
│ • shelf URDF │ saves │ • collision-free IK │ │ • five planners │ │ visual charts │
│ • markers │ env │ • 6 start/goal pairs │ │ • metrics → CSV │ │ & reports │
└──────────────┘ └────────────────────── ┘ └──────────────────────────┘ └───────────────────┘
1. Scene Build (simEnv.py)
- Loads a two-tier shelf URDF.
- Sets gravity and lighting.
- Positions the Franka Panda robot at
[0, -0.6, 0]usingPandaSim.
2. Pair Generation (motion_test.py)
- Defines 6 pick-up grid points and 6 shelf target points.
- Uses
safe_ik()for collision-free IK solutions. - A neutral configuration is used as the IK seed:
[0, -0.6, 0, -1.8, 0, 2.2, 0.8]to avoid stuck states. - Multiple yaw angles from
[-60°, …, +60°]improve reachability. - Valid configurations are saved to
benchmark_pairs.json.
3. Batch Benchmark (benchmark_runner.py)
- Loads the precomputed start/goal pairs.
- Runs 5 planners on each pair.
- Logs metrics to
benchmark_results.csv.
4. Analysis
- Use Jupyter/Excel to compare planners over time and visualize results.
➡️ All planners use identical input pairs.
Each planner is tested under two scenarios:
- With side walls – tighter constraints.
- Without side walls – more open space.
Each test covers 6 start/goal pairs.
| File | Purpose |
|---|---|
motion_planning.py |
Implements the five planners and holds global hyper-parameters (DEFAULT_MAX_ITER, DEFAULT_STEP_SIZE, DEFAULT_NUM_SAMPLES, DEFAULT_K). Provides helpers execute_motion_plan, get_movable_joints. |
simEnv.py |
Builds the shelf scene, registers obstacles, and exposes a pin-hole depth camera (unused in this benchmark but ready for future perception tasks). |
panda_sim_grasp_arm.py |
Loads the Franka URDF, synchronises gripper joints, supplies inverse-kinematics helpers and Cartesian motion primitives for interactive tests. |
motion_test.py |
Interactive script to generate six start/goal joint pairs via collision-aware IK (--regen) and preview any single planner in a GUI. Serialises the pairs to benchmark_pairs.json. |
benchmark_runner.py |
Batch benchmark driver – loops over all five planners and all six pairs, captures metrics, updates benchmark_results.csv. GUI/playback toggle via the VISUALISE flag. |
benchmark_pairs.json |
Six pre-computed collision-free start/goal joint lists (auto-generated). |
benchmark_results.csv |
Growing log of every benchmark invocation (timestamped rows). |
| File | Folder | What it does | Present use |
|---|---|---|---|
test_rrt_plan.py |
root | Manual sandbox that spawns the Panda + shelf, runs only the RRT planner over a 6×6 grid, and visualises each attempt step-by-step. Great for demos and for checking new start/goal pairs. | Experimental |
vision_based_grasping_old.py |
root | Legacy perception pipeline: depth camera → GG-CNN grasp detection → Panda pick-and-place loop. Depends on utils camera.py & tool.py. |
Future vision research |
pytourch_verify.py, misc. |
root | One-off experiments (network checks, mesh viewers). | Scratch / ignore for paper |
camera.py |
utils/ | Maths helpers to convert depth pixels ⇄ 3‑D rays, intrinsic matrix builder. | Only used by vision scripts. |
tool.py |
utils/ | Small point‑cloud & image utilities (depth2Gray3, line drawing, etc.). |
Only used by vision scripts. |
Essential benchmark files:
motion_planning.py,simEnv.py,panda_sim_grasp_arm.py,motion_test.py,benchmark_runner.py.
| Constant | Planner Types | Description | Default |
|---|---|---|---|
DEFAULT_MAX_ITER |
RRT, RRT-Connect | Max tree size | 5000 |
DEFAULT_STEP_SIZE |
All | Step size in radians | 0.05 |
DEFAULT_NUM_SAMPLES |
PRM, Lazy-PRM | Number of roadmap samples | 300 |
DEFAULT_K |
PRM, Lazy-PRM | Number of K-nearest connections | 8 |
The benchmark suite reports key performance indicators after each planner run. These metrics help quantify planning efficiency, path quality, and runtime behavior.
| Metric | What It Tells You |
|---|---|
success_rate% |
Fraction of tested start-goal pairs where a valid, collision-free path was found. 100% means the planner succeeded in all cases. |
plan_time_s |
Average solver-only time to compute a path (not including simulation or IK time). Faster values indicate computational efficiency. |
exec_time_s |
Simulated execution time = number of waypoints × simulation timestep (1/240s). Shorter paths execute faster. |
smoothness |
A lower value means a more natural, less jerky joint-space trajectory. Computed as ∑‖qₖ₊₁ − 2qₖ + qₖ₋₁‖. |
clearance_m |
Closest distance to any obstacle during motion. Negative = collision. Higher positive values are safer. |
mem_MB |
Average increase in system memory usage during planning. Helps identify planners with excessive memory needs. |
| Package | Version | Purpose |
|---|---|---|
python |
3.9–3.11 | Runtime interpreter |
pybullet |
≥ 3.4 | Physics simulator |
pybullet_planning |
≥ 0.7.2 | Motion planners wrapper |
numpy |
≥ 1.24 | Numerical operations |
psutil |
≥ 5.9 | Memory profiling (optional) |
matplotlib |
≥ 3.8 | Plotting (optional) |
| Package | Role |
|---|---|
opencv |
Image display and annotations |
torch |
Runs GG-CNN network |
scipy |
Adds simulation noise |
skimage |
Depth preprocessing |
# Core setup
pip install pybullet pybullet_planning numpy psutil matplotlib
# Vision tools (optional)
pip install opencv-python torch scipy scikit-imagegit clone https://github.com/<you>/<repo>.git
cd <repo>
python motion_test.py --regen # Generate joint pairs
python benchmark_runner.py # Run benchmark & log resultsUse VISUALISE=True to enable GUI playback.
To ensure identical results:
random.seed(42)
np.random.seed(42)
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)These lock randomness and simulation steps to enable fully deterministic outcomes.
| Planner | Success % | Mean planning time (s) | Mean playback time (s) | Smoothness ↑ | Min. clearance (m) | Extra RAM (MB) |
|---|---|---|---|---|---|---|
| RRT | 100 | 0.103 | 0.078 | 2.54 | −0.001 | 0.156 |
| Lazy‑PRM | 100 | 1.492 | 0.016 | 6.08 | −0.001 | 0.000 |
| PRM (eager) | 100 | 9.628 | 0.019 | 7.95 | −0.001 | 0.000 |
| RRT‑Connect | 100 | 0.105 | 0.075 | 2.80 | −0.001 | 0.000 |
| Straight‑Line | 33.3 | 0.010 | 0.175 | 0.00 | −0.001 | 0.000 |
↑ “Smoothness” is the summed squared curvature; lower = smoother.
- Four planners hit 100 % reliability on all six tasks.
- Straight‑line motion succeeded only twice; it works iff the joint‑linear path is unobstructed, making it a baseline sanity check.
- RRT and RRT‑Connect are >10× faster than either PRM variant while keeping perfect success. RRT‑Connect edged out RRT by ~2 ms, well within run‑to‑run noise.
- Lazy‑PRM cut its roadmap from 3.3 s (previous run) to 1.5 s — still an order of magnitude slower than RRT / RRT‑Connect.
- Eager PRM remains the slowest at ~9.6 s, because every edge is collision‑ checked during construction.
Playback time is proportional to path length. All sampling planners produce paths that run in ≈0.08 s except Straight‑Line, whose detour around the shelf takes 0.175 s when it succeeds.
- RRT‑Connect delivers the smoothest of the successful planners (2.80), closely followed by RRT (2.54).
- PRM paths are the most jagged (≈8) despite longer roadmap build time; Lazy‑PRM sits in the middle (6.1).
All planners report the same −1 mm minimum distance. This is a numerical
quirk of the getClosestPoints query (see code comments): the robot brushes
—but does not penetrate— the shelf surface within PyBullet’s floating‑point
resolution. No true collisions occurred.
Peak additional RSS never exceeded 0.2 MB per solve. Memory is therefore irrelevant for this workload.
- Speed vs reliability trade‑off is nonexistent for RRT‑Connect in this scene: it is simultaneously the fastest, smoothest, and perfectly reliable.
- Lazy‑PRM’s success dipped to 83 % in an earlier run; with a different
random seed it regained 100 % but still incurs multi‑second build cost.
Reducing
num_samples(e.g. 200 nodes) would cut that time dramatically. - Straight‑Line remains a valuable lower bound: any case it solves is effectively trivial for all other planners.
- Best all‑round choice: RRT‑Connect — delivers solutions in ~0.1 s with the smoothest joint curves and full reliability.
- RRT is nearly indistinguishable in this setup; choose it if a simpler single‑tree implementation is preferred.
- Lazy‑PRM is viable when roadmap reuse across dozens of goals can amortise its 1–3 s upfront cost.
- Eager PRM offers no advantage here—slower and no smoother—so can be dropped from production runs.
Ideas for further development:
- Parameter sweeps & statistical comparisons
- Online vision-in-the-loop grasping
- Extending to cluttered or dynamic scenes
Apache 2.0
@misc{panda_planner_benchmark2025,
author = {Your Name},
title = {A Reproducible Benchmark of Classical Motion–Planning Algorithms on a Franka Panda},
year = 2025,
howpublished = {GitHub},
url = {https://github.com/<you>/<repo>}
}