File size: 4,019 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
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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
"""Clear verification GIFs: two camera views, phase captions, live tilt / servo state."""
import math, sys
import numpy as np, imageio.v2 as imageio, mujoco
from PIL import Image, ImageDraw, ImageFont
from model import Params
from sim import TarsSim
from tarsai_gaits import walk_forward

CHEAP = dict(cells=0.046, ups=0.013, mu=0.8)
W, H = 480, 360
try:
    FONT = ImageFont.truetype("/System/Library/Fonts/Supplemental/Arial Bold.ttf", 20)
    SMALL = ImageFont.truetype("/System/Library/Fonts/Supplemental/Arial.ttf", 15)
except Exception:
    FONT = SMALL = ImageFont.load_default()


class Recorder:
    def __init__(self, s: TarsSim, views, fps=25):
        self.s, self.views, self.frames, self.caption, self.note = s, views, [], "", ""
        self.r = mujoco.Renderer(s.m, H, W)
        self.every = max(1, round(100 / fps))   # sim logs at 100 Hz
        s.record = True
        s._frame = self.grab                      # hook the sim's frame callback
        s.log_every_frame = True

    def grab(self):
        s = self.s
        if len(s.log["t"]) % self.every:
            return
        tiles = []
        for name, az, el, dist in self.views:
            cam = mujoco.MjvCamera(); cam.azimuth, cam.elevation, cam.distance = az, el, dist
            cam.lookat[:] = [s.d.qpos[0], s.d.qpos[1], 0.075]
            self.r.update_scene(s.d, cam)
            img = Image.fromarray(self.r.render())
            ImageDraw.Draw(img).text((10, H - 24), name, font=SMALL, fill=(30, 30, 30))
            tiles.append(np.asarray(img))
        im = Image.fromarray(np.hstack(tiles))
        d = ImageDraw.Draw(im)
        d.rectangle([0, 0, im.width, 34], fill=(20, 24, 32))
        d.text((10, 6), self.caption, font=FONT, fill=(255, 255, 255))
        servo = "SERVOS OFF" if getattr(s, "limp", False) else "servos on"
        info = f"t={s.d.time:5.1f}s  tilt {s.tilt_deg():4.1f}°  {servo}  {self.note}"
        d.text((im.width - 10 - d.textlength(info, font=SMALL), 9), info, font=SMALL,
               fill=(255, 120, 120) if getattr(s, "limp", False) else (170, 220, 255))
        self.frames.append(np.asarray(im))

    def save(self, path, speedup=1.0):
        step = max(1, int(round(speedup)))
        imageio.mimsave(path, self.frames[::step], duration=1 / 25, loop=0)
        print(path, len(self.frames[::step]), "frames")


SIDE = ("side view", 15, -8, 0.62)
THREE_Q = ("3/4 view", 135, -18, 0.75)


def lie_and_get_up():
    s = TarsSim(Params(**CHEAP)); rec = Recorder(s, [SIDE, THREE_Q])
    rec.caption = "1. Standing"; s.run(1.0)
    rec.caption = "2. Lying down (legs swing back 60°)"; s.ramp(3.0, sL=60, sR=60); s.run(0.5)
    rec.caption = "3. POWER OFF - holds by foot friction"; s.limp = True; s.run(3.0)
    rec.caption = "4. POWER ON - standing up"; s.limp = False; s.ramp(3.0, sL=0, sR=0); s.run(0.8)
    rec.caption = "5. Standing again - walking to prove it works"; walk_forward(s, 0.5, cycles=1); s.run(0.5)
    rec.save("../results/lie_down_get_up.gif", speedup=1.5)
    print("final tilt", round(s.tilt_deg(), 1))


def walk(speed, label, fname, speedup):
    s = TarsSim(Params(**CHEAP)); rec = Recorder(s, [THREE_Q, ("front view", 90, -10, 0.7)])
    y0 = s.d.qpos[1]
    rec.caption = label; s.run(0.5)
    orig = s.run
    def run_with_note(sec):
        orig(sec); rec.note = f"forward {-(s.d.qpos[1]-y0)*100:4.1f} cm"
    s.run = run_with_note
    walk_forward(s, speed); s.run(1.0)
    rec.save(f"../results/{fname}.gif", speedup=speedup)
    print(fname, "forward cm", round(-(s.d.qpos[1] - y0) * 100, 1), "final tilt", round(s.tilt_deg(), 1))


if __name__ == "__main__":
    what = sys.argv[1:] or ["lie", "walk"]
    if "lie" in what:
        lie_and_get_up()
    if "walk" in what:
        walk(0.5, "Walk - normal speed (0.5): 100% reliable in tests  [shown 3x]", "walk_normal", 3)
        walk(0.7, "Walk - faster (0.7): 1.4x faster, 92% reliable  [shown 3x]", "walk_fast", 3)
        walk(0.95, "Walk - too fast (0.95): tips over  [shown 2x]", "walk_too_fast", 2)