"""Reproduce the four curated cases and a seeded random comparison.""" import json import random from pathlib import Path from math import hypot from servo import DEMOS, clearance, direct, forward, plan, safe_edge def evaluate(case): start, goal, obstacle = case route = plan(*case) result = {"direct_accepted": direct(*case), "planner_accepted": bool(route)} if route: error = hypot(*(a-b for a,b in zip(forward(route[-1])[1],goal))) assert error < 1e-9 assert all(safe_edge(a,b,obstacle) for a,b in zip(route,route[1:])) result.update(endpoint_error=error, edges=len(route)-1, minimum_waypoint_clearance=min(clearance(q,obstacle) for q in route)) return result if __name__ == "__main__": suites = {} for seed in (19, 73, 149): rng = random.Random(seed) cases = [] while len(cases) < 80: start, goal = [(rng.uniform(-1.55,1.55),rng.uniform(.2,1.6)) for _ in range(2)] if max(hypot(*p) for p in (start,goal)) >= 1.9: continue obstacle = (rng.uniform(-1,1), rng.uniform(.25,1.3), rng.uniform(.10,.22)) cases.append((start,goal,obstacle)) suites[str(seed)] = {"cases": cases, "results": [evaluate(c) for c in cases]} edge_cases = { "clear_transfer": (((1,.5),(.8,.8),(-1,-1,.15)), True), "blocked_pickup": (((1,.5),(.8,.8),(1,.5,.15)), False), "blocked_drop": (((1,.5),(.8,.8),(.8,.8,.15)), False), "blocked_base": (((1,.5),(.8,.8),(0,0,.15)), False), } checks = {} for name, (case, expected) in edge_cases.items(): result = evaluate(case) assert result["planner_accepted"] == expected, name assert result["direct_accepted"] == expected, name checks[name] = result data = {"curated": [evaluate(c) for c in DEMOS], "suites": suites, "edge_cases": checks} Path("evaluation.json").write_text(json.dumps(data, indent=2), encoding="utf-8") for name, rows in [("curated", data["curated"]), *[ (seed, suite["results"]) for seed, suite in suites.items() ]]: print(name, len(rows), sum(x["planner_accepted"] for x in rows), sum(x["direct_accepted"] for x in rows), max(x["endpoint_error"] for x in rows if x["planner_accepted"]))