File size: 1,891 Bytes
4b7148d
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
"""Self-righting vs torso centre-of-mass height (design-for-self-righting, Kessens et al.).
Scripted get-up: legs push to 85 deg, then close back to 0."""
import json
import numpy as np
from multiprocessing import Pool
from model import Params
from getup import getup

def torso_com_from_foot(p):
    """Torso COM height above the torso foot (m): shell at mid-height, 2 height servos near the hip,
    electronics either centred (default) or packed with the battery at batt_z."""
    m = p.masses()
    return m  # computed below in job for clarity

def job(a):
    layout, ballast, face, mu = a
    if layout == "default":          # electronics at torso centre, battery low (current model)
        kw = dict(cells=0.046 + ballast, ups=0.013, batt_z=-0.045)
    else:                            # ALL electronics packed at the bottom with the battery
        kw = dict(cells=0.046 + 0.015 + 0.010 + 0.013 + 0.040 + ballast, board=0, pca=0, ups=0, misc=0, batt_z=-0.070)
    p = Params(mu=mu, **kw)
    r, s = getup(p, face, 85.0)
    m = s.m; tid = s.torso_id
    # torso-only COM height above the foot, measured in the standing model
    return dict(layout=layout, ballast_g=int(ballast * 1000), face=face, mu=mu, ok=r["ok"],
                final_tilt=r["final_tilt"], tilt_after_push=r["tilt_after_push"], swing_peak=r["swing_peak"])

if __name__ == "__main__":
    jobs = [(l, b, f, mu) for l in ("default", "low") for b in (0.0, 0.03, 0.06) for f in ("back", "front") for mu in (0.5, 0.8)]
    with Pool(9) as p:
        R = p.map(job, jobs)
    json.dump(R, open("../results/com_study.json", "w"), indent=1)
    for r in R:
        print(f"{r['layout']:7s} ballast {r['ballast_g']:2d}g  lying {r['face']:5s} mu {r['mu']}  -> stood up {r['ok']!s:5s}  "
              f"tilt after push {r['tilt_after_push']:6.1f}°  final {r['final_tilt']:5.1f}°  servo peak {r['swing_peak']:3.0f}%")