File size: 2,390 Bytes
81c59cd | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 | """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"]))
|