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/robot/rebot.py
ADDED
|
@@ -0,0 +1,223 @@
|
|
|
1
|
+
"""reBot B601-RS follower and reBot Arm 102 leader on LeRobot 0.7.0, with safety defaults.
|
|
2
|
+
|
|
3
|
+
What this adds on top of the LeRobot classes
|
|
4
|
+
--------------------------------------------
|
|
5
|
+
1. **No accidental re-zeroing.** Without a calibration file, ``connect()`` enters calibration and
|
|
6
|
+
one Enter key writes the current pose as the motor zero. We always connect with
|
|
7
|
+
``calibrate=False`` and refuse to connect if the calibration file is missing.
|
|
8
|
+
2. **Safe shutdown.** ``disconnect()`` disables the motors at once and the forearm drops. We first
|
|
9
|
+
move to zero in two stages (pan and wrist joints, then shoulder lift and elbow), at most
|
|
10
|
+
0.5 deg per 20 ms, then disable.
|
|
11
|
+
3. **Feedforward.** The stock MIT command uses target velocity 0, so the damping term drags every
|
|
12
|
+
motion (about 100 ms of lag), and gravity is held only by the position error (2-3 deg sag). We
|
|
13
|
+
add velocity feedforward (filtered target velocity) and gravity feedforward (URDF model).
|
|
14
|
+
4. **Gripper.** Position control with a lead clamp (the target never leads the measured angle by
|
|
15
|
+
more than ``gripper_lead_deg``), which bounds the squeeze force. The gripper multi-turn counter
|
|
16
|
+
can report one extra turn after power-up; we read the raw position and remove whole turns.
|
|
17
|
+
A small software zero trim is available for grippers whose zero drifted.
|
|
18
|
+
"""
|
|
19
|
+
from __future__ import annotations
|
|
20
|
+
|
|
21
|
+
import glob
|
|
22
|
+
import math
|
|
23
|
+
import time
|
|
24
|
+
|
|
25
|
+
import numpy as np
|
|
26
|
+
|
|
27
|
+
from ..config import ARM, JOINTS
|
|
28
|
+
|
|
29
|
+
STAGE_1 = ["shoulder_pan", "wrist_flex", "wrist_yaw", "wrist_roll"]
|
|
30
|
+
STAGE_2 = ["shoulder_lift", "elbow_flex"]
|
|
31
|
+
|
|
32
|
+
VEL_LPF_ALPHA = 0.35
|
|
33
|
+
VEL_DEADBAND = 2.0 # deg/s
|
|
34
|
+
VEL_MAX = 120.0 # deg/s, cap of the feedforward velocity
|
|
35
|
+
TAU_FF_MAX = {"shoulder_pan": 4.0, "shoulder_lift": 14.0, "elbow_flex": 10.0,
|
|
36
|
+
"wrist_flex": 3.0, "wrist_yaw": 2.0, "wrist_roll": 1.0} # N*m
|
|
37
|
+
FF_RAMP_S = 1.0
|
|
38
|
+
GRIP_WINDOW = (-300.0, 60.0) # plausible gripper range in public coordinates (360 deg wide)
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
def _lerobot():
|
|
42
|
+
from lerobot.robots.rebot_b601_follower.config_rebot_b601_follower import RebotB601FollowerRobotConfig
|
|
43
|
+
from lerobot.robots.rebot_b601_follower.motor_family import GRIPPER_MOTOR, ArmControlMode
|
|
44
|
+
from lerobot.robots.rebot_b601_follower.rebot_b601_follower import RebotB601Follower
|
|
45
|
+
return RebotB601FollowerRobotConfig, RebotB601Follower, GRIPPER_MOTOR, ArmControlMode
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
def find_leader_port() -> str:
|
|
49
|
+
"""The leader's CH340 serial port name changes with the USB socket; pick the only one."""
|
|
50
|
+
ports = sorted(glob.glob("/dev/cu.usbserial-*"))
|
|
51
|
+
if len(ports) != 1:
|
|
52
|
+
raise RuntimeError(f"found {len(ports)} usbserial ports {ports}; cannot tell which one is the leader")
|
|
53
|
+
return ports[0]
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
def _make_ff_class():
|
|
57
|
+
_, RebotB601Follower, GRIPPER_MOTOR, ArmControlMode = _lerobot()
|
|
58
|
+
|
|
59
|
+
class FeedforwardFollower(RebotB601Follower):
|
|
60
|
+
"""Stock follower plus velocity and gravity feedforward in the MIT command. Same interface."""
|
|
61
|
+
|
|
62
|
+
def __init__(self, config, vel_ff: float, grav_comp: float, urdf=None):
|
|
63
|
+
super().__init__(config)
|
|
64
|
+
self.vel_ff, self.grav_comp = float(vel_ff), float(grav_comp)
|
|
65
|
+
self._gm = None
|
|
66
|
+
if self.grav_comp > 0:
|
|
67
|
+
from .gravity import GravityModel
|
|
68
|
+
self._gm = GravityModel(urdf)
|
|
69
|
+
self._prev_goal = self._prev_t = self._t_first = None
|
|
70
|
+
self._vel = {n: 0.0 for n in ARM}
|
|
71
|
+
self._ff = {}
|
|
72
|
+
|
|
73
|
+
def send_action(self, action):
|
|
74
|
+
now = time.perf_counter()
|
|
75
|
+
goal = {k.removesuffix(".pos"): float(v) for k, v in action.items() if k.endswith(".pos")}
|
|
76
|
+
goal = {n: min(max(v, self._joint_limits[n][0]), self._joint_limits[n][1]) if n in self._joint_limits else v
|
|
77
|
+
for n, v in goal.items()}
|
|
78
|
+
if self._t_first is None:
|
|
79
|
+
self._t_first = now
|
|
80
|
+
ramp = min(1.0, (now - self._t_first) / FF_RAMP_S)
|
|
81
|
+
if self._prev_goal is not None and now - self._prev_t > 1e-3:
|
|
82
|
+
dt = now - self._prev_t
|
|
83
|
+
for n in ARM:
|
|
84
|
+
if n in goal and n in self._prev_goal:
|
|
85
|
+
raw = (goal[n] - self._prev_goal[n]) / dt
|
|
86
|
+
self._vel[n] = VEL_LPF_ALPHA * raw + (1 - VEL_LPF_ALPHA) * self._vel[n]
|
|
87
|
+
self._prev_goal, self._prev_t = goal, now
|
|
88
|
+
g = (self._gm.gravity_torque_public([goal[n] for n in ARM])
|
|
89
|
+
if self._gm is not None and all(n in goal for n in ARM) else np.zeros(6))
|
|
90
|
+
for j, n in enumerate(ARM):
|
|
91
|
+
v = self._vel[n]
|
|
92
|
+
v = math.copysign(max(abs(v) - VEL_DEADBAND, 0.0), v)
|
|
93
|
+
v = max(-VEL_MAX, min(VEL_MAX, v)) * self.vel_ff
|
|
94
|
+
tau = max(-TAU_FF_MAX[n], min(TAU_FF_MAX[n], self.grav_comp * ramp * float(g[j])))
|
|
95
|
+
self._ff[n] = (v, tau)
|
|
96
|
+
return super().send_action(action)
|
|
97
|
+
|
|
98
|
+
def _send_joint(self, motor_name, position_deg, state=None):
|
|
99
|
+
ff = self._ff.get(motor_name)
|
|
100
|
+
if motor_name == GRIPPER_MOTOR or ff is None or self.config.control_mode is not ArmControlMode.MIT:
|
|
101
|
+
return super()._send_joint(motor_name, position_deg, state)
|
|
102
|
+
lo, hi = self._joint_limits[motor_name]
|
|
103
|
+
position_deg = max(lo, min(hi, position_deg))
|
|
104
|
+
d = self.profile.joint_directions[motor_name] # public -> motor sign
|
|
105
|
+
v_pub, tau_pub = ff
|
|
106
|
+
self.motors[motor_name].send_mit(math.radians(position_deg * d), math.radians(v_pub * d),
|
|
107
|
+
self._mit_kp[motor_name], self._mit_kd[motor_name], tau_pub * d)
|
|
108
|
+
return position_deg
|
|
109
|
+
|
|
110
|
+
return FeedforwardFollower
|
|
111
|
+
|
|
112
|
+
|
|
113
|
+
def read_gripper_raw(channel: str) -> float:
|
|
114
|
+
"""Read the gripper position (parameter 0x7019, mechPos) without enabling the motor. Public degrees."""
|
|
115
|
+
from lerobot.robots.rebot_b601_follower.rebot_b601_follower import MotorBridgeController
|
|
116
|
+
bus = MotorBridgeController(channel=channel)
|
|
117
|
+
m = bus.add_robstride_motor(7, 0xFD, "rs-00")
|
|
118
|
+
try:
|
|
119
|
+
return -math.degrees(m.robstride_get_param_f32(0x7019, 500)) # RS direction sign is -1
|
|
120
|
+
finally:
|
|
121
|
+
m.close()
|
|
122
|
+
bus.close()
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
def apply_gripper_offset(robot, rcfg: dict) -> float:
|
|
126
|
+
"""Remove whole extra turns of the gripper counter and apply the software zero trim."""
|
|
127
|
+
_, _, GRIPPER_MOTOR, _ = _lerobot()
|
|
128
|
+
raw = read_gripper_raw(robot.config.port)
|
|
129
|
+
turns = -360.0 * math.floor((raw - GRIP_WINDOW[0]) / 360.0)
|
|
130
|
+
trim = float(rcfg.get("gripper_zero_offset_deg") or 0.0)
|
|
131
|
+
off = turns + trim
|
|
132
|
+
print(f"gripper raw {raw:.1f} deg, turn correction {turns:+.0f}, zero trim {trim:+.1f} -> {raw + off:.1f} deg", flush=True)
|
|
133
|
+
robot.gripper_offset = off
|
|
134
|
+
if off:
|
|
135
|
+
to_pub, to_mot = robot._motor_to_public_position, robot._public_to_motor_position
|
|
136
|
+
robot._motor_to_public_position = lambda n, d: to_pub(n, d) + (off if n == GRIPPER_MOTOR else 0.0)
|
|
137
|
+
robot._public_to_motor_position = lambda n, p: to_mot(n, p - (off if n == GRIPPER_MOTOR else 0.0))
|
|
138
|
+
return off
|
|
139
|
+
|
|
140
|
+
|
|
141
|
+
def make_follower(rcfg: dict):
|
|
142
|
+
"""Build (not connect) the follower described by the ``robot`` section of the config."""
|
|
143
|
+
Cfg, RebotB601Follower, _, _ = _lerobot()
|
|
144
|
+
extra = {}
|
|
145
|
+
if rcfg.get("gripper_mode", "mit") == "mit":
|
|
146
|
+
extra["gripper_control_mode"] = "mit"
|
|
147
|
+
elif rcfg["gripper_mode"] != "impedance":
|
|
148
|
+
raise ValueError(f"unknown gripper_mode {rcfg['gripper_mode']}")
|
|
149
|
+
cfg = Cfg(port=rcfg.get("can_channel", "can0"), id=rcfg.get("follower_id", "follower1"), motor_family="rs",
|
|
150
|
+
can_adapter="socketcan", cameras={}, **extra)
|
|
151
|
+
vel_ff = float(rcfg.get("velocity_feedforward") or 0.0)
|
|
152
|
+
grav = float(rcfg.get("gravity_compensation") or 0.0)
|
|
153
|
+
if grav > 0 and not rcfg.get("urdf"):
|
|
154
|
+
print("gravity compensation needs robot.urdf; running without it", flush=True)
|
|
155
|
+
grav = 0.0
|
|
156
|
+
if vel_ff == 0 and grav == 0:
|
|
157
|
+
robot = RebotB601Follower(cfg)
|
|
158
|
+
else:
|
|
159
|
+
from ..system import expand
|
|
160
|
+
robot = _make_ff_class()(cfg, vel_ff, grav, expand(rcfg["urdf"]) if grav > 0 else None)
|
|
161
|
+
robot.feedforward = {"velocity": vel_ff, "gravity": grav}
|
|
162
|
+
print(f"follower feedforward: velocity {vel_ff}, gravity {grav}", flush=True)
|
|
163
|
+
if not robot.is_calibrated:
|
|
164
|
+
raise RuntimeError(f"missing calibration file {robot.calibration_fpath}; not connecting (avoids re-zeroing)")
|
|
165
|
+
apply_gripper_offset(robot, rcfg)
|
|
166
|
+
return robot
|
|
167
|
+
|
|
168
|
+
|
|
169
|
+
def make_leader(port: str | None = None, leader_id: str = "rebot_arm_102_leader"):
|
|
170
|
+
from lerobot.teleoperators.rebot_102_leader.config_rebot_102_leader import RebotArm102LeaderTeleopConfig
|
|
171
|
+
from lerobot.teleoperators.rebot_102_leader.rebot_102_leader import RebotArm102Leader
|
|
172
|
+
leader = RebotArm102Leader(RebotArm102LeaderTeleopConfig(port=port or find_leader_port(), id=leader_id))
|
|
173
|
+
if not leader.is_calibrated:
|
|
174
|
+
raise RuntimeError(f"missing calibration file {leader.calibration_fpath}; not connecting")
|
|
175
|
+
return leader
|
|
176
|
+
|
|
177
|
+
|
|
178
|
+
def clip_gripper(action: dict, measured_deg: float, lead_deg: float) -> dict:
|
|
179
|
+
"""Keep the gripper target within +-lead_deg of the measured angle. Free motion stays real-time;
|
|
180
|
+
when an object blocks the fingers, the torque is bounded by kp * lead."""
|
|
181
|
+
if "gripper.pos" not in action:
|
|
182
|
+
return action
|
|
183
|
+
a = dict(action)
|
|
184
|
+
a["gripper.pos"] = max(measured_deg - lead_deg, min(measured_deg + lead_deg, a["gripper.pos"]))
|
|
185
|
+
return a
|
|
186
|
+
|
|
187
|
+
|
|
188
|
+
def safe_zero(robot, step_s: float = 0.02, deg_per_step: float = 0.5) -> None:
|
|
189
|
+
obs = robot.get_observation()
|
|
190
|
+
pos = {n: float(obs[f"{n}.pos"]) for n in JOINTS}
|
|
191
|
+
for stage in (STAGE_1, STAGE_2):
|
|
192
|
+
start = {n: pos[n] for n in stage}
|
|
193
|
+
frames = max(1, math.ceil(max(abs(v) for v in start.values()) / deg_per_step))
|
|
194
|
+
for k in range(1, frames + 1):
|
|
195
|
+
for n in stage:
|
|
196
|
+
pos[n] = start[n] * (1.0 - k / frames)
|
|
197
|
+
robot.send_action({f"{n}.pos": pos[n] for n in JOINTS})
|
|
198
|
+
time.sleep(step_s)
|
|
199
|
+
time.sleep(0.3)
|
|
200
|
+
|
|
201
|
+
|
|
202
|
+
def safe_disconnect(robot, go_zero: bool = True) -> None:
|
|
203
|
+
try:
|
|
204
|
+
if go_zero:
|
|
205
|
+
safe_zero(robot)
|
|
206
|
+
finally:
|
|
207
|
+
robot.disconnect()
|
|
208
|
+
|
|
209
|
+
|
|
210
|
+
def read_joints_passive(rcfg: dict) -> dict:
|
|
211
|
+
"""Read follower joint angles without enabling any motor (parameter reads only). Public degrees."""
|
|
212
|
+
robot = make_follower(dict(rcfg, velocity_feedforward=0, gravity_compensation=0))
|
|
213
|
+
from lerobot.robots.rebot_b601_follower.rebot_b601_follower import MotorBridgeController
|
|
214
|
+
robot.bus = MotorBridgeController(channel=robot.config.port)
|
|
215
|
+
for name, (sid, rid) in robot._motor_can_ids.items():
|
|
216
|
+
robot.motors[name] = robot.bus.add_robstride_motor(sid, rid, robot.profile.motor_models[name])
|
|
217
|
+
try:
|
|
218
|
+
return {n: robot._motor_to_public_position(n, math.degrees(m.robstride_get_param_f32(0x7019, 500)))
|
|
219
|
+
for n, m in robot.motors.items()}
|
|
220
|
+
finally:
|
|
221
|
+
for m in robot.motors.values():
|
|
222
|
+
m.close()
|
|
223
|
+
robot.bus.close()
|
macbot/system.py
ADDED
|
@@ -0,0 +1,97 @@
|
|
|
1
|
+
"""macOS plumbing: PCAN driver loading, running under sudo, and log files.
|
|
2
|
+
|
|
3
|
+
Why this exists
|
|
4
|
+
---------------
|
|
5
|
+
* The PCAN-USB adapter is driven by the PCBUSB library (MacCAN). ``motorbridge`` loads it by
|
|
6
|
+
bare name, but under ``sudo`` the dynamic loader ignores ``/usr/local/lib``. We set
|
|
7
|
+
``DYLD_FALLBACK_LIBRARY_PATH``, re-exec the interpreter once (dyld only reads it at start),
|
|
8
|
+
and preload the library.
|
|
9
|
+
* The Orbbec SDK needs root on macOS (``uvc_open -3`` otherwise), so the robot commands run
|
|
10
|
+
under ``sudo -E``. Files written as root are handed back to the invoking user, and ``~``
|
|
11
|
+
resolves to that user's home.
|
|
12
|
+
"""
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
|
|
15
|
+
import os
|
|
16
|
+
import sys
|
|
17
|
+
import time
|
|
18
|
+
from pathlib import Path
|
|
19
|
+
|
|
20
|
+
PCBUSB = Path("/usr/local/lib/libPCBUSB.dylib")
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
def preload_pcan() -> bool:
|
|
24
|
+
"""Make the PCBUSB library loadable. May re-exec the current process. Returns True if loaded."""
|
|
25
|
+
if sys.platform != "darwin" or not PCBUSB.exists():
|
|
26
|
+
return False
|
|
27
|
+
if "/usr/local/lib" not in os.environ.get("DYLD_FALLBACK_LIBRARY_PATH", ""):
|
|
28
|
+
os.environ["DYLD_FALLBACK_LIBRARY_PATH"] = "/usr/local/lib:/usr/lib"
|
|
29
|
+
os.execv(sys.executable, [sys.executable] + sys.argv)
|
|
30
|
+
import ctypes
|
|
31
|
+
|
|
32
|
+
ctypes.CDLL(str(PCBUSB), mode=ctypes.RTLD_GLOBAL)
|
|
33
|
+
return True
|
|
34
|
+
|
|
35
|
+
|
|
36
|
+
def user_home() -> Path:
|
|
37
|
+
"""Home of the real user, also under sudo."""
|
|
38
|
+
u = os.environ.get("SUDO_USER")
|
|
39
|
+
return Path(os.path.expanduser(f"~{u}" if u else "~"))
|
|
40
|
+
|
|
41
|
+
|
|
42
|
+
def expand(p, base: Path | None = None) -> Path:
|
|
43
|
+
"""Expand ``~`` to the real user's home; resolve relative paths against ``base``."""
|
|
44
|
+
s = str(p)
|
|
45
|
+
if s == "~" or s.startswith("~/"):
|
|
46
|
+
return user_home() / s[2:]
|
|
47
|
+
q = Path(s)
|
|
48
|
+
return q if q.is_absolute() or base is None else base / q
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
def give_back(path: Path) -> None:
|
|
52
|
+
"""Under sudo, chown a file or folder we created back to the invoking user."""
|
|
53
|
+
uid, gid = os.environ.get("SUDO_UID"), os.environ.get("SUDO_GID")
|
|
54
|
+
if uid and gid:
|
|
55
|
+
try:
|
|
56
|
+
os.chown(path, int(uid), int(gid))
|
|
57
|
+
except OSError:
|
|
58
|
+
pass
|
|
59
|
+
|
|
60
|
+
|
|
61
|
+
def ensure_dir(path: Path) -> Path:
|
|
62
|
+
path.mkdir(parents=True, exist_ok=True)
|
|
63
|
+
for p in (path, path.parent):
|
|
64
|
+
give_back(p)
|
|
65
|
+
return path
|
|
66
|
+
|
|
67
|
+
|
|
68
|
+
class _Tee:
|
|
69
|
+
"""Mirror terminal output into a log file (no shell pipe, so Ctrl-C still works)."""
|
|
70
|
+
|
|
71
|
+
def __init__(self, stream, fp):
|
|
72
|
+
self.stream, self.fp = stream, fp
|
|
73
|
+
|
|
74
|
+
def write(self, s):
|
|
75
|
+
self.stream.write(s)
|
|
76
|
+
try:
|
|
77
|
+
self.fp.write(s)
|
|
78
|
+
self.fp.flush()
|
|
79
|
+
except ValueError:
|
|
80
|
+
pass
|
|
81
|
+
return len(s)
|
|
82
|
+
|
|
83
|
+
def flush(self):
|
|
84
|
+
self.stream.flush()
|
|
85
|
+
|
|
86
|
+
def isatty(self):
|
|
87
|
+
return self.stream.isatty()
|
|
88
|
+
|
|
89
|
+
|
|
90
|
+
def start_log(folder: Path, prefix: str) -> Path:
|
|
91
|
+
ensure_dir(folder)
|
|
92
|
+
p = folder / f"{prefix}_{time.strftime('%Y%m%d_%H%M%S')}.log"
|
|
93
|
+
fp = open(p, "w", buffering=1)
|
|
94
|
+
give_back(p)
|
|
95
|
+
sys.stdout, sys.stderr = _Tee(sys.stdout, fp), _Tee(sys.stderr, fp)
|
|
96
|
+
print(f"log: {p}", flush=True)
|
|
97
|
+
return p
|
macbot/viz/__init__.py
ADDED
|
File without changes
|
macbot/viz/replay.py
ADDED
|
@@ -0,0 +1,113 @@
|
|
|
1
|
+
"""Browse recorded rollouts (``rollout_XXX.npz`` from ``macbot run``) in the browser.
|
|
2
|
+
|
|
3
|
+
Frames are served straight from the npz files; nothing is converted. For each rollout the page shows
|
|
4
|
+
all cameras in sync, the command and measured joint traces with a tick at every new action chunk, and
|
|
5
|
+
the run settings (policy, executor, speed, inference time).
|
|
6
|
+
"""
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
import json
|
|
10
|
+
import re
|
|
11
|
+
import threading
|
|
12
|
+
from http.server import ThreadingHTTPServer
|
|
13
|
+
from pathlib import Path
|
|
14
|
+
|
|
15
|
+
import numpy as np
|
|
16
|
+
|
|
17
|
+
from .. import web
|
|
18
|
+
from ..config import Config
|
|
19
|
+
from ..system import expand
|
|
20
|
+
|
|
21
|
+
|
|
22
|
+
class Store:
|
|
23
|
+
"""Keeps the most recently opened rollout in memory, so scrubbing does not re-read the file."""
|
|
24
|
+
|
|
25
|
+
def __init__(self, folder: Path):
|
|
26
|
+
self.folder = folder
|
|
27
|
+
self.lock = threading.Lock()
|
|
28
|
+
self.cur = (None, None)
|
|
29
|
+
|
|
30
|
+
def files(self):
|
|
31
|
+
return sorted(self.folder.glob("rollout_*.npz"))
|
|
32
|
+
|
|
33
|
+
def index(self):
|
|
34
|
+
out = []
|
|
35
|
+
for p in self.files():
|
|
36
|
+
try:
|
|
37
|
+
with np.load(p) as d:
|
|
38
|
+
m = json.loads(str(d["meta"]))
|
|
39
|
+
except Exception:
|
|
40
|
+
continue
|
|
41
|
+
out.append({"name": p.stem, "result": m.get("result"), "s": round(m.get("frames", 0) / m.get("fps", 30), 1),
|
|
42
|
+
"saved_at": m.get("saved_at", ""), "mode": m.get("mode", "")})
|
|
43
|
+
return out[::-1]
|
|
44
|
+
|
|
45
|
+
def get(self, name: str):
|
|
46
|
+
with self.lock:
|
|
47
|
+
if self.cur[0] != name:
|
|
48
|
+
p = self.folder / f"{name}.npz"
|
|
49
|
+
if not re.fullmatch(r"rollout_\d+", name) or not p.exists():
|
|
50
|
+
return None
|
|
51
|
+
with np.load(p) as d:
|
|
52
|
+
self.cur = (name, {k: d[k] for k in d.files})
|
|
53
|
+
return self.cur[1]
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
def summary(d) -> dict:
|
|
57
|
+
meta = json.loads(str(d["meta"]))
|
|
58
|
+
cams = meta.get("cameras") or [k[:-5] for k in d if k.endswith("_jpeg")]
|
|
59
|
+
sizes = {}
|
|
60
|
+
import cv2
|
|
61
|
+
for c in cams:
|
|
62
|
+
off = d[f"{c}_offsets"]
|
|
63
|
+
if len(off) > 1:
|
|
64
|
+
im = cv2.imdecode(d[f"{c}_jpeg"][off[0]:off[1]], cv2.IMREAD_COLOR)
|
|
65
|
+
sizes[c] = [int(im.shape[1]), int(im.shape[0])]
|
|
66
|
+
t = d["t"].astype(float)
|
|
67
|
+
ct = d["chunk_t"].astype(float) if "chunk_t" in d else np.zeros(0)
|
|
68
|
+
ims = d["infer_ms"].astype(float) if "infer_ms" in d else np.zeros(0)
|
|
69
|
+
return {"meta": meta, "cams": cams, "sizes": sizes, "t": np.round(t, 3).tolist(),
|
|
70
|
+
"q": np.round(d["qpos"].astype(float), 2).tolist(), "cmd": np.round(d["cmd"].astype(float), 2).tolist(),
|
|
71
|
+
"chunk_t": np.round(ct, 3).tolist(), "infer_ms": round(float(np.median(ims)), 1) if len(ims) else None,
|
|
72
|
+
"chunks": int(len(ct))}
|
|
73
|
+
|
|
74
|
+
|
|
75
|
+
def make_handler(store: Store, task: str):
|
|
76
|
+
class H(web.Handler):
|
|
77
|
+
def do_GET(self):
|
|
78
|
+
path = self.path.split("?")[0]
|
|
79
|
+
if path == "/":
|
|
80
|
+
return self.send(200, web.page("replay", task=task, folder=str(store.folder)), "text/html; charset=utf-8")
|
|
81
|
+
if self.static(path):
|
|
82
|
+
return
|
|
83
|
+
if path == "/api/rollouts":
|
|
84
|
+
return self.json({"folder": str(store.folder), "items": store.index()})
|
|
85
|
+
m = re.fullmatch(r"/api/rollout/(\w+)", path)
|
|
86
|
+
if m:
|
|
87
|
+
d = store.get(m.group(1))
|
|
88
|
+
return self.json(summary(d)) if d is not None else self.json({}, 404)
|
|
89
|
+
m = re.fullmatch(r"/api/rollout/(\w+)/(\w+)/(\d+)", path)
|
|
90
|
+
if m:
|
|
91
|
+
d = store.get(m.group(1))
|
|
92
|
+
key = f"{m.group(2)}_offsets"
|
|
93
|
+
if d is None or key not in d:
|
|
94
|
+
return self.send(404, b"", "text/plain")
|
|
95
|
+
off, k = d[key], int(m.group(3))
|
|
96
|
+
k = max(0, min(k, len(off) - 2))
|
|
97
|
+
return self.send(200, d[f"{m.group(2)}_jpeg"][off[k]:off[k + 1]].tobytes(), "image/jpeg")
|
|
98
|
+
self.send(404, b"not found", "text/plain")
|
|
99
|
+
|
|
100
|
+
return H
|
|
101
|
+
|
|
102
|
+
|
|
103
|
+
def run(cfg: Config, args):
|
|
104
|
+
folder = expand(args.dir) if args.dir else cfg.rollout_dir()
|
|
105
|
+
if not folder.is_dir():
|
|
106
|
+
raise SystemExit(f"{folder} does not exist")
|
|
107
|
+
srv = ThreadingHTTPServer(("127.0.0.1", args.port), make_handler(Store(folder), cfg.task))
|
|
108
|
+
srv.daemon_threads = True
|
|
109
|
+
print(f"rollouts: {folder}\nopen http://127.0.0.1:{args.port} (Ctrl-C to stop)", flush=True)
|
|
110
|
+
try:
|
|
111
|
+
srv.serve_forever()
|
|
112
|
+
except KeyboardInterrupt:
|
|
113
|
+
pass
|
macbot/viz/video.py
ADDED
|
@@ -0,0 +1,83 @@
|
|
|
1
|
+
"""Rollout or episode -> one shareable MP4 with all cameras side by side at equal height.
|
|
2
|
+
|
|
3
|
+
Frames are decoded from the stored JPEGs and encoded once with H.264 (``ffmpeg`` on PATH, for example
|
|
4
|
+
``brew install ffmpeg``). Without ffmpeg, OpenCV writes an MPEG-4 file instead.
|
|
5
|
+
"""
|
|
6
|
+
from __future__ import annotations
|
|
7
|
+
|
|
8
|
+
import json
|
|
9
|
+
import shutil
|
|
10
|
+
import subprocess
|
|
11
|
+
|
|
12
|
+
import cv2
|
|
13
|
+
import numpy as np
|
|
14
|
+
|
|
15
|
+
from ..system import expand, give_back
|
|
16
|
+
|
|
17
|
+
GAP, BG = 8, (20, 18, 18)
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
def _frames(d, cam):
|
|
21
|
+
off, buf = d[f"{cam}_offsets"], d[f"{cam}_jpeg"]
|
|
22
|
+
for i in range(len(off) - 1):
|
|
23
|
+
yield cv2.imdecode(buf[off[i]:off[i + 1]], cv2.IMREAD_COLOR)
|
|
24
|
+
|
|
25
|
+
|
|
26
|
+
def _fit(im, h):
|
|
27
|
+
w = round(im.shape[1] * h / im.shape[0])
|
|
28
|
+
return cv2.resize(im, (w, h), interpolation=cv2.INTER_LANCZOS4 if h > im.shape[0] else cv2.INTER_AREA)
|
|
29
|
+
|
|
30
|
+
|
|
31
|
+
def _tag(im, text, corner):
|
|
32
|
+
font, s, th = cv2.FONT_HERSHEY_SIMPLEX, im.shape[0] / 900, max(1, round(im.shape[0] / 500))
|
|
33
|
+
(tw, tht), _ = cv2.getTextSize(text, font, s, th)
|
|
34
|
+
pad = round(10 * s + 4)
|
|
35
|
+
x = pad if corner == "left" else im.shape[1] - tw - 3 * pad
|
|
36
|
+
y = im.shape[0] - tht - 3 * pad
|
|
37
|
+
ov = im.copy()
|
|
38
|
+
cv2.rectangle(ov, (x, y), (x + tw + 2 * pad, y + tht + 2 * pad), (0, 0, 0), -1)
|
|
39
|
+
cv2.addWeighted(ov, 0.55, im, 0.45, 0, im)
|
|
40
|
+
cv2.putText(im, text, (x + pad, y + tht + pad), font, s, (255, 255, 255), th, cv2.LINE_AA)
|
|
41
|
+
|
|
42
|
+
|
|
43
|
+
def run(args):
|
|
44
|
+
src = expand(args.rollout)
|
|
45
|
+
d = np.load(src)
|
|
46
|
+
meta = json.loads(str(d["meta"]))
|
|
47
|
+
cams = meta.get("cameras") if isinstance(meta.get("cameras"), list) else list(meta.get("cameras") or [])
|
|
48
|
+
cams = [c for c in cams if f"{c}_jpeg" in d.files] or [k[:-5] for k in d.files if k.endswith("_jpeg")]
|
|
49
|
+
fps = float(meta.get("fps", 30))
|
|
50
|
+
h = args.height - args.height % 2
|
|
51
|
+
out = expand(args.out) if args.out else src.with_suffix(".mp4")
|
|
52
|
+
streams = [_frames(d, c) for c in cams]
|
|
53
|
+
first = [_fit(next(_frames(d, c)), h) for c in cams]
|
|
54
|
+
W = sum(f.shape[1] for f in first) + GAP * (len(cams) - 1)
|
|
55
|
+
W += W % 2
|
|
56
|
+
ff = shutil.which("ffmpeg")
|
|
57
|
+
if ff:
|
|
58
|
+
enc = subprocess.Popen([ff, "-y", "-loglevel", "error", "-f", "rawvideo", "-pix_fmt", "bgr24", "-s", f"{W}x{h}",
|
|
59
|
+
"-r", f"{fps}", "-i", "-", "-c:v", "libx264", "-preset", "medium", "-crf", "21",
|
|
60
|
+
"-pix_fmt", "yuv420p", "-movflags", "+faststart", str(out)], stdin=subprocess.PIPE)
|
|
61
|
+
write = lambda fr: enc.stdin.write(fr.tobytes()) # noqa: E731
|
|
62
|
+
else:
|
|
63
|
+
vw = cv2.VideoWriter(str(out), cv2.VideoWriter_fourcc(*"mp4v"), fps, (W, h))
|
|
64
|
+
write = vw.write
|
|
65
|
+
n = 0
|
|
66
|
+
for imgs in zip(*streams):
|
|
67
|
+
canvas = np.full((h, W, 3), BG, np.uint8)
|
|
68
|
+
x = 0
|
|
69
|
+
for c, im in zip(cams, imgs):
|
|
70
|
+
im = _fit(im, h)
|
|
71
|
+
_tag(im, c, "left")
|
|
72
|
+
canvas[:, x:x + im.shape[1]] = im
|
|
73
|
+
x += im.shape[1] + GAP
|
|
74
|
+
_tag(canvas, f"{n / fps:5.1f} s", "right")
|
|
75
|
+
write(np.ascontiguousarray(canvas))
|
|
76
|
+
n += 1
|
|
77
|
+
if ff:
|
|
78
|
+
enc.stdin.close()
|
|
79
|
+
enc.wait()
|
|
80
|
+
else:
|
|
81
|
+
vw.release()
|
|
82
|
+
give_back(out)
|
|
83
|
+
print(f"wrote {out} ({n} frames, {n / fps:.1f} s, {W}x{h})")
|
macbot/web/__init__.py
ADDED
|
@@ -0,0 +1,85 @@
|
|
|
1
|
+
"""Tiny HTTP layer shared by the collect, run and replay pages (standard library only)."""
|
|
2
|
+
from __future__ import annotations
|
|
3
|
+
|
|
4
|
+
import json
|
|
5
|
+
import mimetypes
|
|
6
|
+
import re
|
|
7
|
+
import time
|
|
8
|
+
from http.server import BaseHTTPRequestHandler
|
|
9
|
+
from pathlib import Path
|
|
10
|
+
|
|
11
|
+
import cv2
|
|
12
|
+
|
|
13
|
+
HERE = Path(__file__).resolve().parent
|
|
14
|
+
STATIC = HERE / "static"
|
|
15
|
+
TEMPLATES = HERE / "templates"
|
|
16
|
+
BRAND = ('<a class="brand" href="/"><img class="logo-light" src="/static/logo.svg" alt="macbot">'
|
|
17
|
+
'<img class="logo-dark" src="/static/logo-dark.svg" alt="macbot"></a>')
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
def page(name: str, **vars) -> bytes:
|
|
21
|
+
"""Read templates/<name>.html and fill the shared header and {{var}} placeholders."""
|
|
22
|
+
html = (TEMPLATES / f"{name}.html").read_text(encoding="utf-8")
|
|
23
|
+
head = (TEMPLATES / "_head.html").read_text(encoding="utf-8")
|
|
24
|
+
html = html.replace("{{head}}", head).replace("{{brand}}", BRAND)
|
|
25
|
+
for k, v in vars.items():
|
|
26
|
+
html = html.replace("{{" + k + "}}", str(v))
|
|
27
|
+
return html.encode()
|
|
28
|
+
|
|
29
|
+
|
|
30
|
+
class Handler(BaseHTTPRequestHandler):
|
|
31
|
+
"""Base handler: JSON, static files and MJPEG camera streams. Subclasses add routes."""
|
|
32
|
+
|
|
33
|
+
def log_message(self, *a):
|
|
34
|
+
pass
|
|
35
|
+
|
|
36
|
+
def send(self, code: int, body: bytes, ctype: str):
|
|
37
|
+
self.send_response(code)
|
|
38
|
+
self.send_header("Content-Type", ctype)
|
|
39
|
+
self.send_header("Content-Length", str(len(body)))
|
|
40
|
+
self.send_header("Cache-Control", "no-store")
|
|
41
|
+
self.end_headers()
|
|
42
|
+
self.wfile.write(body)
|
|
43
|
+
|
|
44
|
+
def json(self, obj, code: int = 200):
|
|
45
|
+
self.send(code, json.dumps(obj, ensure_ascii=False).encode(), "application/json")
|
|
46
|
+
|
|
47
|
+
def body_json(self) -> dict:
|
|
48
|
+
n = int(self.headers.get("Content-Length", 0))
|
|
49
|
+
return json.loads(self.rfile.read(n) or b"{}")
|
|
50
|
+
|
|
51
|
+
def static(self, path: str) -> bool:
|
|
52
|
+
m = re.fullmatch(r"/static/([\w./-]+)", path)
|
|
53
|
+
if not m or ".." in m.group(1):
|
|
54
|
+
return False
|
|
55
|
+
f = STATIC / m.group(1)
|
|
56
|
+
if not f.is_file():
|
|
57
|
+
self.send(404, b"", "text/plain")
|
|
58
|
+
return True
|
|
59
|
+
ctype = mimetypes.guess_type(f.name)[0] or "application/octet-stream"
|
|
60
|
+
self.send_response(200)
|
|
61
|
+
data = f.read_bytes()
|
|
62
|
+
self.send_header("Content-Type", ctype)
|
|
63
|
+
self.send_header("Content-Length", str(len(data)))
|
|
64
|
+
self.send_header("Cache-Control", "max-age=3600")
|
|
65
|
+
self.end_headers()
|
|
66
|
+
self.wfile.write(data)
|
|
67
|
+
return True
|
|
68
|
+
|
|
69
|
+
def mjpeg(self, cam, size, stop, fps: float = 15.0):
|
|
70
|
+
"""Stream the latest frame of ``cam`` (resized to ``size``) until ``stop()`` is true."""
|
|
71
|
+
self.send_response(200)
|
|
72
|
+
self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame")
|
|
73
|
+
self.send_header("Cache-Control", "no-store")
|
|
74
|
+
self.end_headers()
|
|
75
|
+
try:
|
|
76
|
+
while not stop():
|
|
77
|
+
img, _ = cam.latest()
|
|
78
|
+
if img is not None:
|
|
79
|
+
_, b = cv2.imencode(".jpg", cv2.resize(img, tuple(size), interpolation=cv2.INTER_AREA),
|
|
80
|
+
[cv2.IMWRITE_JPEG_QUALITY, 75])
|
|
81
|
+
self.wfile.write(b"--frame\r\nContent-Type: image/jpeg\r\nContent-Length: "
|
|
82
|
+
+ str(len(b)).encode() + b"\r\n\r\n" + b.tobytes() + b"\r\n")
|
|
83
|
+
time.sleep(1 / fps)
|
|
84
|
+
except (BrokenPipeError, ConnectionResetError):
|
|
85
|
+
pass
|