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.
Files changed (46) hide show
  1. macbot/__init__.py +3 -0
  2. macbot/cameras/__init__.py +239 -0
  3. macbot/cameras/swift/uvc_stream.swift +83 -0
  4. macbot/cli.py +179 -0
  5. macbot/collect/__init__.py +0 -0
  6. macbot/collect/checks.py +36 -0
  7. macbot/collect/server.py +371 -0
  8. macbot/config.py +56 -0
  9. macbot/data/__init__.py +0 -0
  10. macbot/data/export.py +116 -0
  11. macbot/data/health.py +67 -0
  12. macbot/default.yaml +55 -0
  13. macbot/doctor.py +86 -0
  14. macbot/i18n.py +10 -0
  15. macbot/policy/__init__.py +0 -0
  16. macbot/policy/bench.py +28 -0
  17. macbot/policy/executors.py +222 -0
  18. macbot/policy/gripper.py +47 -0
  19. macbot/policy/models.py +158 -0
  20. macbot/policy/runner.py +345 -0
  21. macbot/robot/__init__.py +3 -0
  22. macbot/robot/fake.py +63 -0
  23. macbot/robot/gravity.py +108 -0
  24. macbot/robot/rebot.py +223 -0
  25. macbot/system.py +97 -0
  26. macbot/viz/__init__.py +0 -0
  27. macbot/viz/replay.py +113 -0
  28. macbot/viz/video.py +83 -0
  29. macbot/web/__init__.py +85 -0
  30. macbot/web/static/common.js +139 -0
  31. macbot/web/static/fonts/JetBrainsMono.ttf +0 -0
  32. macbot/web/static/fonts/OFL-JetBrainsMono.txt +93 -0
  33. macbot/web/static/logo-dark.svg +1 -0
  34. macbot/web/static/logo-mark.svg +1 -0
  35. macbot/web/static/logo.svg +1 -0
  36. macbot/web/static/theme.css +200 -0
  37. macbot/web/templates/_head.html +5 -0
  38. macbot/web/templates/collect.html +223 -0
  39. macbot/web/templates/replay.html +132 -0
  40. macbot/web/templates/run.html +124 -0
  41. macbot-0.1.0.dist-info/METADATA +183 -0
  42. macbot-0.1.0.dist-info/RECORD +46 -0
  43. macbot-0.1.0.dist-info/WHEEL +5 -0
  44. macbot-0.1.0.dist-info/entry_points.txt +2 -0
  45. macbot-0.1.0.dist-info/licenses/LICENSE +202 -0
  46. macbot-0.1.0.dist-info/top_level.txt +1 -0
@@ -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)
@@ -0,0 +1,3 @@
1
+ """Robot drivers. Only the reBot B601-RS + Arm 102 pair is implemented; add others next to it."""
2
+ from .fake import FakeFollower, FakeLeader # noqa: F401
3
+ __all__ = ["FakeFollower", "FakeLeader"]
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
@@ -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