macbot 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- macbot/__init__.py +3 -0
- macbot/cameras/__init__.py +239 -0
- macbot/cameras/swift/uvc_stream.swift +83 -0
- macbot/cli.py +179 -0
- macbot/collect/__init__.py +0 -0
- macbot/collect/checks.py +36 -0
- macbot/collect/server.py +371 -0
- macbot/config.py +56 -0
- macbot/data/__init__.py +0 -0
- macbot/data/export.py +116 -0
- macbot/data/health.py +67 -0
- macbot/default.yaml +55 -0
- macbot/doctor.py +86 -0
- macbot/i18n.py +10 -0
- macbot/policy/__init__.py +0 -0
- macbot/policy/bench.py +28 -0
- macbot/policy/executors.py +222 -0
- macbot/policy/gripper.py +47 -0
- macbot/policy/models.py +158 -0
- macbot/policy/runner.py +345 -0
- macbot/robot/__init__.py +3 -0
- macbot/robot/fake.py +63 -0
- macbot/robot/gravity.py +108 -0
- macbot/robot/rebot.py +223 -0
- macbot/system.py +97 -0
- macbot/viz/__init__.py +0 -0
- macbot/viz/replay.py +113 -0
- macbot/viz/video.py +83 -0
- macbot/web/__init__.py +85 -0
- macbot/web/static/common.js +139 -0
- macbot/web/static/fonts/JetBrainsMono.ttf +0 -0
- macbot/web/static/fonts/OFL-JetBrainsMono.txt +93 -0
- macbot/web/static/logo-dark.svg +1 -0
- macbot/web/static/logo-mark.svg +1 -0
- macbot/web/static/logo.svg +1 -0
- macbot/web/static/theme.css +200 -0
- macbot/web/templates/_head.html +5 -0
- macbot/web/templates/collect.html +223 -0
- macbot/web/templates/replay.html +132 -0
- macbot/web/templates/run.html +124 -0
- macbot-0.1.0.dist-info/METADATA +183 -0
- macbot-0.1.0.dist-info/RECORD +46 -0
- macbot-0.1.0.dist-info/WHEEL +5 -0
- macbot-0.1.0.dist-info/entry_points.txt +2 -0
- macbot-0.1.0.dist-info/licenses/LICENSE +202 -0
- macbot-0.1.0.dist-info/top_level.txt +1 -0
macbot/collect/server.py
ADDED
|
@@ -0,0 +1,371 @@
|
|
|
1
|
+
"""Leader-follower teleoperation with recording, driven from a local web page.
|
|
2
|
+
|
|
3
|
+
Workflow (same as ``lerobot-record``): record one episode, reset the scene, record the next.
|
|
4
|
+
Keys on the page: S start, Enter or right arrow save, left arrow discard. Every saved episode can
|
|
5
|
+
be replayed (all cameras plus joint traces) or deleted (moved to ``deleted/``, numbers are not reused).
|
|
6
|
+
Each save runs quality checks (frame age, repeated frames, joint jumps, late samples).
|
|
7
|
+
|
|
8
|
+
Episode file ``<data_root>/raw/<task>/episode_XXX.npz``:
|
|
9
|
+
qpos (N,7) follower measured joints, degrees
|
|
10
|
+
action (N,7) command sent to the follower (gripper: leader value before the force clamp)
|
|
11
|
+
timestamp (N,) k / fps (fixed grid); wall_time (N,) actual sampling time
|
|
12
|
+
<cam>_jpeg, <cam>_offsets JPEG stream per camera, frame i = jpeg[off[i]:off[i+1]]
|
|
13
|
+
<cam>_age, <cam>_repeat age of the frame at sampling time; same frame as the previous sample
|
|
14
|
+
meta JSON: task, instruction, fps, joint names, camera info, follower feedforward, check result
|
|
15
|
+
"""
|
|
16
|
+
from __future__ import annotations
|
|
17
|
+
|
|
18
|
+
import json
|
|
19
|
+
import re
|
|
20
|
+
import shutil
|
|
21
|
+
import threading
|
|
22
|
+
import time
|
|
23
|
+
from http.server import ThreadingHTTPServer
|
|
24
|
+
|
|
25
|
+
import cv2
|
|
26
|
+
import numpy as np
|
|
27
|
+
|
|
28
|
+
from .. import web
|
|
29
|
+
from ..cameras import camera_info, open_cameras
|
|
30
|
+
from ..config import ARM, JOINTS, Config
|
|
31
|
+
from ..system import ensure_dir, give_back, start_log
|
|
32
|
+
from ..i18n import text, tr
|
|
33
|
+
from .checks import check_episode
|
|
34
|
+
|
|
35
|
+
CAM_STALL_S = 0.5 # a camera without a new frame for this long counts as disconnected
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
class State:
|
|
39
|
+
def __init__(self, cfg: Config, out, fake: bool):
|
|
40
|
+
self.cfg, self.out, self.fake = cfg, out, fake
|
|
41
|
+
self.cam_names = list(cfg["cameras"])
|
|
42
|
+
self.fps = int(cfg["record"]["fps"])
|
|
43
|
+
self.lock = threading.Lock()
|
|
44
|
+
self.phase = "idle" # idle | recording
|
|
45
|
+
self.t0 = 0.0
|
|
46
|
+
self.instruction = cfg["task"]["instruction"]
|
|
47
|
+
self.msg = tr("Put both arms at the zero pose. Teleoperation starts after the alignment check.",
|
|
48
|
+
"两臂摆到零位后启动;对齐检查通过才开始遥操作。")
|
|
49
|
+
self.q, self.cmd = [0.0] * 7, [0.0] * 7
|
|
50
|
+
self.limits, self.ready, self.quit = {}, False, False
|
|
51
|
+
self.buf = None
|
|
52
|
+
self.saving = 0
|
|
53
|
+
self.ff = {}
|
|
54
|
+
self.cam_info = camera_info(cfg["cameras"])
|
|
55
|
+
self.summaries = {} # file name -> (mtime, summary)
|
|
56
|
+
self.cams = {}
|
|
57
|
+
self.refresh()
|
|
58
|
+
|
|
59
|
+
def files(self):
|
|
60
|
+
return sorted(self.out.glob("episode_*.npz"))
|
|
61
|
+
|
|
62
|
+
def refresh(self):
|
|
63
|
+
keep = {}
|
|
64
|
+
for p in self.files():
|
|
65
|
+
m = p.stat().st_mtime
|
|
66
|
+
old = self.summaries.get(p.name)
|
|
67
|
+
if old and old[0] == m:
|
|
68
|
+
keep[p.name] = old
|
|
69
|
+
continue
|
|
70
|
+
idx = int(p.stem.split("_")[1])
|
|
71
|
+
try:
|
|
72
|
+
with np.load(p) as z:
|
|
73
|
+
meta = json.loads(str(z["meta"]))
|
|
74
|
+
keep[p.name] = (m, {"idx": idx, "dur": meta["duration_s"], "frames": meta["frames"],
|
|
75
|
+
"time": meta["saved_at"][5:16], "instruction": meta["instruction"],
|
|
76
|
+
"check": meta.get("check")})
|
|
77
|
+
except Exception as e: # half-written or damaged
|
|
78
|
+
keep[p.name] = (m, {"idx": idx, "dur": 0, "frames": 0, "time": "", "instruction": "",
|
|
79
|
+
"check": {"ok": False, "errors": [tr(f"unreadable: {e}", f"读不出:{e}")], "warnings": []}})
|
|
80
|
+
self.summaries = keep
|
|
81
|
+
|
|
82
|
+
def next_idx(self):
|
|
83
|
+
ids = [int(p.stem.split("_")[1]) for p in self.files()]
|
|
84
|
+
deleted = self.out / "deleted"
|
|
85
|
+
if deleted.exists():
|
|
86
|
+
ids += [int(m.group(1)) for p in deleted.glob("episode_*.npz") if (m := re.match(r"episode_(\d+)", p.stem))]
|
|
87
|
+
return max(ids, default=-1) + 1
|
|
88
|
+
|
|
89
|
+
def cam_age(self):
|
|
90
|
+
return {k: (round(a, 2) if (a := c.age()) is not None else None) for k, c in self.cams.items()}
|
|
91
|
+
|
|
92
|
+
def stalled(self):
|
|
93
|
+
return [k for k, a in self.cam_age().items() if a is None or a > CAM_STALL_S]
|
|
94
|
+
|
|
95
|
+
def status(self):
|
|
96
|
+
ages = self.cam_age()
|
|
97
|
+
with self.lock:
|
|
98
|
+
eps = [v[1] for _, v in sorted(self.summaries.items())][::-1]
|
|
99
|
+
rec = self.phase == "recording"
|
|
100
|
+
return {"phase": self.phase, "ready": self.ready, "fake": self.fake,
|
|
101
|
+
"elapsed": time.time() - self.t0 if rec else 0.0,
|
|
102
|
+
"frames": len(self.buf["qpos"]) if (rec and self.buf) else 0,
|
|
103
|
+
"msg": self.msg, "instruction": self.instruction, "task": self.cfg.task,
|
|
104
|
+
"target": self.cfg["task"]["target_episodes"], "count": len(eps), "saving": self.saving,
|
|
105
|
+
"q": self.q, "cmd": self.cmd, "names": JOINTS, "limits": self.limits, "episodes": eps,
|
|
106
|
+
"fps": self.fps, "cams": self.cam_info, "cam_names": self.cam_names, "out": str(self.out),
|
|
107
|
+
"cam_age": ages, "stall_s": CAM_STALL_S}
|
|
108
|
+
|
|
109
|
+
def command(self, c):
|
|
110
|
+
a = c.get("action")
|
|
111
|
+
with self.lock:
|
|
112
|
+
if a == "instruction" and self.phase == "idle":
|
|
113
|
+
text = str(c.get("text", "")).strip()
|
|
114
|
+
if text:
|
|
115
|
+
self.instruction = text
|
|
116
|
+
self.msg = tr(f"Instruction set: {text}", f"指令改为:{text}")
|
|
117
|
+
elif a == "start" and self.phase == "idle":
|
|
118
|
+
if not self.ready:
|
|
119
|
+
self.msg = tr("Teleoperation has not started yet.", "遥操作还没启动。")
|
|
120
|
+
return
|
|
121
|
+
bad = self.stalled()
|
|
122
|
+
if bad:
|
|
123
|
+
self.msg = tr(f"No frames from {', '.join(bad)} (cable loose?). Fix it and restart.",
|
|
124
|
+
f"{'、'.join(bad)} 相机没有画面(线松了?),插好后重启采集程序。")
|
|
125
|
+
return
|
|
126
|
+
keys = ["qpos", "action", "wall", "late"] + [f"{k}_{s}" for k in self.cam_names for s in ("jpg", "age", "repeat")]
|
|
127
|
+
self.buf = {k: [] for k in keys}
|
|
128
|
+
self.phase, self.t0 = "recording", time.time()
|
|
129
|
+
self.msg = tr("Recording. Enter saves, left arrow discards.", "录制中。回车保存,← 丢弃。")
|
|
130
|
+
elif a == "discard" and self.phase == "recording":
|
|
131
|
+
self.phase, self.buf = "idle", None
|
|
132
|
+
self.msg = tr("Discarded. Reset the scene and press S.", "已丢弃。复位场景后按 S 重录。")
|
|
133
|
+
elif a == "save" and self.phase == "recording":
|
|
134
|
+
buf, self.buf, self.phase = self.buf, None, "idle"
|
|
135
|
+
n = len(buf["qpos"])
|
|
136
|
+
if n / self.fps < self.cfg["record"]["min_episode_s"]:
|
|
137
|
+
self.msg = tr(f"Not saved: only {n / self.fps:.1f} s.", f"没保存:只有 {n / self.fps:.1f} 秒。")
|
|
138
|
+
return
|
|
139
|
+
idx = self.next_idx() + self.saving
|
|
140
|
+
self.saving += 1
|
|
141
|
+
self.msg = tr(f"Saving episode {idx:03d} ({n / self.fps:.1f} s)...", f"正在保存第 {idx:03d} 条({n / self.fps:.1f} 秒)…")
|
|
142
|
+
threading.Thread(target=self._save, args=(idx, buf, self.instruction), daemon=True).start()
|
|
143
|
+
elif a == "delete" and self.phase == "idle":
|
|
144
|
+
p = self.out / f"episode_{int(c.get('idx', -1)):03d}.npz"
|
|
145
|
+
if p.exists():
|
|
146
|
+
d = ensure_dir(self.out / "deleted")
|
|
147
|
+
shutil.move(str(p), str(d / p.name))
|
|
148
|
+
self.refresh()
|
|
149
|
+
self.msg = tr(f"Episode {p.stem[-3:]} moved to deleted/.", f"第 {p.stem[-3:]} 条已移到 deleted/。")
|
|
150
|
+
elif a == "quit":
|
|
151
|
+
self.quit = True
|
|
152
|
+
self.msg = tr("Shutting down: the follower returns to zero slowly, then disables.", "正在结束:从臂慢速回零后失能。")
|
|
153
|
+
|
|
154
|
+
def _save(self, idx, buf, instruction):
|
|
155
|
+
try:
|
|
156
|
+
arr = {k: np.asarray(v) for k, v in buf.items() if not k.endswith("_jpg")}
|
|
157
|
+
rc = self.cfg["record"]
|
|
158
|
+
chk = check_episode(arr, self.cam_names, self.fps, self.cfg["checks"], rc["min_episode_s"])
|
|
159
|
+
n = len(buf["qpos"])
|
|
160
|
+
meta = {"task": self.cfg.task, "instruction": instruction, "fps": self.fps, "frames": n,
|
|
161
|
+
"duration_s": round(n / self.fps, 2), "joint_names": JOINTS, "units": "deg",
|
|
162
|
+
"state": "follower measured position", "action": "command sent to follower (gripper: leader value)",
|
|
163
|
+
"cameras": self.cam_info, "follower_ff": self.ff, "fake": self.fake,
|
|
164
|
+
"saved_at": time.strftime("%Y-%m-%d %H:%M:%S"), "check": chk, "writer": "macbot"}
|
|
165
|
+
extra = {}
|
|
166
|
+
for k in self.cam_names:
|
|
167
|
+
extra[f"{k}_jpeg"] = np.concatenate(buf[f"{k}_jpg"])
|
|
168
|
+
extra[f"{k}_offsets"] = np.cumsum([0] + [len(b) for b in buf[f"{k}_jpg"]]).astype(np.int64)
|
|
169
|
+
extra[f"{k}_age"] = arr[f"{k}_age"].astype(np.float32)
|
|
170
|
+
extra[f"{k}_repeat"] = arr[f"{k}_repeat"].astype(bool)
|
|
171
|
+
path = self.out / f"episode_{idx:03d}.npz"
|
|
172
|
+
tmp = path.with_suffix(".tmp.npz")
|
|
173
|
+
np.savez(tmp, qpos=arr["qpos"].astype(np.float32), action=arr["action"].astype(np.float32),
|
|
174
|
+
timestamp=np.arange(n) / self.fps, wall_time=arr["wall"],
|
|
175
|
+
meta=np.array(json.dumps(meta, ensure_ascii=False)), **extra)
|
|
176
|
+
tmp.rename(path)
|
|
177
|
+
give_back(path)
|
|
178
|
+
items = chk["errors"] + chk["warnings"]
|
|
179
|
+
en, zh = "; ".join(text(x, "en") for x in items), ";".join(text(x, "zh") for x in items)
|
|
180
|
+
with self.lock:
|
|
181
|
+
self.saving -= 1
|
|
182
|
+
self.refresh()
|
|
183
|
+
k = len(self.summaries)
|
|
184
|
+
self.msg = tr(f"Saved episode {idx:03d} ({n / self.fps:.1f} s)" + (f", check: {en}" if en else ", checks passed")
|
|
185
|
+
+ f". {k} in total. Reset the scene and press S.",
|
|
186
|
+
f"已保存第 {idx:03d} 条({n / self.fps:.1f} 秒)" + (f",注意:{zh}" if zh else ",检查通过")
|
|
187
|
+
+ f"。共 {k} 条。复位场景后按 S 录下一条。")
|
|
188
|
+
except Exception as e:
|
|
189
|
+
with self.lock:
|
|
190
|
+
self.saving -= 1
|
|
191
|
+
self.msg = tr(f"Saving episode {idx:03d} failed: {e}", f"保存第 {idx:03d} 条失败:{e}")
|
|
192
|
+
|
|
193
|
+
|
|
194
|
+
class Recorder:
|
|
195
|
+
"""Samples joints and cameras on a fixed 1/fps grid while recording."""
|
|
196
|
+
|
|
197
|
+
def __init__(self, st: State):
|
|
198
|
+
self.st = st
|
|
199
|
+
self.size = {k: tuple(st.cfg["cameras"][k]["save_size"]) for k in st.cam_names}
|
|
200
|
+
self.q = int(st.cfg["record"]["jpeg_quality"])
|
|
201
|
+
threading.Thread(target=self._loop, daemon=True).start()
|
|
202
|
+
|
|
203
|
+
def _loop(self):
|
|
204
|
+
st, dt = self.st, 1.0 / self.st.fps
|
|
205
|
+
while not st.quit:
|
|
206
|
+
with st.lock:
|
|
207
|
+
buf, t0 = (st.buf, st.t0) if st.phase == "recording" else (None, 0.0)
|
|
208
|
+
if buf is None:
|
|
209
|
+
time.sleep(0.005)
|
|
210
|
+
continue
|
|
211
|
+
last = {k: None for k in st.cam_names}
|
|
212
|
+
k = 0
|
|
213
|
+
while True:
|
|
214
|
+
target = t0 + k * dt
|
|
215
|
+
wait = target - time.time()
|
|
216
|
+
if wait > 0:
|
|
217
|
+
time.sleep(wait)
|
|
218
|
+
with st.lock:
|
|
219
|
+
if st.buf is not buf:
|
|
220
|
+
break
|
|
221
|
+
q, cmd = list(st.q), list(st.cmd)
|
|
222
|
+
now = time.time()
|
|
223
|
+
row = {"qpos": q, "action": cmd, "wall": now - t0, "late": (now - target) > dt}
|
|
224
|
+
bad = [c for c in st.cam_names if now - st.cams[c].latest()[1] > CAM_STALL_S]
|
|
225
|
+
if bad:
|
|
226
|
+
with st.lock:
|
|
227
|
+
if st.buf is buf:
|
|
228
|
+
st.phase, st.buf = "idle", None
|
|
229
|
+
st.msg = tr(f"{', '.join(bad)} lost frames for over {CAM_STALL_S} s; this episode was discarded. "
|
|
230
|
+
"Check the cable and restart (a dropped camera must be reopened).",
|
|
231
|
+
f"{'、'.join(bad)} 相机断流超过 {CAM_STALL_S} 秒,这一条已自动丢弃。检查线后重启采集程序。")
|
|
232
|
+
print(f"camera stalled: {bad}; episode discarded", flush=True)
|
|
233
|
+
break
|
|
234
|
+
for c in st.cam_names:
|
|
235
|
+
img, stamp = st.cams[c].latest()
|
|
236
|
+
_, jpg = cv2.imencode(".jpg", cv2.resize(img, self.size[c], interpolation=cv2.INTER_AREA),
|
|
237
|
+
[cv2.IMWRITE_JPEG_QUALITY, self.q])
|
|
238
|
+
row[f"{c}_jpg"], row[f"{c}_age"], row[f"{c}_repeat"] = jpg.reshape(-1), now - stamp, stamp == last[c]
|
|
239
|
+
last[c] = stamp
|
|
240
|
+
with st.lock:
|
|
241
|
+
if st.buf is not buf:
|
|
242
|
+
break
|
|
243
|
+
for key, v in row.items():
|
|
244
|
+
buf[key].append(v)
|
|
245
|
+
k += 1
|
|
246
|
+
|
|
247
|
+
|
|
248
|
+
def make_handler(st: State):
|
|
249
|
+
preview = {k: tuple(st.cfg["cameras"][k]["save_size"]) for k in st.cam_names}
|
|
250
|
+
|
|
251
|
+
class H(web.Handler):
|
|
252
|
+
def do_GET(self):
|
|
253
|
+
path = self.path.split("?")[0]
|
|
254
|
+
if path == "/":
|
|
255
|
+
return self.send(200, web.page("collect", task=st.cfg.task), "text/html; charset=utf-8")
|
|
256
|
+
if self.static(path):
|
|
257
|
+
return
|
|
258
|
+
if path == "/api/status":
|
|
259
|
+
return self.json(st.status())
|
|
260
|
+
m = re.fullmatch(r"/stream/(\w+)\.mjpg", path)
|
|
261
|
+
if m and m.group(1) in st.cams:
|
|
262
|
+
return self.mjpeg(st.cams[m.group(1)], preview[m.group(1)], lambda: st.quit)
|
|
263
|
+
m = re.fullmatch(r"/api/episode/(\d+)", path)
|
|
264
|
+
if m:
|
|
265
|
+
p = st.out / f"episode_{int(m.group(1)):03d}.npz"
|
|
266
|
+
if not p.exists():
|
|
267
|
+
return self.json({}, 404)
|
|
268
|
+
with np.load(p) as d:
|
|
269
|
+
return self.json({"qpos": d["qpos"].round(2).tolist(), "action": d["action"].round(2).tolist(),
|
|
270
|
+
"ts": d["timestamp"].round(3).tolist(), "meta": json.loads(str(d["meta"]))})
|
|
271
|
+
m = re.fullmatch(r"/api/episode/(\d+)/(\w+)/(\d+)", path)
|
|
272
|
+
if m:
|
|
273
|
+
p = st.out / f"episode_{int(m.group(1)):03d}.npz"
|
|
274
|
+
if not p.exists():
|
|
275
|
+
return self.send(404, b"", "text/plain")
|
|
276
|
+
with np.load(p) as d:
|
|
277
|
+
off, k = d[f"{m.group(2)}_offsets"], int(m.group(3))
|
|
278
|
+
k = max(0, min(k, len(off) - 2))
|
|
279
|
+
return self.send(200, d[f"{m.group(2)}_jpeg"][off[k]:off[k + 1]].tobytes(), "image/jpeg")
|
|
280
|
+
self.send(404, b"not found", "text/plain")
|
|
281
|
+
|
|
282
|
+
def do_POST(self):
|
|
283
|
+
if self.path != "/api/cmd":
|
|
284
|
+
return self.send(404, b"", "text/plain")
|
|
285
|
+
try:
|
|
286
|
+
st.command(self.body_json())
|
|
287
|
+
except Exception as e:
|
|
288
|
+
with st.lock:
|
|
289
|
+
st.msg = tr(f"Command failed: {e}", f"操作失败:{e}")
|
|
290
|
+
self.json(st.status())
|
|
291
|
+
|
|
292
|
+
return H
|
|
293
|
+
|
|
294
|
+
|
|
295
|
+
def run(cfg: Config, fake: bool = False, port: int | None = None, out=None):
|
|
296
|
+
start_log(cfg.log_dir, "collect")
|
|
297
|
+
out = ensure_dir(out or (cfg.raw_dir() if not fake else cfg.raw_dir(cfg.task + "_fake")))
|
|
298
|
+
st = State(cfg, out, fake)
|
|
299
|
+
T = cfg["teleop"]
|
|
300
|
+
port = port or T["port"]
|
|
301
|
+
srv = ThreadingHTTPServer(("127.0.0.1", port), make_handler(st)) # bind first: fails fast if the port is taken
|
|
302
|
+
srv.daemon_threads = True
|
|
303
|
+
st.cams = open_cameras(cfg["cameras"], st.fps, fake)
|
|
304
|
+
Recorder(st)
|
|
305
|
+
threading.Thread(target=srv.serve_forever, daemon=True).start()
|
|
306
|
+
print(f"web page: http://127.0.0.1:{port} data: {out}", flush=True)
|
|
307
|
+
|
|
308
|
+
from ..robot.rebot import clip_gripper
|
|
309
|
+
rcfg = cfg["robot"]
|
|
310
|
+
if fake:
|
|
311
|
+
from ..robot import FakeFollower, FakeLeader
|
|
312
|
+
robot, leader = FakeFollower(), FakeLeader()
|
|
313
|
+
else:
|
|
314
|
+
from ..robot.rebot import make_follower, make_leader
|
|
315
|
+
robot, leader = make_follower(rcfg), make_leader(leader_id=rcfg.get("leader_id", "rebot_arm_102_leader"))
|
|
316
|
+
st.ff = dict(robot.feedforward, gripper_mode=rcfg.get("gripper_mode"), gripper_lead_deg=rcfg["gripper_lead_deg"])
|
|
317
|
+
leader.connect(calibrate=False)
|
|
318
|
+
robot.connect(calibrate=False)
|
|
319
|
+
lim = robot.config.joint_limits
|
|
320
|
+
started = False
|
|
321
|
+
try:
|
|
322
|
+
a0, o0 = leader.get_action(), robot.get_observation()
|
|
323
|
+
bad = [f"{n} leader {a0[f'{n}.pos']:.0f} / follower {o0[f'{n}.pos']:.0f}" for n in ARM
|
|
324
|
+
if abs(min(max(a0[f"{n}.pos"], lim[n][0]), lim[n][1]) - o0[f"{n}.pos"]) > T["align_max_deg"]]
|
|
325
|
+
if bad and not fake:
|
|
326
|
+
with st.lock:
|
|
327
|
+
st.msg = tr(f"Arms not aligned (more than {T['align_max_deg']} deg): " + "; ".join(bad) + ". Align and restart.",
|
|
328
|
+
f"两臂没对齐(差超过 {T['align_max_deg']}°):" + ";".join(bad) + "。对齐后重启。")
|
|
329
|
+
print(st.msg["en"], flush=True)
|
|
330
|
+
time.sleep(3) # let the page show the message
|
|
331
|
+
raise SystemExit(2)
|
|
332
|
+
with st.lock:
|
|
333
|
+
st.limits = {n: list(lim[n]) for n in JOINTS}
|
|
334
|
+
st.ready = True
|
|
335
|
+
st.msg = tr(f"Teleoperation running. Set up the scene and press S. {len(st.summaries)} episodes so far.",
|
|
336
|
+
f"遥操作已启动。摆好场景后按 S 开始录制。已有 {len(st.summaries)} 条。")
|
|
337
|
+
started = True
|
|
338
|
+
print("teleoperation running", flush=True)
|
|
339
|
+
hz, soft, lead = T["hz"], T["soft_start_s"], float(rcfg["gripper_lead_deg"])
|
|
340
|
+
t_start = time.perf_counter()
|
|
341
|
+
while not st.quit:
|
|
342
|
+
tick = time.perf_counter()
|
|
343
|
+
act = leader.get_action()
|
|
344
|
+
r = min(1.0, (tick - t_start) / soft) # soft start from the follower's pose
|
|
345
|
+
act = {key: o0.get(key, v) * (1 - r) + v * r for key, v in act.items()}
|
|
346
|
+
obs = robot.get_observation()
|
|
347
|
+
sent = robot.send_action(clip_gripper(act, float(obs["gripper.pos"]), lead))
|
|
348
|
+
q = [float(obs[f"{n}.pos"]) for n in JOINTS]
|
|
349
|
+
cmd = [float(sent.get(f"{n}.pos", act[f"{n}.pos"])) for n in ARM] + [float(act["gripper.pos"])]
|
|
350
|
+
with st.lock:
|
|
351
|
+
st.q, st.cmd = q, cmd
|
|
352
|
+
time.sleep(max(0.0, 1 / hz - (time.perf_counter() - tick)))
|
|
353
|
+
except KeyboardInterrupt:
|
|
354
|
+
pass
|
|
355
|
+
finally:
|
|
356
|
+
st.quit = True
|
|
357
|
+
try:
|
|
358
|
+
if not fake:
|
|
359
|
+
from ..robot.rebot import safe_disconnect
|
|
360
|
+
try:
|
|
361
|
+
leader.disconnect()
|
|
362
|
+
finally:
|
|
363
|
+
safe_disconnect(robot, go_zero=started)
|
|
364
|
+
finally:
|
|
365
|
+
for c in st.cams.values():
|
|
366
|
+
c.close()
|
|
367
|
+
t = time.time()
|
|
368
|
+
while st.saving and time.time() - t < 10: # wait for background saves
|
|
369
|
+
time.sleep(0.1)
|
|
370
|
+
srv.shutdown()
|
|
371
|
+
print("done", flush=True)
|
macbot/config.py
ADDED
|
@@ -0,0 +1,56 @@
|
|
|
1
|
+
"""Configuration loading. One YAML file describes the task, the robot, the cameras and the paths."""
|
|
2
|
+
from __future__ import annotations
|
|
3
|
+
|
|
4
|
+
import copy
|
|
5
|
+
from pathlib import Path
|
|
6
|
+
|
|
7
|
+
import yaml
|
|
8
|
+
|
|
9
|
+
from .system import expand
|
|
10
|
+
|
|
11
|
+
PACKAGE = Path(__file__).resolve().parent
|
|
12
|
+
DEFAULT = PACKAGE / "default.yaml"
|
|
13
|
+
|
|
14
|
+
JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_yaw", "wrist_roll", "gripper"]
|
|
15
|
+
ARM = JOINTS[:6]
|
|
16
|
+
CAMERAS = ("top", "wrist")
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
def _merge(base: dict, over: dict) -> dict:
|
|
20
|
+
out = copy.deepcopy(base)
|
|
21
|
+
for k, v in (over or {}).items():
|
|
22
|
+
out[k] = _merge(out[k], v) if isinstance(v, dict) and isinstance(out.get(k), dict) else v
|
|
23
|
+
return out
|
|
24
|
+
|
|
25
|
+
|
|
26
|
+
class Config(dict):
|
|
27
|
+
"""A dict with a few path helpers."""
|
|
28
|
+
|
|
29
|
+
@property
|
|
30
|
+
def root(self) -> Path:
|
|
31
|
+
return expand(self["data_root"])
|
|
32
|
+
|
|
33
|
+
@property
|
|
34
|
+
def task(self) -> str:
|
|
35
|
+
return self["task"]["name"]
|
|
36
|
+
|
|
37
|
+
def raw_dir(self, task: str | None = None) -> Path:
|
|
38
|
+
return self.root / "raw" / (task or self.task)
|
|
39
|
+
|
|
40
|
+
def dataset_dir(self, name: str) -> Path:
|
|
41
|
+
return self.root / "datasets" / name
|
|
42
|
+
|
|
43
|
+
def rollout_dir(self, task: str | None = None) -> Path:
|
|
44
|
+
return self.root / "rollouts" / (task or self.task)
|
|
45
|
+
|
|
46
|
+
@property
|
|
47
|
+
def log_dir(self) -> Path:
|
|
48
|
+
return self.root / "logs"
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
def load(path: str | Path | None = None) -> Config:
|
|
52
|
+
"""Load the default config, then overlay ``path`` if given."""
|
|
53
|
+
cfg = yaml.safe_load(DEFAULT.read_text())
|
|
54
|
+
if path:
|
|
55
|
+
cfg = _merge(cfg, yaml.safe_load(expand(path).read_text()) or {})
|
|
56
|
+
return Config(cfg)
|
macbot/data/__init__.py
ADDED
|
File without changes
|
macbot/data/export.py
ADDED
|
@@ -0,0 +1,116 @@
|
|
|
1
|
+
"""Recorded episodes (``episode_XXX.npz``) -> LeRobot dataset (v3 format, camera streams as video).
|
|
2
|
+
|
|
3
|
+
Feature names match LeRobot's ``rebot_b601_follower``, so a trained policy plugs straight into
|
|
4
|
+
``macbot run``:
|
|
5
|
+
|
|
6
|
+
observation.state (7,) measured follower joints, degrees
|
|
7
|
+
action (7,) command sent to the follower (gripper: leader angle, or 0/1)
|
|
8
|
+
observation.images.<camera> one entry per configured camera
|
|
9
|
+
|
|
10
|
+
``--gripper binary`` turns the gripper action into 0 (open) / 1 (closed) with hysteresis on the leader
|
|
11
|
+
reading, and writes ``gripper.json`` next to the dataset. ``macbot run`` finds that file and maps the
|
|
12
|
+
policy output back to the two angles. Each frame's task string is the episode's instruction.
|
|
13
|
+
Episodes that failed the quality check are skipped; ``--skip-warnings`` also skips those with warnings.
|
|
14
|
+
"""
|
|
15
|
+
from __future__ import annotations
|
|
16
|
+
|
|
17
|
+
import json
|
|
18
|
+
import shutil
|
|
19
|
+
import sys
|
|
20
|
+
|
|
21
|
+
import cv2
|
|
22
|
+
import numpy as np
|
|
23
|
+
|
|
24
|
+
from ..config import JOINTS, Config
|
|
25
|
+
from ..i18n import text
|
|
26
|
+
from ..system import expand
|
|
27
|
+
|
|
28
|
+
|
|
29
|
+
def binarize(g, close_above: float, open_below: float) -> np.ndarray:
|
|
30
|
+
"""Leader gripper angle -> 0/1 (1 = closed) with hysteresis, so values near a threshold do not flicker."""
|
|
31
|
+
out, s = np.zeros(len(g), np.float32), 0.0
|
|
32
|
+
for i, v in enumerate(g):
|
|
33
|
+
if s == 0.0 and v > close_above:
|
|
34
|
+
s = 1.0
|
|
35
|
+
elif s == 1.0 and v < open_below:
|
|
36
|
+
s = 0.0
|
|
37
|
+
out[i] = s
|
|
38
|
+
return out
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
def frames(d, cam):
|
|
42
|
+
off, blob = d[f"{cam}_offsets"], d[f"{cam}_jpeg"]
|
|
43
|
+
for i in range(len(off) - 1):
|
|
44
|
+
yield cv2.cvtColor(cv2.imdecode(blob[off[i]:off[i + 1]], cv2.IMREAD_COLOR), cv2.COLOR_BGR2RGB)
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def _msg(items) -> str:
|
|
48
|
+
return "; ".join(text(m, "en") if isinstance(m, dict) else str(m) for m in items)
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
def run(cfg: Config, args):
|
|
52
|
+
binary = args.gripper == "binary"
|
|
53
|
+
name = cfg.task + ("_bin" if binary else "")
|
|
54
|
+
data = expand(args.data) if args.data else cfg.raw_dir()
|
|
55
|
+
out = expand(args.out) if args.out else cfg.dataset_dir(name)
|
|
56
|
+
repo_id = args.repo_id or f"local/{name}"
|
|
57
|
+
files = sorted(data.glob("episode_*.npz"))
|
|
58
|
+
if not files:
|
|
59
|
+
sys.exit(f"no episode_*.npz in {data}")
|
|
60
|
+
if out.exists():
|
|
61
|
+
if not args.overwrite:
|
|
62
|
+
sys.exit(f"{out} exists; add --overwrite to rebuild it")
|
|
63
|
+
shutil.rmtree(out)
|
|
64
|
+
|
|
65
|
+
keep, skipped = [], []
|
|
66
|
+
for p in files:
|
|
67
|
+
with np.load(p) as d:
|
|
68
|
+
meta = json.loads(str(d["meta"]))
|
|
69
|
+
c = meta.get("check") or {"ok": True, "errors": [], "warnings": []}
|
|
70
|
+
if not c["ok"] or (args.skip_warnings and c["warnings"]):
|
|
71
|
+
skipped.append((p.name, _msg(c.get("errors", []) + c.get("warnings", []))))
|
|
72
|
+
else:
|
|
73
|
+
keep.append((p, meta))
|
|
74
|
+
if not keep:
|
|
75
|
+
sys.exit("nothing to export")
|
|
76
|
+
|
|
77
|
+
m0 = keep[0][1]
|
|
78
|
+
fps, cams = int(m0["fps"]), list(m0["cameras"])
|
|
79
|
+
for p, m in keep[1:]:
|
|
80
|
+
if int(m["fps"]) != fps or any(m["cameras"].get(c, {}).get("size") != m0["cameras"][c]["size"] for c in cams):
|
|
81
|
+
sys.exit(f"{p.name}: fps or image size differs from the first episode; they cannot share a dataset")
|
|
82
|
+
with np.load(keep[0][0]) as d:
|
|
83
|
+
shape = {c: next(frames(d, c)).shape for c in cams}
|
|
84
|
+
|
|
85
|
+
names = [f"{n}.pos" for n in JOINTS]
|
|
86
|
+
kind = "image" if args.images else "video"
|
|
87
|
+
features = {"observation.state": {"dtype": "float32", "shape": (7,), "names": names},
|
|
88
|
+
"action": {"dtype": "float32", "shape": (7,), "names": names},
|
|
89
|
+
**{f"observation.images.{c}": {"dtype": kind, "shape": shape[c], "names": ["height", "width", "channels"]}
|
|
90
|
+
for c in cams}}
|
|
91
|
+
from lerobot.datasets.lerobot_dataset import LeRobotDataset
|
|
92
|
+
ds = LeRobotDataset.create(repo_id=repo_id, fps=fps, features=features, root=out,
|
|
93
|
+
robot_type="rebot_b601_follower", use_videos=not args.images)
|
|
94
|
+
total = 0
|
|
95
|
+
for p, m in keep:
|
|
96
|
+
with np.load(p) as d:
|
|
97
|
+
q, a = d["qpos"].astype(np.float32), d["action"].astype(np.float32)
|
|
98
|
+
if binary:
|
|
99
|
+
a[:, 6] = binarize(a[:, 6], args.close_above, args.open_below)
|
|
100
|
+
streams = [frames(d, c) for c in cams]
|
|
101
|
+
for i, imgs in enumerate(zip(*streams)):
|
|
102
|
+
ds.add_frame({"observation.state": q[i], "action": a[i], "task": m["instruction"],
|
|
103
|
+
**{f"observation.images.{c}": im for c, im in zip(cams, imgs)}})
|
|
104
|
+
ds.save_episode()
|
|
105
|
+
total += len(q)
|
|
106
|
+
print(f" {p.name}: {len(q)} frames \"{m['instruction']}\"", flush=True)
|
|
107
|
+
ds.finalize()
|
|
108
|
+
if binary:
|
|
109
|
+
(out / "gripper.json").write_text(json.dumps({
|
|
110
|
+
"mode": "binary", "close_value": 1.0, "open_value": 0.0, "open_deg": args.open_deg, "close_deg": args.close_deg,
|
|
111
|
+
"label_from": "leader gripper with hysteresis", "close_above": args.close_above, "open_below": args.open_below}, indent=1))
|
|
112
|
+
print(f"binary gripper: closed above {args.close_above} deg, open below {args.open_below} deg; "
|
|
113
|
+
f"deploy open {args.open_deg} deg, closed {args.close_deg} deg")
|
|
114
|
+
print(f"exported {len(keep)} episodes, {total} frames ({total / fps / 60:.1f} min) to {out} (repo_id {repo_id}, {fps} fps)")
|
|
115
|
+
for n, why in skipped:
|
|
116
|
+
print(f" skipped {n}: {why}")
|
macbot/data/health.py
ADDED
|
@@ -0,0 +1,67 @@
|
|
|
1
|
+
"""Per-episode tracking and timing report of recorded data.
|
|
2
|
+
|
|
3
|
+
For every episode: the lag (frames) that best aligns command and measurement, p95 and max tracking error
|
|
4
|
+
of the six arm joints at that lag, the median offset while the leader is still (a calibration drift
|
|
5
|
+
shows up here), the gripper range, the worst frame interval and the repeated camera frames.
|
|
6
|
+
``--split N`` prints medians before and after episode N, for example to compare data recorded before
|
|
7
|
+
and after a repair.
|
|
8
|
+
"""
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
import sys
|
|
12
|
+
|
|
13
|
+
import numpy as np
|
|
14
|
+
|
|
15
|
+
from ..config import Config
|
|
16
|
+
from ..system import expand
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
def episode_stats(d, fps: float, cams) -> dict:
|
|
20
|
+
q, a = d["qpos"].astype(float), d["action"].astype(float)
|
|
21
|
+
best = None
|
|
22
|
+
for lag in range(8):
|
|
23
|
+
err = np.abs(a[:len(a) - lag, :6] - q[lag:, :6])
|
|
24
|
+
m = np.percentile(err, 95, axis=0).mean()
|
|
25
|
+
if best is None or m < best[0]:
|
|
26
|
+
best = (m, lag, err)
|
|
27
|
+
_, lag, err = best
|
|
28
|
+
still = np.abs(np.diff(a[:, :6], axis=0)).max(1) * fps < 2.0
|
|
29
|
+
off = (a[1:, :6] - q[1:, :6])[still]
|
|
30
|
+
wt = d["wall_time"] if "wall_time" in d.files else d["timestamp"]
|
|
31
|
+
dt = np.diff(wt) if len(wt) > 1 else np.zeros(1)
|
|
32
|
+
return {"n": len(q), "lag": lag, "p95": np.percentile(err, 95, axis=0), "max": err.max(0),
|
|
33
|
+
"still": np.median(off, 0) if len(off) else np.full(6, np.nan),
|
|
34
|
+
"vmax": np.abs(np.diff(q[:, :6], axis=0)).max(0) * fps,
|
|
35
|
+
"gmin": q[:, 6].min(), "gmax": q[:, 6].max(), "dt_max": dt.max(), "dt_late": int((dt > 1.5 / fps).sum()),
|
|
36
|
+
"repeat": {c: int(d[f"{c}_repeat"].sum()) for c in cams if f"{c}_repeat" in d.files},
|
|
37
|
+
"nan": int(np.isnan(q).sum() + np.isnan(a).sum())}
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
def run(cfg: Config, args):
|
|
41
|
+
data = expand(args.data) if args.data else cfg.raw_dir()
|
|
42
|
+
files = sorted(data.glob("episode_*.npz"))
|
|
43
|
+
if not files:
|
|
44
|
+
sys.exit(f"no episode_*.npz in {data}")
|
|
45
|
+
fps, cams = float(cfg["record"]["fps"]), list(cfg["cameras"])
|
|
46
|
+
np.set_printoptions(precision=1, suppress=True, linewidth=200)
|
|
47
|
+
rows = []
|
|
48
|
+
print("ep frames lag p95 tracking error J1..J6 (deg) offset while still (deg) gripper range dt max late repeats")
|
|
49
|
+
for f in files:
|
|
50
|
+
idx = int(f.stem.split("_")[-1])
|
|
51
|
+
with np.load(f) as d:
|
|
52
|
+
r = episode_stats(d, fps, cams)
|
|
53
|
+
r["idx"] = idx
|
|
54
|
+
rows.append(r)
|
|
55
|
+
rep = "/".join(str(v) for v in r["repeat"].values())
|
|
56
|
+
print(f"{idx:03d} {r['n']:6d} {r['lag']:3d} {r['p95']} {r['still']} {r['gmin']:6.1f}/{r['gmax']:6.1f}"
|
|
57
|
+
f" {r['dt_max'] * 1e3:5.0f}ms {r['dt_late']:4d} {rep}" + (f" NaN {r['nan']}" if r["nan"] else ""))
|
|
58
|
+
if args.split is not None:
|
|
59
|
+
a = [r for r in rows if r["idx"] < args.split]
|
|
60
|
+
b = [r for r in rows if r["idx"] >= args.split]
|
|
61
|
+
if not a or not b:
|
|
62
|
+
sys.exit(f"--split {args.split} leaves one side empty")
|
|
63
|
+
print(f"\nmedian over episodes, before {args.split} ({len(a)}) vs from {args.split} ({len(b)}):")
|
|
64
|
+
for label, k in [("p95 tracking", "p95"), ("max error", "max"), ("still offset", "still"), ("max speed", "vmax")]:
|
|
65
|
+
print(f" {label:13s} {np.median([r[k] for r in a], 0)} -> {np.median([r[k] for r in b], 0)}")
|
|
66
|
+
for label, k in [("gripper min", "gmin"), ("gripper max", "gmax")]:
|
|
67
|
+
print(f" {label:13s} {np.median([r[k] for r in a]):7.1f} -> {np.median([r[k] for r in b]):7.1f}")
|
macbot/default.yaml
ADDED
|
@@ -0,0 +1,55 @@
|
|
|
1
|
+
# macbot configuration. Angles are in degrees (LeRobot public joint coordinates).
|
|
2
|
+
# Built-in defaults. Override any subset in your own YAML: `macbot --config my.yaml <command>`.
|
|
3
|
+
|
|
4
|
+
task:
|
|
5
|
+
name: cubes_to_box # dataset name; data goes to <data_root>/raw/<name>/
|
|
6
|
+
instruction: "put the cubes into the paper box" # language prompt (used by pi0.5); editable per episode in the web UI
|
|
7
|
+
target_episodes: 50 # progress bar target
|
|
8
|
+
|
|
9
|
+
data_root: ~/macbot-data # raw/, datasets/, rollouts/, logs/ live here
|
|
10
|
+
|
|
11
|
+
robot:
|
|
12
|
+
type: rebot_b601 # reBot B601-RS follower + reBot Arm 102 leader
|
|
13
|
+
can_channel: can0 # PCAN-USB through PCBUSB on macOS
|
|
14
|
+
follower_id: follower1 # LeRobot calibration ids (files under ~/.cache/huggingface/lerobot/calibration/)
|
|
15
|
+
leader_id: rebot_arm_102_leader
|
|
16
|
+
velocity_feedforward: 0.7 # 0 = LeRobot default (target velocity 0 in the MIT command)
|
|
17
|
+
gravity_compensation: 0.9 # 0 = off; needs robot.urdf
|
|
18
|
+
urdf: null # path to ReBot_Arm_RS.urdf (from Seeed's reBot description package)
|
|
19
|
+
gripper_mode: mit # mit (position control with a lead clamp) | impedance (LeRobot default)
|
|
20
|
+
gripper_lead_deg: 20.0 # max lead of the gripper target over the measured angle (limits squeeze force)
|
|
21
|
+
gripper_zero_offset_deg: 0.0 # software zero trim for the gripper (reading and command shift together)
|
|
22
|
+
|
|
23
|
+
cameras:
|
|
24
|
+
top: # fixed scene camera, any UVC webcam
|
|
25
|
+
type: uvc
|
|
26
|
+
unique_id: "0x0000000000000000" # run `macbot cameras` to list IDs; opening by name can grab an iPhone
|
|
27
|
+
crop: [625, 140, 1610, 1080] # x0 y0 x1 y1 on the 1920x1080 frame
|
|
28
|
+
save_size: [492, 470] # stored (width, height)
|
|
29
|
+
wrist:
|
|
30
|
+
type: orbbec # Orbbec Gemini 2 color stream (needs sudo on macOS)
|
|
31
|
+
expect_size: [1280, 720]
|
|
32
|
+
save_size: [640, 360]
|
|
33
|
+
|
|
34
|
+
record:
|
|
35
|
+
fps: 30 # fixed sampling grid
|
|
36
|
+
jpeg_quality: 92
|
|
37
|
+
min_episode_s: 2.0
|
|
38
|
+
|
|
39
|
+
teleop:
|
|
40
|
+
port: 8766
|
|
41
|
+
hz: 60 # leader -> follower control rate
|
|
42
|
+
align_max_deg: 10 # refuse to start if leader and follower differ more than this
|
|
43
|
+
soft_start_s: 1.5
|
|
44
|
+
|
|
45
|
+
checks: # run on every saved episode, shown in the web UI
|
|
46
|
+
max_cam_age_s: 0.10
|
|
47
|
+
max_repeat_ratio: 0.25
|
|
48
|
+
max_joint_step_deg: 15
|
|
49
|
+
max_late_ratio: 0.05
|
|
50
|
+
|
|
51
|
+
run:
|
|
52
|
+
port: 8767
|
|
53
|
+
hz: 60
|
|
54
|
+
speed: 0.5 # global slow-down for first runs (0.1 - 1.0)
|
|
55
|
+
max_vel: 120.0 # deg/s per arm joint, multiplied by speed
|