#!/usr/bin/env python3 # -*- coding: utf-8 -*- """Self-contained lightweight 2D RRT benchmark cases. Algorithms: RRT, RRT*, RRT-Connect. Outputs: JSON, CSV, Markdown, HTML with SVG visualizations. No third-party dependencies. """ from __future__ import annotations import csv import html import json import math import random import statistics import time from dataclasses import dataclass from pathlib import Path from typing import Any, Callable, Dict, List, Optional, Tuple Point = Tuple[float, float] @dataclass(frozen=True) class Rect: x1: float y1: float x2: float y2: float label: str = "" def contains(self, p: Point, margin: float = 0.0) -> bool: x, y = p return self.x1 - margin <= x <= self.x2 + margin and self.y1 - margin <= y <= self.y2 + margin @dataclass class Node: p: Point parent: int cost: float @dataclass class Scenario: name: str width: float height: float start: Point goal: Point obstacles: List[Rect] step: float = 4.5 goal_radius: float = 5.5 max_iter: int = 2600 description: str = "" class CollisionChecker: def __init__(self, scenario: Scenario, resolution: float = 1.0): self.sc = scenario self.resolution = resolution self.checks = 0 def in_free(self, p: Point) -> bool: self.checks += 1 x, y = p return 0 <= x <= self.sc.width and 0 <= y <= self.sc.height and not any(o.contains(p) for o in self.sc.obstacles) def segment_free(self, a: Point, b: Point) -> bool: self.checks += 1 d = dist(a, b) n = max(1, int(d / self.resolution)) for i in range(n + 1): t = i / n p = (a[0] * (1 - t) + b[0] * t, a[1] * (1 - t) + b[1] * t) x, y = p if not (0 <= x <= self.sc.width and 0 <= y <= self.sc.height): return False if any(o.contains(p) for o in self.sc.obstacles): return False return True def dist(a: Point, b: Point) -> float: return math.hypot(a[0] - b[0], a[1] - b[1]) def steer(a: Point, b: Point, step: float) -> Point: d = dist(a, b) if d <= step: return b return (a[0] + (b[0] - a[0]) * step / d, a[1] + (b[1] - a[1]) * step / d) def nearest(nodes: List[Node], p: Point) -> int: return min(range(len(nodes)), key=lambda i: dist(nodes[i].p, p)) def neighbors(nodes: List[Node], p: Point, radius: float) -> List[int]: return [i for i, n in enumerate(nodes) if dist(n.p, p) <= radius] def extract(nodes: List[Node], idx: int) -> List[Point]: path: List[Point] = [] while idx != -1: path.append(nodes[idx].p) idx = nodes[idx].parent return list(reversed(path)) def path_len(path: List[Point]) -> Optional[float]: if not path: return None return sum(dist(path[i], path[i + 1]) for i in range(len(path) - 1)) def shortcut(path: List[Point], cc: CollisionChecker) -> List[Point]: if not path: return path out = [path[0]] i = 0 while i < len(path) - 1: j = len(path) - 1 while j > i + 1 and not cc.segment_free(path[i], path[j]): j -= 1 out.append(path[j]) i = j return out def sample_free(rng: random.Random, sc: Scenario, cc: CollisionChecker, goal_bias: float) -> Point: if rng.random() < goal_bias: return sc.goal for _ in range(100): p = (rng.random() * sc.width, rng.random() * sc.height) if cc.in_free(p): return p return (rng.random() * sc.width, rng.random() * sc.height) def result(success: bool, path: List[Point], nodes: int, iterations: int, cc: CollisionChecker, reason: str) -> Dict[str, Any]: return { "success": success, "path": path, "path_length": path_len(path), "nodes": nodes, "iterations": iterations, "collision_checks": cc.checks, "failure_reason": None if success else reason, } def run_rrt(seed: int, sc: Scenario) -> Dict[str, Any]: rng = random.Random(100000 + seed) cc = CollisionChecker(sc) if not cc.in_free(sc.start) or not cc.in_free(sc.goal): return result(False, [], 0, 0, cc, "start_or_goal_in_collision") nodes = [Node(sc.start, -1, 0.0)] best: Optional[int] = None it = 0 for it in range(1, sc.max_iter + 1): q = sample_free(rng, sc, cc, 0.08) ni = nearest(nodes, q) np = steer(nodes[ni].p, q, sc.step) if cc.segment_free(nodes[ni].p, np): nodes.append(Node(np, ni, nodes[ni].cost + dist(nodes[ni].p, np))) idx = len(nodes) - 1 if dist(np, sc.goal) <= sc.goal_radius and cc.segment_free(np, sc.goal): nodes.append(Node(sc.goal, idx, nodes[idx].cost + dist(np, sc.goal))) best = len(nodes) - 1 break path = shortcut(extract(nodes, best), cc) if best is not None else [] return result(bool(path), path, len(nodes), it, cc, "max_iter_exhausted") def run_rrt_star(seed: int, sc: Scenario) -> Dict[str, Any]: rng = random.Random(200000 + seed) cc = CollisionChecker(sc) if not cc.in_free(sc.start) or not cc.in_free(sc.goal): return result(False, [], 0, 0, cc, "start_or_goal_in_collision") nodes = [Node(sc.start, -1, 0.0)] best: Optional[int] = None best_cost = float("inf") for it in range(1, sc.max_iter + 1): q = sample_free(rng, sc, cc, 0.10) ni = nearest(nodes, q) np = steer(nodes[ni].p, q, sc.step) if not cc.segment_free(nodes[ni].p, np): continue # Practical finite-sample radius cap for 2D examples. radius = min(18.0, max(7.0, 34.0 * math.sqrt(math.log(len(nodes) + 2) / (len(nodes) + 2)))) nbs = neighbors(nodes, np, radius) parent = ni cost = nodes[ni].cost + dist(nodes[ni].p, np) for j in nbs: c = nodes[j].cost + dist(nodes[j].p, np) if c < cost and cc.segment_free(nodes[j].p, np): parent, cost = j, c nodes.append(Node(np, parent, cost)) idx = len(nodes) - 1 for j in nbs: c = cost + dist(np, nodes[j].p) if c < nodes[j].cost and cc.segment_free(np, nodes[j].p): nodes[j].parent = idx nodes[j].cost = c if dist(np, sc.goal) <= sc.goal_radius and cc.segment_free(np, sc.goal): c = cost + dist(np, sc.goal) if c < best_cost: nodes.append(Node(sc.goal, idx, c)) best, best_cost = len(nodes) - 1, c path = shortcut(extract(nodes, best), cc) if best is not None else [] return result(bool(path), path, len(nodes), sc.max_iter, cc, "max_iter_exhausted") def extend(tree: List[Node], target: Point, sc: Scenario, cc: CollisionChecker) -> Tuple[str, int]: ni = nearest(tree, target) np = steer(tree[ni].p, target, sc.step) if not cc.segment_free(tree[ni].p, np): return "trapped", ni tree.append(Node(np, ni, tree[ni].cost + dist(tree[ni].p, np))) idx = len(tree) - 1 return ("reached" if dist(np, target) < 1e-9 else "advanced"), idx def run_rrt_connect(seed: int, sc: Scenario) -> Dict[str, Any]: rng = random.Random(300000 + seed) cc = CollisionChecker(sc) if not cc.in_free(sc.start) or not cc.in_free(sc.goal): return result(False, [], 0, 0, cc, "start_or_goal_in_collision") ta = [Node(sc.start, -1, 0.0)] tb = [Node(sc.goal, -1, 0.0)] ta_is_start = True meet: Optional[Tuple[int, int, bool]] = None it = 0 for it in range(1, sc.max_iter + 1): q = sample_free(rng, sc, cc, 0.03) status, ia = extend(ta, q, sc, cc) if status != "trapped": while True: status2, ib = extend(tb, ta[ia].p, sc, cc) if status2 == "trapped": break if dist(tb[ib].p, ta[ia].p) <= sc.step and cc.segment_free(tb[ib].p, ta[ia].p): meet = (ia, ib, ta_is_start) break if status2 == "reached": meet = (ia, ib, ta_is_start) break if meet: break ta, tb = tb, ta ta_is_start = not ta_is_start path: List[Point] = [] if meet: ia, ib, a_is_start = meet pa = extract(ta, ia) pb = extract(tb, ib) path = pa + list(reversed(pb)) if a_is_start else pb + list(reversed(pa)) path = shortcut(path, cc) return result(bool(path), path, len(ta) + len(tb), it, cc, "max_iter_exhausted") def scenarios() -> List[Scenario]: # Coordinates use an SVG-like 0..100 canvas; rectangles are closed obstacles. return [ Scenario( "open_space", 100, 100, (5, 5), (95, 95), [Rect(42, 42, 50, 50), Rect(20, 72, 28, 82)], description="Mostly open map with two small sparse rectangular obstacles.", max_iter=1600, ), Scenario( "narrow_passage", 100, 100, (8, 50), (92, 50), [Rect(47, 0, 54, 43), Rect(47, 57, 54, 100), Rect(15, 15, 28, 25), Rect(72, 75, 85, 85)], description="Vertical wall with a narrow central gate; tests passage discovery.", max_iter=3400, goal_radius=5.0, ), Scenario( "bug_trap", 100, 100, (35, 50), (90, 50), [Rect(55, 20, 61, 80), Rect(20, 20, 61, 26), Rect(20, 74, 61, 80), Rect(72, 0, 78, 42), Rect(72, 58, 78, 100)], description="U-shaped trap opening away from the goal, plus a second gate near the goal side.", max_iter=3800, goal_radius=5.0, ), Scenario( "cluttered", 100, 100, (6, 8), (94, 92), [Rect(15, 10, 24, 28), Rect(35, 6, 44, 22), Rect(61, 8, 72, 20), Rect(78, 18, 90, 30), Rect(8, 38, 20, 50), Rect(31, 34, 47, 46), Rect(58, 36, 70, 52), Rect(82, 44, 94, 58), Rect(18, 66, 32, 78), Rect(44, 62, 54, 84), Rect(68, 68, 80, 88)], description="Many rectangles create a cluttered static planning scene.", max_iter=3000, ), Scenario( "maze", 100, 100, (6, 6), (94, 94), [Rect(15, 0, 21, 72), Rect(32, 28, 38, 100), Rect(49, 0, 55, 72), Rect(66, 28, 72, 100), Rect(83, 0, 89, 72), Rect(0, 83, 18, 89), Rect(38, 11, 55, 17), Rect(72, 83, 90, 89)], description="Alternating walls form a simple maze-like corridor with a final right-side exit gap.", max_iter=5200, goal_radius=5.0, ), Scenario( "start_goal_near_obstacle", 100, 100, (12, 12), (88, 88), [Rect(14, 4, 24, 22), Rect(4, 22, 22, 30), Rect(76, 78, 86, 96), Rect(78, 70, 96, 78), Rect(42, 0, 49, 64), Rect(58, 36, 65, 100)], description="Start and goal are close to obstacle corners; tests collision margin behavior.", max_iter=3300, goal_radius=5.0, ), Scenario( "long_distance", 150, 100, (5, 5), (145, 95), [Rect(25, 20, 35, 80), Rect(60, 0, 70, 55), Rect(92, 45, 102, 100), Rect(123, 18, 133, 70)], description="Longer start-goal distance with sparse barriers over a 150x100 map.", max_iter=3600, step=6.0, goal_radius=7.0, ), ] ALGORITHMS: List[Tuple[str, Callable[[int, Scenario], Dict[str, Any]]]] = [ ("RRT", run_rrt), ("RRT*", run_rrt_star), ("RRT-Connect", run_rrt_connect), ] SEEDS = list(range(7)) def failure_summary(rows: List[Dict[str, Any]]) -> Dict[str, int]: out: Dict[str, int] = {} for r in rows: if not r["success"]: out[r["failure_reason"] or "unknown"] = out.get(r["failure_reason"] or "unknown", 0) + 1 return out def summarize(rows: List[Dict[str, Any]]) -> List[Dict[str, Any]]: summary: List[Dict[str, Any]] = [] for sc in scenarios(): for alg, _ in ALGORITHMS: rs = [r for r in rows if r["scenario"] == sc.name and r["algorithm"] == alg] succ = [r for r in rs if r["success"]] summary.append({ "scenario": sc.name, "algorithm": alg, "seed_count": len(rs), "successes": len(succ), "success_rate": len(succ) / len(rs) if rs else 0.0, "avg_planning_time_sec": statistics.mean(r["planning_time_sec"] for r in rs) if rs else None, "avg_path_length": statistics.mean(r["path_length"] for r in succ) if succ else None, "avg_nodes_or_iterations": statistics.mean(r["nodes"] for r in rs) if rs else None, "avg_iterations": statistics.mean(r["iterations"] for r in rs) if rs else None, "avg_collision_checks": statistics.mean(r["collision_checks"] for r in rs) if rs else None, "failure_reason_summary": failure_summary(rs), }) return summary def fmt(v: Any, nd: int = 3) -> str: if v is None: return "—" if isinstance(v, float): return f"{v:.{nd}f}" return str(v) def svg_for(sc: Scenario, paths: Dict[str, List[Point]]) -> str: colors = {"RRT": "#2563eb", "RRT*": "#dc2626", "RRT-Connect": "#16a34a"} sw = 1.25 if sc.width <= 110 else 1.6 parts = [ f'', f'', '', ] grid = 10 for x in range(0, int(sc.width) + 1, grid): parts.append(f'') for y in range(0, int(sc.height) + 1, grid): parts.append(f'') parts.append('') for o in sc.obstacles: parts.append(f'') parts.append(f'start') parts.append(f'goal') for name, path in paths.items(): if path: pts = " ".join(f"{x:.2f},{y:.2f}" for x, y in path) parts.append(f'{name}') parts.append('● start ● goal') parts.append('') return "\n".join(parts) def write_outputs(out: Path, rows: List[Dict[str, Any]], summary: List[Dict[str, Any]], example_paths: Dict[str, Dict[str, List[Point]]]) -> None: out.mkdir(parents=True, exist_ok=True) assets = out / "assets" assets.mkdir(exist_ok=True) scs = scenarios() for sc in scs: (assets / f"{sc.name}.svg").write_text(svg_for(sc, example_paths.get(sc.name, {})), encoding="utf-8") payload = { "benchmark_name": "lightweight_static_2d_rrt_cases", "generated_at": time.strftime("%Y-%m-%d %H:%M:%S %z"), "config": {"seeds": SEEDS, "algorithms": [a for a, _ in ALGORITHMS], "collision_resolution": 1.0}, "benchmark_notes": { "standard_benchmarks": [ "OMPL benchmark suite + Planner Arena for planner comparison databases/logs (standard in sampling-based planning research).", "MoveIt benchmarking for ROS/MoveIt planning pipelines, commonly backed by OMPL planners.", "MotionBenchMaker / motion-planning problem sets for robot motion planning research workloads.", "PythonRobotics RRT/RRT*/RRT-Connect examples: educational reference cases rather than a formal benchmark suite.", ], "current_scope": "Dependency-light static 2D geometric tests, suitable for CI/smoke/regression tests rather than publication-grade robotics benchmarking.", }, "scenarios": [sc.__dict__ | {"obstacles": [o.__dict__ for o in sc.obstacles]} for sc in scs], "summary": summary, "runs": rows, } (out / "rrt_benchmark_results.json").write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8") fieldnames = ["scenario", "algorithm", "seed", "success", "planning_time_sec", "path_length", "nodes", "iterations", "collision_checks", "failure_reason"] with (out / "rrt_benchmark_results.csv").open("w", newline="", encoding="utf-8") as f: w = csv.DictWriter(f, fieldnames=fieldnames) w.writeheader() w.writerows([{k: r.get(k) for k in fieldnames} for r in rows]) md_rows = "\n".join( f"| {s['scenario']} | {s['algorithm']} | {s['success_rate']*100:.1f}% ({s['successes']}/{s['seed_count']}) | {fmt(s['avg_planning_time_sec'],4)} | {fmt(s['avg_path_length'],2)} | {fmt(s['avg_nodes_or_iterations'],1)} | {fmt(s['avg_collision_checks'],1)} | {s['failure_reason_summary'] or '—'} |" for s in summary ) scenario_list = "\n".join(f"- `{sc.name}`: {sc.description}" for sc in scs) md = f"""# RRT Benchmark 测试案例扩展报告 本报告扩展了轻量静态 2D RRT 测试集，覆盖 `{', '.join(sc.name for sc in scs)}` 共 {len(scs)} 个场景；每个 `scenario × algorithm` 使用 {len(SEEDS)} 个 seed。脚本仅依赖 Python 标准库，适合作为快速回归/冒烟测试。 ## 标准 benchmark 调研结论 - **OMPL benchmark + Planner Arena**：采样式运动规划领域最标准、最可复用的 benchmark/log 分析组合，适合严肃比较 RRT/RRT*/RRT-Connect/PRM/KPIECE 等规划器；需要 OMPL 依赖，Planner Arena 可分析 OMPL benchmark log。 - **MoveIt benchmarking / ROS planning pipeline benchmark**：适合机械臂/机器人模型、场景和规划流水线比较；依赖 ROS/MoveIt/OMPL，较重。 - **MotionBenchMaker / 机器人运动规划问题集**：更接近机器人任务级 benchmark，适合后续扩展到真实 robot scene；依赖和数据集维护成本高于本次轻量测试。 - **PythonRobotics path planning examples**：有 RRT、RRT*、RRT-Connect 等教学示例，适合参考实现和简单 2D 案例；它本身更像示例集合，不是严格标准 benchmark。 - **常见轻量 2D 场景**：open space、narrow passage、bug trap、cluttered、maze-like、start/goal near obstacle、long-distance sparse barriers；适合当前页面展示和 CI 回归。 ## 新增场景 {scenario_list} ## 算法与指标 算法：RRT、RRT*、RRT-Connect。指标：success_rate、avg_planning_time_sec、avg_path_length、avg_nodes_or_iterations、avg_iterations、collision_checks、failure_reason_summary。 | scenario | algorithm | success_rate | avg_time_sec | avg_path_length | avg_nodes_or_iterations | avg_collision_checks | failure_reason_summary | |---|---|---:|---:|---:|---:|---:|---| {md_rows} ## 可视化 每个场景有 seed=0 的路径示意 SVG，颜色：蓝色 RRT、红色 RRT*、绿色 RRT-Connect，黑色为障碍物。 """ for sc in scs: md += f"\n### {sc.name}\n\n![{sc.name}](assets/{sc.name}.svg)\n" (out / "RRT_BENCHMARK_REPORT.md").write_text(md, encoding="utf-8") rows_html = "\n".join( f"{html.escape(s['scenario'])}{html.escape(s['algorithm'])}{s['success_rate']*100:.1f}% ({s['successes']}/{s['seed_count']}){fmt(s['avg_planning_time_sec'],4)}{fmt(s['avg_path_length'],2)}{fmt(s['avg_nodes_or_iterations'],1)}{fmt(s['avg_collision_checks'],1)}{html.escape(str(s['failure_reason_summary'] or '—'))}" for s in summary ) cards = "\n".join( f"

{html.escape(sc.name)}

{html.escape(sc.description)}

{html.escape(sc.name)} RRT paths
" for sc in scs ) html_doc = f""" RRT Benchmark 测试案例

RRT Benchmark 测试案例

轻量静态 2D benchmark：{len(scs)} 个场景 × {len(ALGORITHMS)} 个算法 × {len(SEEDS)} 个 seed。算法包括 RRT、RRT*、RRT-Connect。

RRTRRT*RRT-Connect

标准 benchmark 简述

指标摘要

{rows_html}
scenarioalgorithmsuccess_rateavg_time_secavg_path_lengthavg_nodes_or_iterationsavg_collision_checksfailure_reason_summary

场景可视化

{cards}

下载产物

""" (out / "index.html").write_text(html_doc, encoding="utf-8") def main() -> None: out = Path(__file__).resolve().parent rows: List[Dict[str, Any]] = [] example_paths: Dict[str, Dict[str, List[Point]]] = {} for sc in scenarios(): example_paths[sc.name] = {} for seed in SEEDS: for alg, fn in ALGORITHMS: t0 = time.perf_counter() r = fn(seed, sc) elapsed = time.perf_counter() - t0 rows.append({ "scenario": sc.name, "algorithm": alg, "seed": seed, "success": bool(r["success"]), "planning_time_sec": elapsed, "path_length": r["path_length"], "nodes": r["nodes"], "iterations": r["iterations"], "collision_checks": r["collision_checks"], "failure_reason": r["failure_reason"], }) if seed == 0: example_paths[sc.name][alg] = r["path"] summary = summarize(rows) write_outputs(out, rows, summary, example_paths) print(json.dumps({"summary": summary, "out": str(out)}, ensure_ascii=False, indent=2)) if __name__ == "__main__": main()