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/policy/runner.py
ADDED
|
@@ -0,0 +1,345 @@
|
|
|
1
|
+
"""Run a trained policy on the robot, driven from a local web page.
|
|
2
|
+
|
|
3
|
+
Shares cameras, follower settings and the safe shutdown with ``macbot collect``. Observations match
|
|
4
|
+
training: images resized to the recorded size and passed through JPEG, state = measured joints (deg).
|
|
5
|
+
|
|
6
|
+
Safety: starts paused; arm joints move at most ``max_vel x speed`` deg/s; joint limits; gripper force
|
|
7
|
+
clamp; a camera that stalls for 0.5 s pauses the run; pause holds the current command; "home" moves
|
|
8
|
+
slowly to the zero pose; quitting returns to zero in two stages before disabling.
|
|
9
|
+
|
|
10
|
+
Each trial is saved as ``<data_root>/rollouts/<task>/rollout_XXX.npz``: qpos, cmd, t, chunk_id per
|
|
11
|
+
frame, chunk_t and infer_ms per chunk, executor extras, JPEG frames per camera, and meta (result).
|
|
12
|
+
"""
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
|
|
15
|
+
import json
|
|
16
|
+
import re
|
|
17
|
+
import sys
|
|
18
|
+
import threading
|
|
19
|
+
import time
|
|
20
|
+
from http.server import ThreadingHTTPServer
|
|
21
|
+
from pathlib import Path
|
|
22
|
+
|
|
23
|
+
import numpy as np
|
|
24
|
+
|
|
25
|
+
from .. import web
|
|
26
|
+
from ..cameras import open_cameras
|
|
27
|
+
from ..config import ARM, JOINTS, Config
|
|
28
|
+
from ..i18n import tr
|
|
29
|
+
from ..system import ensure_dir, expand, give_back, start_log
|
|
30
|
+
from .executors import AsyncChunk, Ensemble, SyncChunk
|
|
31
|
+
from .gripper import BinaryGripper, find_gripper_config
|
|
32
|
+
from .models import load_policy, prep_image
|
|
33
|
+
|
|
34
|
+
CAM_STALL_S = 0.5
|
|
35
|
+
HOME_POSE = {n: 0.0 for n in JOINTS}
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
class State:
|
|
39
|
+
def __init__(self, cfg: Config, out: Path, args):
|
|
40
|
+
self.cfg, self.out, self.args = cfg, out, args
|
|
41
|
+
self.cam_names = list(cfg["cameras"])
|
|
42
|
+
self.lock = threading.Lock()
|
|
43
|
+
self.mode = "paused" # paused | running | homing
|
|
44
|
+
self.want = None # request from the page
|
|
45
|
+
self.msg = tr("Paused. Set up the scene, make sure it is safe, then press Start.",
|
|
46
|
+
"已暂停。场景摆好、确认安全后按“开始”。")
|
|
47
|
+
self.q, self.cmd = [0.0] * 7, [0.0] * 7
|
|
48
|
+
self.cams, self.chunks, self.infer_ms = {}, 0, []
|
|
49
|
+
self.quit = False
|
|
50
|
+
self.rec = None
|
|
51
|
+
self.saved = []
|
|
52
|
+
self.executor = ""
|
|
53
|
+
|
|
54
|
+
def next_idx(self):
|
|
55
|
+
ids = [int(m.group(1)) for p in self.out.glob("rollout_*.npz") if (m := re.search(r"_(\d+)\.npz$", p.name))]
|
|
56
|
+
return max(ids, default=-1) + 1
|
|
57
|
+
|
|
58
|
+
def log_chunk(self, t_chunk, ms, **extra):
|
|
59
|
+
"""Called by executors after every inference."""
|
|
60
|
+
with self.lock:
|
|
61
|
+
self.infer_ms.append(ms)
|
|
62
|
+
if extra.get("switched", 1):
|
|
63
|
+
self.chunks += 1
|
|
64
|
+
r = self.rec
|
|
65
|
+
if r is not None:
|
|
66
|
+
if t_chunk is not None:
|
|
67
|
+
r["chunk_t"].append(t_chunk - r["t0"])
|
|
68
|
+
r["infer_ms"].append(ms)
|
|
69
|
+
for k, v in extra.items():
|
|
70
|
+
r.setdefault(f"ex_{k}", []).append(v)
|
|
71
|
+
|
|
72
|
+
def status(self):
|
|
73
|
+
with self.lock:
|
|
74
|
+
now = time.time()
|
|
75
|
+
ages = {c: (round(now - self.cams[c].latest()[1], 2) if c in self.cams else None) for c in self.cam_names}
|
|
76
|
+
r = self.rec
|
|
77
|
+
return {"mode": self.mode, "msg": self.msg, "q": self.q, "cmd": self.cmd, "names": JOINTS,
|
|
78
|
+
"cam_names": self.cam_names, "cam_age": ages,
|
|
79
|
+
"cam_size": {c: self.cfg["cameras"][c]["save_size"] for c in self.cam_names}, "stall_s": CAM_STALL_S, "chunks": self.chunks,
|
|
80
|
+
"infer_ms": round(float(np.median(self.infer_ms[-20:])), 1) if self.infer_ms else None,
|
|
81
|
+
"executor": self.executor, "speed": self.args.speed, "fake": self.args.fake,
|
|
82
|
+
"policy": "/".join(Path(self.args.policy).parts[-2:]), "task": self.cfg.task,
|
|
83
|
+
"rec_s": round(len(r["t"]) / self.cfg["record"]["fps"], 1) if r else 0,
|
|
84
|
+
"saved": self.saved[-15:], "next": self.next_idx()}
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
def save_rollout(st: State, rec: dict, result: str):
|
|
88
|
+
p = st.out / f"rollout_{st.next_idx():03d}.npz"
|
|
89
|
+
arrs = {}
|
|
90
|
+
for c in st.cam_names:
|
|
91
|
+
js = rec[f"{c}_jpg"]
|
|
92
|
+
arrs[f"{c}_offsets"] = np.cumsum([0] + [len(j) for j in js]).astype(np.int64)
|
|
93
|
+
arrs[f"{c}_jpeg"] = np.concatenate(js) if js else np.zeros(0, np.uint8)
|
|
94
|
+
a = st.args
|
|
95
|
+
fps = st.cfg["record"]["fps"]
|
|
96
|
+
meta = {"task": st.cfg.task, "instruction": st.cfg["task"]["instruction"], "policy": str(a.policy),
|
|
97
|
+
"executor": st.executor, "mode": a.mode, "k": a.k, "te_m": a.te_m, "rtc_beta": a.rtc_beta,
|
|
98
|
+
"grip_debounce": a.grip_debounce, "speed": a.speed, "max_vel": st.cfg["run"]["max_vel"], "result": result,
|
|
99
|
+
"fps": fps, "frames": len(rec["t"]), "joint_names": JOINTS, "cameras": st.cam_names,
|
|
100
|
+
"saved_at": time.strftime("%Y-%m-%d %H:%M:%S"), "fake": a.fake, "writer": "macbot"}
|
|
101
|
+
ex = {k: np.asarray(v, np.float64) for k, v in rec.items() if k.startswith("ex_")}
|
|
102
|
+
tmp = p.with_suffix(".tmp.npz")
|
|
103
|
+
np.savez(tmp, qpos=np.asarray(rec["q"], np.float32), cmd=np.asarray(rec["cmd"], np.float32),
|
|
104
|
+
t=np.asarray(rec["t"], np.float64), chunk_id=np.asarray(rec["chunk"], np.int32),
|
|
105
|
+
chunk_t=np.asarray(rec["chunk_t"], np.float64), infer_ms=np.asarray(rec["infer_ms"], np.float32),
|
|
106
|
+
gp=np.asarray(rec["gp"], np.float32), meta=json.dumps(meta, ensure_ascii=False), **ex, **arrs)
|
|
107
|
+
tmp.rename(p)
|
|
108
|
+
give_back(p)
|
|
109
|
+
return p.name, meta
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
def make_handler(st: State):
|
|
113
|
+
preview = {k: tuple(st.cfg["cameras"][k]["save_size"]) for k in st.cam_names}
|
|
114
|
+
|
|
115
|
+
class H(web.Handler):
|
|
116
|
+
def do_GET(self):
|
|
117
|
+
path = self.path.split("?")[0]
|
|
118
|
+
if path == "/":
|
|
119
|
+
return self.send(200, web.page("run", task=st.cfg.task), "text/html; charset=utf-8")
|
|
120
|
+
if self.static(path):
|
|
121
|
+
return
|
|
122
|
+
if path == "/api/status":
|
|
123
|
+
return self.json(st.status())
|
|
124
|
+
m = re.fullmatch(r"/stream/(\w+)\.mjpg", path)
|
|
125
|
+
if m and m.group(1) in st.cams:
|
|
126
|
+
return self.mjpeg(st.cams[m.group(1)], preview[m.group(1)], lambda: st.quit)
|
|
127
|
+
self.send(404, b"not found", "text/plain")
|
|
128
|
+
|
|
129
|
+
def do_POST(self):
|
|
130
|
+
if self.path != "/api/cmd":
|
|
131
|
+
return self.send(404, b"", "text/plain")
|
|
132
|
+
try:
|
|
133
|
+
act = self.body_json().get("action")
|
|
134
|
+
if act in ("start", "pause", "home", "save_ok", "save_fail", "discard", "quit"):
|
|
135
|
+
with st.lock:
|
|
136
|
+
st.want = act
|
|
137
|
+
except Exception as e:
|
|
138
|
+
with st.lock:
|
|
139
|
+
st.msg = tr(f"Command failed: {e}", f"操作失败:{e}")
|
|
140
|
+
self.json(st.status())
|
|
141
|
+
|
|
142
|
+
return H
|
|
143
|
+
|
|
144
|
+
|
|
145
|
+
def run(cfg: Config, args):
|
|
146
|
+
start_log(cfg.log_dir, "run")
|
|
147
|
+
rc = cfg["run"]
|
|
148
|
+
args.speed = args.speed if args.speed is not None else rc["speed"]
|
|
149
|
+
if not 0.1 <= args.speed <= 1.0:
|
|
150
|
+
sys.exit("--speed must be between 0.1 and 1.0")
|
|
151
|
+
if args.autostart and not args.fake:
|
|
152
|
+
sys.exit("--autostart only works with --fake")
|
|
153
|
+
args.policy = expand(args.policy)
|
|
154
|
+
gcfg = find_gripper_config(args.policy, args.gripper)
|
|
155
|
+
gripper = BinaryGripper(gcfg, args.grip_debounce) if gcfg else None
|
|
156
|
+
|
|
157
|
+
fps = int(cfg["record"]["fps"])
|
|
158
|
+
out = ensure_dir(cfg.rollout_dir(cfg.task + ("_fake" if args.fake else "")))
|
|
159
|
+
st = State(cfg, out, args)
|
|
160
|
+
port = args.port or rc["port"]
|
|
161
|
+
srv = ThreadingHTTPServer(("127.0.0.1", port), make_handler(st))
|
|
162
|
+
srv.daemon_threads = True
|
|
163
|
+
|
|
164
|
+
print(f"loading policy {args.policy} on {args.device}", flush=True)
|
|
165
|
+
pol = load_policy(args.policy, args.device, cfg["task"]["instruction"], args.steps)
|
|
166
|
+
if pol.kind == "pi05" and not args.no_compile:
|
|
167
|
+
pol.enable_compile()
|
|
168
|
+
print("pi0.5: torch.compile on, the first inference compiles (~25 s)...", flush=True)
|
|
169
|
+
if args.mode == "auto":
|
|
170
|
+
args.mode = "rtc" if pol.kind == "pi05" else "ensemble"
|
|
171
|
+
if args.mode == "rtc" and not pol.supports_rtc:
|
|
172
|
+
sys.exit("--mode rtc needs pi0.5")
|
|
173
|
+
sizes = {c: tuple(cfg["cameras"][c]["save_size"]) for c in st.cam_names}
|
|
174
|
+
for c in (st.cam_names if pol.shapes is not None else []):
|
|
175
|
+
want = (3, sizes[c][1], sizes[c][0])
|
|
176
|
+
if pol.shapes.get(c) != want:
|
|
177
|
+
sys.exit(f"policy input {c} is {pol.shapes.get(c)}, but the camera config gives {want}")
|
|
178
|
+
jq = int(cfg["record"]["jpeg_quality"])
|
|
179
|
+
dummy = {c: np.zeros((sizes[c][1], sizes[c][0], 3), np.uint8) for c in st.cam_names}
|
|
180
|
+
for _ in range(3): # warm-up (MPS is slow at first; compile happens here)
|
|
181
|
+
t = time.perf_counter()
|
|
182
|
+
pol(dummy, [0.0] * 7)
|
|
183
|
+
print(f"policy ready ({pol.kind}, chunk {pol.H}): {(time.perf_counter() - t) * 1e3:.0f} ms per inference", flush=True)
|
|
184
|
+
if args.mode == "rtc":
|
|
185
|
+
pol.enable_rtc(args.rtc_beta)
|
|
186
|
+
pol(dummy, [0.0] * 7) # unguided call (first chunk) compiles separately
|
|
187
|
+
lo = np.zeros((pol.H, 7), np.float32)
|
|
188
|
+
pol(dummy, [0.0] * 7, leftover=lo, delay=8)
|
|
189
|
+
t = time.perf_counter()
|
|
190
|
+
pol(dummy, [0.0] * 7, leftover=lo, delay=8)
|
|
191
|
+
print(f"rtc warm-up: {(time.perf_counter() - t) * 1e3:.0f} ms per guided inference", flush=True)
|
|
192
|
+
|
|
193
|
+
st.cams = open_cameras(cfg["cameras"], fps, args.fake)
|
|
194
|
+
cams = st.cams
|
|
195
|
+
|
|
196
|
+
def observe():
|
|
197
|
+
imgs = {c: prep_image(cams[c].latest()[0], sizes[c], jq)[0] for c in st.cam_names}
|
|
198
|
+
with st.lock:
|
|
199
|
+
return imgs, list(st.q)
|
|
200
|
+
|
|
201
|
+
rate = fps * args.speed
|
|
202
|
+
stop = lambda: st.quit # noqa: E731
|
|
203
|
+
if args.mode == "chunk":
|
|
204
|
+
ex = SyncChunk(pol, observe, rate, st.log_chunk, args.k)
|
|
205
|
+
elif args.mode == "ensemble":
|
|
206
|
+
ex = Ensemble(pol, observe, rate, st.log_chunk, args.te_m, args.te_hz, stop)
|
|
207
|
+
else:
|
|
208
|
+
ex = AsyncChunk(pol, observe, rate, st.log_chunk, guide=args.mode == "rtc", horizon=args.rtc_horizon, stop=stop)
|
|
209
|
+
ex.start()
|
|
210
|
+
st.executor = ex.describe()
|
|
211
|
+
threading.Thread(target=srv.serve_forever, daemon=True).start()
|
|
212
|
+
print(f"executor: {st.executor}\nweb page: http://127.0.0.1:{port} rollouts: {out}", flush=True)
|
|
213
|
+
|
|
214
|
+
from ..robot.rebot import clip_gripper
|
|
215
|
+
if args.fake:
|
|
216
|
+
from ..robot import FakeFollower
|
|
217
|
+
robot = FakeFollower()
|
|
218
|
+
else:
|
|
219
|
+
from ..robot.rebot import make_follower
|
|
220
|
+
robot = make_follower(cfg["robot"])
|
|
221
|
+
robot.connect(calibrate=False)
|
|
222
|
+
lim = robot.config.joint_limits
|
|
223
|
+
lead = float(cfg["robot"]["gripper_lead_deg"])
|
|
224
|
+
started = False
|
|
225
|
+
hz = rc["hz"]
|
|
226
|
+
dt = 1.0 / hz
|
|
227
|
+
vmax = rc["max_vel"] * args.speed * dt # max arm joint step per control cycle (deg)
|
|
228
|
+
|
|
229
|
+
def clamp(n, v):
|
|
230
|
+
return min(lim[n][1], max(lim[n][0], v))
|
|
231
|
+
|
|
232
|
+
def say(en, zh, mode=None):
|
|
233
|
+
with st.lock:
|
|
234
|
+
st.msg = tr(en, zh)
|
|
235
|
+
if mode:
|
|
236
|
+
st.mode = mode
|
|
237
|
+
|
|
238
|
+
try:
|
|
239
|
+
o = robot.get_observation()
|
|
240
|
+
cmd = {n: float(o[f"{n}.pos"]) for n in JOINTS} # start from the measured pose (no jump)
|
|
241
|
+
started = True
|
|
242
|
+
chunk_id, gp_last = -1, float("nan")
|
|
243
|
+
next_rec = time.time()
|
|
244
|
+
auto = [(0.0, "start"), (args.autostart, "save_fail"), (args.autostart + 0.5, "quit")] if args.autostart else []
|
|
245
|
+
auto_t0 = time.time()
|
|
246
|
+
print("ready (paused)", flush=True)
|
|
247
|
+
while True:
|
|
248
|
+
tick = time.perf_counter()
|
|
249
|
+
now = time.time()
|
|
250
|
+
if auto and now - auto_t0 >= auto[0][0]:
|
|
251
|
+
with st.lock:
|
|
252
|
+
st.want = auto.pop(0)[1]
|
|
253
|
+
with st.lock:
|
|
254
|
+
want, st.want = st.want, None
|
|
255
|
+
if want == "quit":
|
|
256
|
+
break
|
|
257
|
+
stalled = [c for c in st.cam_names if now - cams[c].latest()[1] > CAM_STALL_S]
|
|
258
|
+
if want == "start":
|
|
259
|
+
if stalled:
|
|
260
|
+
say(f"No frames from {', '.join(stalled)}; cannot start.", f"{'、'.join(stalled)} 相机没有画面,不能开始。")
|
|
261
|
+
else:
|
|
262
|
+
say("Policy running. Space pauses.", "策略运行中。空格暂停。", "running")
|
|
263
|
+
with st.lock:
|
|
264
|
+
if st.rec is None:
|
|
265
|
+
st.rec = {"q": [], "cmd": [], "t": [], "chunk": [], "chunk_t": [], "infer_ms": [], "gp": [],
|
|
266
|
+
"t0": now, **{f"{c}_jpg": [] for c in st.cam_names}}
|
|
267
|
+
ex.reset(True) # every start re-plans from the current observation
|
|
268
|
+
elif want == "pause":
|
|
269
|
+
say("Paused (holding the current command).", "已暂停(保持当前指令)。", "paused")
|
|
270
|
+
elif want == "home":
|
|
271
|
+
say("Moving slowly to the start pose...", "正在慢速回到起始姿态…", "homing")
|
|
272
|
+
elif want in ("save_ok", "save_fail", "discard"):
|
|
273
|
+
with st.lock:
|
|
274
|
+
rec, st.rec, st.mode = st.rec, None, "paused"
|
|
275
|
+
if rec and want != "discard" and rec["t"]:
|
|
276
|
+
name, meta = save_rollout(st, rec, "success" if want == "save_ok" else "failure")
|
|
277
|
+
with st.lock:
|
|
278
|
+
st.saved.append({"name": name, "result": meta["result"], "s": round(meta["frames"] / fps, 1)})
|
|
279
|
+
ok = want == "save_ok"
|
|
280
|
+
say(f"Saved {name} ({'success' if ok else 'failure'}). Reset the scene and press Start.",
|
|
281
|
+
f"已保存 {name}({'成功' if ok else '失败'})。复位场景后按开始。")
|
|
282
|
+
print(f"saved {name} {meta['result']} {meta['frames']} frames", flush=True)
|
|
283
|
+
else:
|
|
284
|
+
say("Trial discarded. Reset the scene and press Start.", "这次试验已丢弃。复位场景后按开始。")
|
|
285
|
+
with st.lock:
|
|
286
|
+
mode = st.mode
|
|
287
|
+
if ex.err:
|
|
288
|
+
say(f"Inference failed, paused: {ex.err}", f"推理出错,已暂停:{ex.err}", "paused")
|
|
289
|
+
print(f"inference error: {ex.err}", flush=True)
|
|
290
|
+
ex.err, mode = None, "paused"
|
|
291
|
+
if mode != "running" and getattr(ex, "active", True):
|
|
292
|
+
ex.reset(False)
|
|
293
|
+
if mode == "running" and stalled:
|
|
294
|
+
say(f"Camera stalled ({', '.join(stalled)}), paused.", f"相机断流({'、'.join(stalled)}),已自动暂停。", "paused")
|
|
295
|
+
mode = "paused"
|
|
296
|
+
|
|
297
|
+
o = robot.get_observation()
|
|
298
|
+
q = [float(o[f"{n}.pos"]) for n in JOINTS]
|
|
299
|
+
with st.lock:
|
|
300
|
+
st.q = q
|
|
301
|
+
target = dict(cmd)
|
|
302
|
+
if mode == "running":
|
|
303
|
+
a, chunk_id = ex.target(time.time())
|
|
304
|
+
if a is not None:
|
|
305
|
+
target = {n: float(a[j]) for j, n in enumerate(JOINTS)}
|
|
306
|
+
gp_last = target["gripper"]
|
|
307
|
+
if gripper is not None:
|
|
308
|
+
target["gripper"] = gripper(gp_last, now)
|
|
309
|
+
elif mode == "homing":
|
|
310
|
+
target = dict(HOME_POSE)
|
|
311
|
+
if all(abs(cmd[n] - HOME_POSE[n]) < 0.5 for n in ARM):
|
|
312
|
+
say("Back at the start pose. Reset the scene and press Start.", "已回到起始姿态。复位场景后按开始。", "paused")
|
|
313
|
+
|
|
314
|
+
for n in ARM: # velocity limit + joint limits
|
|
315
|
+
cmd[n] = clamp(n, cmd[n] + max(-vmax, min(vmax, target[n] - cmd[n])))
|
|
316
|
+
gstep = 300.0 * args.speed * dt if mode == "homing" else 1e9
|
|
317
|
+
cmd["gripper"] = clamp("gripper", cmd["gripper"] + max(-gstep, min(gstep, target["gripper"] - cmd["gripper"])))
|
|
318
|
+
sent = robot.send_action(clip_gripper({f"{n}.pos": cmd[n] for n in JOINTS}, q[6], lead))
|
|
319
|
+
sent_v = [float(sent.get(f"{n}.pos", cmd[n])) for n in JOINTS]
|
|
320
|
+
with st.lock:
|
|
321
|
+
st.cmd = sent_v
|
|
322
|
+
rec = st.rec if st.mode == "running" else None
|
|
323
|
+
if rec is not None and now >= next_rec: # record at fps
|
|
324
|
+
next_rec = max(next_rec + 1 / fps, now)
|
|
325
|
+
rec["q"].append(q)
|
|
326
|
+
rec["cmd"].append(sent_v)
|
|
327
|
+
rec["t"].append(now - rec["t0"])
|
|
328
|
+
rec["chunk"].append(chunk_id)
|
|
329
|
+
rec["gp"].append(gp_last)
|
|
330
|
+
for c in st.cam_names:
|
|
331
|
+
rec[f"{c}_jpg"].append(prep_image(cams[c].latest()[0], sizes[c], jq)[1])
|
|
332
|
+
time.sleep(max(0.0, dt - (time.perf_counter() - tick)))
|
|
333
|
+
except KeyboardInterrupt:
|
|
334
|
+
pass
|
|
335
|
+
finally:
|
|
336
|
+
st.quit = True
|
|
337
|
+
try:
|
|
338
|
+
if not args.fake:
|
|
339
|
+
from ..robot.rebot import safe_disconnect
|
|
340
|
+
safe_disconnect(robot, go_zero=started)
|
|
341
|
+
finally:
|
|
342
|
+
for c in cams.values():
|
|
343
|
+
c.close()
|
|
344
|
+
srv.shutdown()
|
|
345
|
+
print("done", flush=True)
|
macbot/robot/__init__.py
ADDED
macbot/robot/fake.py
ADDED
|
@@ -0,0 +1,63 @@
|
|
|
1
|
+
"""Simulated follower and leader for `--fake` runs: no hardware, same interface."""
|
|
2
|
+
from __future__ import annotations
|
|
3
|
+
|
|
4
|
+
import math
|
|
5
|
+
import time
|
|
6
|
+
|
|
7
|
+
from ..config import JOINTS
|
|
8
|
+
|
|
9
|
+
LIMITS = {"shoulder_pan": (-145, 145), "shoulder_lift": (-170, 0), "elbow_flex": (-200, 0),
|
|
10
|
+
"wrist_flex": (-90, 80), "wrist_yaw": (-90, 90), "wrist_roll": (-90, 90), "gripper": (-270, 0)}
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
class FakeFollower:
|
|
14
|
+
"""First-order lag per joint (time constant 70 ms), roughly like the real servo loop."""
|
|
15
|
+
|
|
16
|
+
class _Cfg:
|
|
17
|
+
joint_limits = LIMITS
|
|
18
|
+
|
|
19
|
+
def __init__(self, tau: float = 0.07):
|
|
20
|
+
self.config, self.tau = self._Cfg(), tau
|
|
21
|
+
self.pos = {n: 0.0 for n in JOINTS}
|
|
22
|
+
self.cmd = dict(self.pos)
|
|
23
|
+
self.t = None
|
|
24
|
+
self.feedforward = {"fake": True}
|
|
25
|
+
|
|
26
|
+
def connect(self, calibrate=False):
|
|
27
|
+
pass
|
|
28
|
+
|
|
29
|
+
def send_action(self, a):
|
|
30
|
+
for k, v in a.items():
|
|
31
|
+
n = k.removesuffix(".pos")
|
|
32
|
+
lo, hi = LIMITS[n]
|
|
33
|
+
self.cmd[n] = max(lo, min(hi, float(v)))
|
|
34
|
+
return {f"{n}.pos": self.cmd[n] for n in JOINTS}
|
|
35
|
+
|
|
36
|
+
def get_observation(self):
|
|
37
|
+
now = time.perf_counter()
|
|
38
|
+
dt = 0.0 if self.t is None else now - self.t
|
|
39
|
+
self.t = now
|
|
40
|
+
for n in JOINTS:
|
|
41
|
+
self.pos[n] += (self.cmd[n] - self.pos[n]) * min(1.0, dt / self.tau)
|
|
42
|
+
return {f"{n}.pos": self.pos[n] for n in JOINTS}
|
|
43
|
+
|
|
44
|
+
def disconnect(self):
|
|
45
|
+
pass
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
class FakeLeader:
|
|
49
|
+
"""Leader that waves slowly."""
|
|
50
|
+
|
|
51
|
+
def connect(self, calibrate=False):
|
|
52
|
+
self.t0 = time.time()
|
|
53
|
+
|
|
54
|
+
def get_action(self):
|
|
55
|
+
t = time.time() - self.t0
|
|
56
|
+
a = {f"{n}.pos": 25 * math.sin(0.4 * t + i) * (i < 6) for i, n in enumerate(JOINTS)}
|
|
57
|
+
a["shoulder_lift.pos"] -= 40
|
|
58
|
+
a["elbow_flex.pos"] -= 60
|
|
59
|
+
a["gripper.pos"] = -30 + 30 * math.sin(0.7 * t)
|
|
60
|
+
return a
|
|
61
|
+
|
|
62
|
+
def disconnect(self):
|
|
63
|
+
pass
|
macbot/robot/gravity.py
ADDED
|
@@ -0,0 +1,108 @@
|
|
|
1
|
+
"""Static gravity torque model of the reBot B601-RS, read from the URDF.
|
|
2
|
+
|
|
3
|
+
``gravity_torque_public(q_deg)`` returns the torque (N*m) each of the 6 arm joints must produce
|
|
4
|
+
to hold the arm still, in LeRobot public joint coordinates. URDF angles relate to public
|
|
5
|
+
angles by ``q_urdf = SIGN * q_pub`` with the same zero. SIGN was fitted on static frames of
|
|
6
|
+
teleoperation logs: model/measured ratio 0.98 (shoulder) and 0.90 (elbow).
|
|
7
|
+
|
|
8
|
+
The URDF is not redistributed here. Get ``ReBot_Arm_RS.urdf`` from Seeed's reBot description
|
|
9
|
+
package and set ``robot.urdf`` in your config.
|
|
10
|
+
"""
|
|
11
|
+
import math
|
|
12
|
+
import xml.etree.ElementTree as ET
|
|
13
|
+
|
|
14
|
+
import numpy as np
|
|
15
|
+
|
|
16
|
+
ARM = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
|
|
17
|
+
G = np.array([0.0, 0.0, -9.81])
|
|
18
|
+
SIGN = np.array([1.0, -1.0, -1.0, -1.0, -1.0, 1.0])
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
def _rpy(r, p, y):
|
|
22
|
+
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
|
|
23
|
+
return np.array([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
|
24
|
+
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
|
25
|
+
[-sp, cp * sr, cp * cr]])
|
|
26
|
+
|
|
27
|
+
|
|
28
|
+
def _axis_rot(axis, th):
|
|
29
|
+
a = axis / np.linalg.norm(axis)
|
|
30
|
+
K = np.array([[0, -a[2], a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]])
|
|
31
|
+
return np.eye(3) + math.sin(th) * K + (1 - math.cos(th)) * K @ K
|
|
32
|
+
|
|
33
|
+
|
|
34
|
+
def _f(s):
|
|
35
|
+
return np.array([float(v) for v in s.split()])
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
class GravityModel:
|
|
39
|
+
def __init__(self, urdf, sign=None):
|
|
40
|
+
root = ET.parse(urdf).getroot()
|
|
41
|
+
self.links = {}
|
|
42
|
+
for l in root.findall("link"):
|
|
43
|
+
i = l.find("inertial")
|
|
44
|
+
if i is not None:
|
|
45
|
+
self.links[l.get("name")] = (float(i.find("mass").get("value")), _f(i.find("origin").get("xyz")))
|
|
46
|
+
self.joints = [] # kinematic chain from base_link (gripper fingers treated as fixed)
|
|
47
|
+
for j in root.findall("joint"):
|
|
48
|
+
o = j.find("origin")
|
|
49
|
+
a = j.find("axis")
|
|
50
|
+
self.joints.append(dict(
|
|
51
|
+
name=j.get("name"), type=j.get("type"), parent=j.find("parent").get("link"),
|
|
52
|
+
child=j.find("child").get("link"), xyz=_f(o.get("xyz")), R=_rpy(*_f(o.get("rpy"))),
|
|
53
|
+
axis=_f(a.get("xyz")) if a is not None else np.zeros(3)))
|
|
54
|
+
self.sign = np.array(SIGN if sign is None else sign, dtype=float)
|
|
55
|
+
|
|
56
|
+
def _fk(self, q_urdf):
|
|
57
|
+
"""(R, p) of every link, and (world axis, world origin) of every arm joint."""
|
|
58
|
+
frames = {"base_link": (np.eye(3), np.zeros(3))}
|
|
59
|
+
jinfo = {}
|
|
60
|
+
qmap = dict(zip(ARM, q_urdf))
|
|
61
|
+
pending = list(self.joints)
|
|
62
|
+
while pending:
|
|
63
|
+
for j in list(pending):
|
|
64
|
+
if j["parent"] not in frames:
|
|
65
|
+
continue
|
|
66
|
+
Rp, pp = frames[j["parent"]]
|
|
67
|
+
Rj = Rp @ j["R"]
|
|
68
|
+
pj = pp + Rp @ j["xyz"]
|
|
69
|
+
if j["type"] == "revolute":
|
|
70
|
+
th = qmap.get(j["name"], 0.0)
|
|
71
|
+
jinfo[j["name"]] = (Rj @ (j["axis"] / np.linalg.norm(j["axis"])), pj)
|
|
72
|
+
Rj = Rj @ _axis_rot(j["axis"], th)
|
|
73
|
+
frames[j["child"]] = (Rj, pj)
|
|
74
|
+
pending.remove(j)
|
|
75
|
+
return frames, jinfo
|
|
76
|
+
|
|
77
|
+
def _descendants(self, joint_name):
|
|
78
|
+
children = {}
|
|
79
|
+
for j in self.joints:
|
|
80
|
+
children.setdefault(j["parent"], []).append(j["child"])
|
|
81
|
+
start = next(j["child"] for j in self.joints if j["name"] == joint_name)
|
|
82
|
+
out, stack = [], [start]
|
|
83
|
+
while stack:
|
|
84
|
+
k = stack.pop()
|
|
85
|
+
out.append(k)
|
|
86
|
+
stack += children.get(k, [])
|
|
87
|
+
return out
|
|
88
|
+
|
|
89
|
+
def torque_urdf(self, q_urdf):
|
|
90
|
+
"""Torque each motor must produce to hold the arm against gravity (URDF frame)."""
|
|
91
|
+
frames, jinfo = self._fk(q_urdf)
|
|
92
|
+
tau = np.zeros(len(ARM))
|
|
93
|
+
for i, jn in enumerate(ARM):
|
|
94
|
+
axis, p = jinfo[jn]
|
|
95
|
+
t = 0.0
|
|
96
|
+
for ln in self._descendants(jn):
|
|
97
|
+
if ln not in self.links:
|
|
98
|
+
continue
|
|
99
|
+
m, com = self.links[ln]
|
|
100
|
+
R, pl = frames[ln]
|
|
101
|
+
c = pl + R @ com
|
|
102
|
+
t += axis @ np.cross(c - p, m * G)
|
|
103
|
+
tau[i] = -t # motor torque = minus gravity torque
|
|
104
|
+
return tau
|
|
105
|
+
|
|
106
|
+
def gravity_torque_public(self, q_pub_deg):
|
|
107
|
+
q = np.radians(np.asarray(q_pub_deg[:6], dtype=float)) * self.sign
|
|
108
|
+
return self.torque_urdf(q) * self.sign
|