Depth Dodge

Install the overlay into an existing controller environment without replacing its platform dependencies.

python3 scripts/prepare_lerobot.py --clone --apply --checkout external/lerobot
python3 scripts/fetch_assets.py --scope runtime
python -m pip install --no-deps -e external/lerobot
python robot/doctor.py --writer-python "$HOME/rs_pub_venv/bin/python" \
  --out results/robot/environment.json
python scripts/check_release.py --weights-dir weights/released

This uses synthetic standing body state and the real camera pipeline. It does not connect an actuator controller.

python robot/run_dodge.py --dry-run --camera shm --mode dodge --seconds 30 \
  --rs_python "$HOME/rs_pub_venv/bin/python" --weights-dir weights/released \
  --log results/robot/dryrun.jsonl

The detector defaults below apply here. Range gating is off unless supplied through --writer_args.

python robot/run_dodge.py --dry-run --camera shm --mode dodge --seconds 30 \
  --rs_python "$HOME/rs_pub_venv/bin/python" --weights-dir weights/released \
  --ball_only --view_port 8080 --writer_args "--trace" \
  --log results/robot/orange-mask-check.jsonl

Use the robot’s established low-level-control procedure, support arrangement, and physical emergency stop. Replace eth0 with the configured DDS interface. The runner starts in stock SONIC mode; Space toggles Dodge and q ends the session.

python robot/run_dodge.py --real --network_interface eth0 --seconds 120 \
  --rs_python "$HOME/rs_pub_venv/bin/python" --weights-dir weights/released \
  --log results/robot/run01.jsonl

Press h, m, or f to annotate a hit, miss, or fall. Outcomes are operator labels, not automatic contact measurements.

python robot/summarize_session.py results/robot/run01.jsonl \
  --out results/robot/run01-summary.json

Exact BallMasker.apply implementation. Depth inside an accepted disc passes through unchanged.

def apply(self, color_bgr: np.ndarray, depth_mm: np.ndarray) -> np.ndarray:
    det = self.detect(color_bgr, depth_mm)
    self.last_far = None
    if det is not None and self.max_range > 0 and det["z"] > self.max_range:
        self.last_far, det = det, None
    self.last = det
    out = np.full_like(depth_mm, self.background_mm)
    if det is not None:
        disc = np.zeros(depth_mm.shape, np.uint8)
        self.cv2.circle(disc, (int(det["u"]), int(det["v"])), max(int(det["r_px"]), 1), 255, -1)
        keep = disc > 0
        out[keep] = depth_mm[keep]
    return out

Median-depth support, circularity, apparent radius, and a combined score select one candidate per frame.

def detect(self, color_bgr: np.ndarray, depth_mm: np.ndarray) -> dict | None:
    cv2 = self.cv2
    self.last_mask = self.mask(color_bgr)
    contours = cv2.findContours(self.last_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[0]
    best = None
    self.rejects: list[str] = []  # why the largest orange blobs were not accepted (for --trace)
    for contour in contours:
        area = cv2.contourArea(contour)
        if area < self.min_area:
            continue
        (u, v), r_px = cv2.minEnclosingCircle(contour)
        circ = area / max(np.pi * r_px**2, 1.0)
        if circ < self.min_circularity:
            self.rejects.append(f"{int(area)}px circ={circ:.2f}")
            continue
        inner = np.zeros(depth_mm.shape, np.uint8)
        cv2.circle(inner, (int(u), int(v)), max(int(0.7 * r_px), 2), 255, -1)
        inside = depth_mm[(inner > 0) & (depth_mm > 0)]
        if inside.size < 10:
            self.rejects.append(f"{int(area)}px no-depth")
            continue
        centre_m = float(np.median(inside)) / 1000.0 + self.radius
        metric_r = r_px * centre_m / self.fx
        size_err = abs(metric_r - self.radius) / self.radius if self.radius > 0 else 0.0
        if self.radius > 0 and size_err > self.size_tolerance:
            self.rejects.append(f"{int(area)}px r={metric_r * 100:.0f}cm@{centre_m:.1f}m")
            continue
        score = size_err + 0.5 * (1.0 - circ)
        if best is None or score < best["score"]:
            best = {"u": u, "v": v, "r_px": r_px, "z": centre_m, "circ": circ, "score": score}
    return best

The opening and closing operate on the colour mask. They do not fill holes in measured depth.

def mask(self, color_bgr: np.ndarray) -> np.ndarray:
    cv2 = self.cv2
    hsv = cv2.cvtColor(color_bgr, cv2.COLOR_BGR2HSV)
    if self.low[0] <= self.high[0]:
        m = cv2.inRange(hsv, self.low, self.high)
    else:  # hue wraps through red
        m = cv2.bitwise_or(
            cv2.inRange(hsv, np.array([0, self.low[1], self.low[2]], np.uint8), self.high),
            cv2.inRange(hsv, self.low, np.array([179, self.high[1], self.high[2]], np.uint8)),
        )
    m = cv2.morphologyEx(m, cv2.MORPH_OPEN, self.kernel)
    return cv2.morphologyEx(m, cv2.MORPH_CLOSE, self.kernel)

Optical-Z metres are rounded through float16, encoded as 255 near / 1 far / 0 invalid, then resized to 224 × 224. The image graph repeats grayscale across RGB channels.

def depth_to_theia_image_np(
    depth_m: np.ndarray, near: float = DEPTH_NEAR_M, far: float = DEPTH_FAR_M
) -> np.ndarray:
    """numpy/cv2 twin of ``depth_dodge.depth_to_theia_image`` (255 near, 1 far, 0 invalid; bilinear to
    224x224). Differs from the torch version by at most 1 grey level on ~1% of pixels (resize rounding)."""
    import cv2  # noqa: PLC0415

    depth = np.asarray(depth_m, np.float32).astype(np.float16).astype(np.float32)
    valid = np.isfinite(depth) & (depth >= near) & (depth <= far)
    value = np.round(np.clip(1.0 + 254.0 * (far - depth) / (far - near), 1.0, 255.0))
    gray = np.where(valid, value, 0.0).astype(np.float32)
    x = cv2.resize(gray, (THEIA_SIZE, THEIA_SIZE), interpolation=cv2.INTER_LINEAR)
    return np.clip(np.round(x), 0, 255).astype(np.uint8)

The full controller step shows the stock-SONIC branch, reference and proprioception packing, inference inputs, and residual-to-target conversion.

def _run_step(self, lowstate) -> dict:
    q = np.array([lowstate.motor_state[m.value].q for m in G1_29_JointIndex], np.float32)
    dq = np.array([lowstate.motor_state[m.value].dq for m in G1_29_JointIndex], np.float32)
    quat = np.array(lowstate.imu_state.quaternion, np.float32)
    quat = quat / (np.linalg.norm(quat) + 1e-8)
    ang = np.array(lowstate.imu_state.gyroscope, np.float32)

    frames = (ang, q - self.default_angles, dq, self.last_action.copy(), get_gravity_orientation(quat))
    for hist, frame in zip(
        (self.h_ang, self.h_q, self.h_dq, self.h_act, self.h_grav), frames, strict=True
    ):
        if not hist:  # training env fills the whole history from the first frame on reset
            hist.extend([frame] * HISTORY_LEN)
        else:
            hist.append(frame)

    if self._mode == "sonic":
        # Stock SONIC on its neutral token; no token keys in ``action`` -> it holds the idle
        # latent. Its residual comes back in its own joint order; keep ours (SDK order) fed so
        # switching to dodge starts from a warm, consistent action history.
        targets = self.sonic.run_step({}, lowstate)
        self.last_action = self.sonic.last_action_mj[ISAACLAB_TO_MUJOCO].astype(np.float32)
        return targets

    policy = np.concatenate(
        [np.concatenate(list(h)) for h in (self.h_ang, self.h_q, self.h_dq, self.h_act, self.h_grav)]
    )
    tokenizer = self._tokenizer(quat)
    conditioning = np.concatenate([tokenizer, self._percept()[0]])
    residual = self.actor.run(
        None,
        {
            "tokenizer": tokenizer[None].astype(np.float32),
            "policy": policy[None].astype(np.float32),
            "conditioning": conditioning[None].astype(np.float32),
        },
    )[0][0].astype(np.float32)
    if not np.isfinite(residual).all():
        raise FloatingPointError("Non-finite Dodge action")
    self.last_action = residual
    target = self.default_angles + residual * self.action_scale
    return {f"{m.name}.q": float(target[m.value]) for m in G1_29_JointIndex}

These consecutive source lines crop and resize depth, call Theia once, match the cached precision, and publish tokens with a capture timestamp.

writer.write(depth_mm, t_cap)
policy_img = None
if theia is not None:
    t0 = time.perf_counter()
    roi = depth_mm[y0:y1, x0:x1]
    small = cv2.resize(
        roi, (POLICY_DEPTH_SHAPE[1], POLICY_DEPTH_SHAPE[0]), interpolation=cv2.INTER_NEAREST
    )
    depth_m = small.astype(np.float32) * 1e-3
    policy_img = depth_to_theia_image_np(depth_m)
    tokens = theia.run(None, {"image": policy_img[None]})[0][0]
    tokens = tokens.astype(np.float16).astype(np.float32)  # training cached fp16 features
    dt_ms = (time.perf_counter() - t0) * 1e3
    theia_ms = 0.9 * theia_ms + 0.1 * dt_ms
    pwriter.write(tokens, depth_m, t_cap, dt_ms)

The model runs outside the ring lock. The processed snapshot version is retained even if a new frame arrives during inference.

def _percept(self) -> np.ndarray:
    with self._ring_lock:
        version = self._ring_version
        ring = np.stack(self.depth_ring)[None] if version != self._percept_version else None
    if ring is not None:
        # A new camera frame may arrive during inference. Record which
        # snapshot was processed; never clear a newer frame's dirty flag.
        percept = self.perception.run(None, {"depth_tokens": ring})[0]
        self._cached_percept = percept
        self._percept_version = version
    return self._cached_percept

The next deadline advances over missed slots instead of replaying a backlog of delayed frames.

class Cadence:
    """Pick frames at a fixed rate from a faster stream, by capture time (not arrival time), so the
    ring contains 16 frames at 25 Hz like in training even though the writer runs at 60 Hz."""

    def __init__(self, hz: float):
        self.period = 1.0 / hz
        self.next: float | None = None

    def take(self, t_capture: float) -> bool:
        if self.next is None:
            self.next = t_capture + self.period
            return True
        if t_capture + 1e-4 < self.next:
            return False
        # Advance to the slot containing this frame; skip slots if we fell behind (stall).
        self.next += self.period * max(1, math.floor((t_capture - self.next) / self.period) + 1)
        return True

Odd sequence values mean a write is in progress. The reader verifies the sequence before and after copying.

def try_read(self) -> tuple[np.ndarray, np.ndarray, dict] | None:
    """Return (tokens f32[197,192], depth_m f32[64,96], meta) for a new complete frame, else None."""
    seq0, meta = self._header()
    if seq0 & 1 or seq0 == self._last_seq or seq0 == 0:
        return None
    tokens, depth = self._tokens.copy(), self._depth.copy()
    seq1, _ = self._header()
    if seq1 != seq0:
        return None
    self._last_seq = seq0
    meta["seq"] = seq0
    return tokens, depth, meta

The worker performs rendering and JPEG encoding; the acquisition loop copies only what the preview needs.

def submit(self, render, *arrays) -> None:
    """Queue ``render(*arrays)`` for the worker; drops the frame if the previous one is still rendering."""
    with self._pending_cond:
        if self._pending is not None:
            return
        self._pending = (render, tuple(a.copy() if isinstance(a, np.ndarray) else a for a in arrays))
        self._pending_cond.notify()