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"]))