File size: 2,122 Bytes
573eba1
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
53
54
55
56
57
58
59
60
61
62
"""Next Balance: run the tuned fixed-PID and adaptive controllers on a scenario.

Usage:
    python inference.py --selftest
    python inference.py --dm 8 --delay 4 --noise 0.003
"""
import argparse
import json
import os
import sys

HERE = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, HERE)

import balance_core as bc  # noqa: E402


def load(path=os.path.join(HERE, "model.json")):
    with open(path, encoding="utf-8") as f:
        return json.load(f)["meta"]["params"]


def run(scenario, controller="adaptive", params=None):
    params = params or load()
    tr = bc.simulate(scenario, controller, params[controller])
    return bc.metrics(scenario, tr), tr


def _selftest():
    params = load()
    with open(os.path.join(HERE, "selftest.json"), encoding="utf-8") as f:
        cases = json.load(f)["cases"]
    worst = 0.0
    for c in cases:
        m, _ = run(c["scenario"], c["ctrl"], params)
        for k, v in c["expected"].items():
            if isinstance(v, bool):
                assert m[k] == v, (k, m[k], v)
            else:
                worst = max(worst, abs(m[k] - v))
    ok = worst < 1e-9
    print(json.dumps({"passed": ok, "cases": len(cases), "max_abs_diff": worst}))
    return ok


if __name__ == "__main__":
    ap = argparse.ArgumentParser()
    ap.add_argument("--selftest", action="store_true")
    ap.add_argument("--payload0", type=float, default=2.5)
    ap.add_argument("--dm", type=float, default=5.0)
    ap.add_argument("--fric_gain", type=float, default=2.5)
    ap.add_argument("--delay", type=int, default=3, help="actuator delay in 10 ms steps")
    ap.add_argument("--noise", type=float, default=0.002, help="sensor noise std in m")
    ap.add_argument("--seed", type=int, default=7)
    a = ap.parse_args()
    if a.selftest:
        sys.exit(0 if _selftest() else 1)
    sc = {"payload0": a.payload0, "t_load": 4.0, "dm": a.dm, "c0": 3.5, "F0": 2.0, "t_fric": 7.0,
          "fric_gain": a.fric_gain, "delay": a.delay, "noise": a.noise, "T": 12.0, "seed": a.seed}
    for ctrl in ("pid", "adaptive"):
        print(ctrl, json.dumps(run(sc, ctrl)[0]))