ur3-optical-resample / deploy /arc_interp.py
JingwuLuo's picture
pi0.5 UR3 optical bench, curvature-resampled chunks (span 30, H 10): JAX checkpoint 9999, norm stats, resampling report, serving config
e0bd416 verified
Raw History Blame Contribute Delete
17.9 kB
"""Server side: a model's resampled points -> one target per control frame. numpy + scipy only.
Standalone on purpose: it imports nothing from the rest of vla_tools, so this one file can be copied to a
serving machine. It runs the label builder's sampling backwards:
joints not-a-knot cubic spline through the current pose at frame 0 and the points (two knots: the
chord; three: the parabola through them)
commands the nearest point in time, re-binarised. The current state is not a knot: its command
value is the previous command echoed back, and letting it decide the frames before the
first point re-opens a gripper the model already closed whenever the echo lags
Point k is placed at (offset + k * span / H) / rate frames after the observation, offset 0 for anchor
"state" and 1 for "action". The execution rate is a deployment choice: at rate 1 a chunk that covers span
recorded frames is executed over span frames, the recording's average pace; at rate r the same spline
path is traversed r times faster. Under metric "time" at rate 1 the placement is exact; otherwise the
timing differs from the recording, which for static tasks costs nothing but speed, measured by the label
replay.
Output: out[j] is the target for frame t + j + 1, the meaning a per-frame policy's actions have.
Inference latency: a chunk that arrives d frames after its observation, while the robot keeps executing the
previous chunk, is rebuilt from the pose commanded at arrival (after_latency): knots = that pose at 0 and the
points whose time minus d exceeds half a point spacing, at their times minus d. The switch between chunks
is continuous in position; out[j] is then the target for frame arrival + j + 1.
Velocity timing (VelocityTiming, a deployment setting like the rate): the same joint spline, traversed with
chunk time T (frames of the uniform timing above) mapped to control frames
s(T) = integral of max(1, fps * max_j |dq_j/dT| / (fraction * v_j)) dT, the integrand taken as the larger
endpoint value on intervals of 1/32 control frame; one target at each s = 1, 2, ..., floor(s(T_end)). A
chunk is never faster than the rate (the cap) and never moves a joint faster than the fraction of its
velocity limit along the spline; the rate then caps only the slow and still stretches. At the first target
whose command differs from the previous target, or from the pose the chunk starts at, the arm stays
round(hold * fps) further frames at that target (rise_hold_s for a command rising above 0.5, fall_hold_s for
one falling below it): a fixed still segment for the gripper to act, since velocity timing passes the nearly
still stretch around a switch at the cap. The start pose's command must then be the commanded value, not a
lagging echo, or the hold repeats. A chunk that replaces one before its hold is over starts with the rest of
the hold (VelocityTiming.owed_hold, from the commanded poses so far). Arriving after a latency, the chunk time already passed is that of target
d of the chunk timed from the pose commanded at the observation (arrival_time), instead of d.
Execution (chunk_targets): the targets one chunk executes, from the poses commanded so far and the index of
its observation: the owed hold, then the chunk (rebuilt after the latency in async mode), cut to the execution
length. The label replay and a robot or simulator loop that own the commanded history both call it.
"""
from __future__ import annotations
import dataclasses
import json
from collections.abc import Sequence
from pathlib import Path
from typing import Literal
import numpy as np
import scipy.interpolate
Mode = Literal["sync", "async"] # sync: the robot waits for each inference; async: it keeps executing
def point_frames(span_frames: int, action_horizon: int, anchor: str, rate: float) -> np.ndarray:
offsets = {"state": 0.0, "action": 1.0}
if rate <= 0.0:
raise ValueError(f"rate must be positive, got {rate}")
if anchor not in offsets:
raise ValueError(f"anchor must be one of {sorted(offsets)}, got {anchor!r}")
return (
offsets[anchor] + np.arange(1, action_horizon + 1, dtype=np.float64) * (span_frames / action_horizon)
) / rate
@dataclasses.dataclass(frozen=True)
class VelocityTiming:
"""Chunk timing at a fraction of the joint velocity limits, capped by the rate, with a still segment at
each command switch."""
velocity_limits: tuple[float, ...] # rad/s, one per non-command dim in dim order
fraction: float # of each limit
fps: float # control rate
rise_hold_s: float # still after a command rises above 0.5
fall_hold_s: float # still after a command falls below 0.5
def __post_init__(self) -> None:
if not self.velocity_limits or min(self.velocity_limits) <= 0.0:
raise ValueError(f"velocity_limits must be positive, got {self.velocity_limits}")
if self.fraction <= 0.0 or self.fps <= 0.0 or min(self.rise_hold_s, self.fall_hold_s) < 0.0:
raise ValueError(f"fraction and fps must be positive and holds >= 0, got {self}")
def frame_times(self, spline: scipy.interpolate.CubicSpline, end: float) -> np.ndarray:
"""Chunk time T of control frames 1, 2, ..., floor(s(end)) along the joint spline. The integrand is
sampled every 1/32 control frame (a first pass at 1/32 of a chunk-time unit sets the step)."""
limits = self.fraction * np.asarray(self.velocity_limits) / self.fps # rad per control frame
def cost(grid: np.ndarray) -> np.ndarray:
return np.maximum(1.0, (np.abs(spline(grid, 1)) / limits).max(axis=1))
grid = np.linspace(0.0, end, int(np.ceil(end * 32.0)) + 1)
grid = np.linspace(0.0, end, int(np.ceil(end * 32.0 * float(cost(grid).max()))) + 1)
c = cost(grid)
s = np.concatenate([[0.0], np.cumsum(np.maximum(c[1:], c[:-1]) * np.diff(grid))])
return np.interp(np.arange(1, int(np.floor(s[-1] + 1e-9)) + 1, dtype=np.float64), s, grid)
def hold_frames(self, before: np.ndarray, after: np.ndarray) -> int:
"""Further frames at a target whose command values `after` differ from `before`."""
rising = bool(((after > 0.5) & (before <= 0.5)).any())
falling = bool(((after <= 0.5) & (before > 0.5)).any())
return max(
round(self.rise_hold_s * self.fps) if rising else 0, round(self.fall_hold_s * self.fps) if falling else 0
)
def owed_hold(self, history: Sequence[np.ndarray], command_dims: tuple[int, ...]) -> int:
"""Further frames the arm must still stay at the last of the commanded (D,) poses `history` (oldest
first): what remains of the hold that the last command switch started, when every pose since the
switch repeats the switching target; 0 otherwise. A chunk that replaces one still holding starts with
that many copies of the commanded pose."""
commands = list(command_dims)
last, k = history[-1], len(history) - 1
while k > 0 and np.array_equal(history[k - 1], last):
k -= 1
if k == 0 or not commands or ((history[k - 1][commands] > 0.5) == (last[commands] > 0.5)).all():
return 0
return max(0, self.hold_frames(history[k - 1][commands], last[commands]) - (len(history) - 1 - k))
def _timed(
points: np.ndarray, current: np.ndarray, times: np.ndarray, command_dims: tuple[int, ...],
timing: VelocityTiming | None,
) -> tuple[np.ndarray, np.ndarray]: # fmt: skip
"""-> (n, D) targets and the (n,) chunk time of each."""
dim = points.shape[1]
joints = [d for d in range(dim) if d not in command_dims]
commands = list(command_dims)
knots = np.concatenate([[0.0], times])
values = np.concatenate([current[None, joints], points[:, joints]], axis=0)
spline = scipy.interpolate.CubicSpline(knots, values, axis=0, bc_type="not-a-knot")
if timing is None:
at = np.arange(1, int(np.floor(times[-1])) + 1, dtype=np.float64)
else:
at = timing.frame_times(spline, float(times[-1]))
out = np.zeros((len(at), dim), dtype=np.float64)
out[:, joints] = spline(at)
nearest = np.abs(at[:, None] - times[None, :]).argmin(axis=1)
out[:, commands] = np.round(points[nearest][:, commands])
if timing is None or not commands:
return out, at
repeats = np.ones(len(out), dtype=np.int64)
previous = current[commands]
for k in range(len(out)):
if ((out[k, commands] > 0.5) != (previous > 0.5)).any():
repeats[k] += timing.hold_frames(previous, out[k, commands])
previous = out[k, commands]
return np.repeat(out, repeats, axis=0), np.repeat(at, repeats)
def to_frames(
points: np.ndarray, current: np.ndarray, times: np.ndarray, command_dims: tuple[int, ...],
timing: VelocityTiming | None = None,
) -> np.ndarray: # fmt: skip
"""(H, D) points at `times`, starting from the (D,) current pose -> (n, D) targets: n = floor(times[-1])
with uniform timing, at least that with velocity timing (holds included)."""
return _timed(points, current, times, command_dims, timing)[0]
def target_times(
points: np.ndarray,
current: np.ndarray,
times: np.ndarray,
command_dims: tuple[int, ...],
timing: VelocityTiming | None = None,
) -> np.ndarray:
"""(n,) chunk time of each target of to_frames; a held target repeats its time."""
return _timed(points, current, times, command_dims, timing)[1]
def arrival_time(
points: np.ndarray, observed: np.ndarray, times: np.ndarray, command_dims: tuple[int, ...],
timing: VelocityTiming | None, elapsed_frames: int,
) -> float: # fmt: skip
"""Chunk time passed `elapsed_frames` after the observation: that of target `elapsed_frames` of the chunk
timed from the (D,) pose commanded at the observation (the elapsed frames with uniform timing; the end
of the chunk once it is spent)."""
if elapsed_frames <= 0:
return 0.0
at = target_times(points, observed, times, command_dims, timing)
return float(at[elapsed_frames - 1]) if elapsed_frames <= len(at) else float(times[-1])
def after_latency(
points: np.ndarray, times: np.ndarray, commanded: np.ndarray, elapsed: float, command_dims: tuple[int, ...],
timing: VelocityTiming | None = None,
) -> np.ndarray: # fmt: skip
"""(H, D) points at `times` after the observation, arriving when `elapsed` of the chunk time has passed
(the elapsed frames with uniform timing) and the robot is commanded to the (D,) pose `commanded` -> (n, D)
targets for the frames after the arrival. Points within half a point spacing of the arrival or before it
are dropped; with none left n is 0 and the commanded pose is held."""
spacing = float(np.diff(np.concatenate([[0.0], times])).min())
keep = times - elapsed > 0.5 * spacing
if not keep.any():
return np.zeros((0, points.shape[1]), dtype=np.float64)
return to_frames(points[keep], commanded, times[keep] - elapsed, command_dims, timing)
@dataclasses.dataclass(frozen=True)
class ChunkTargets:
targets: np.ndarray # (n, D), one per control frame after the arrival
stalls: int # frames of `targets` that repeat the chunk's last target because it ran out (async)
def chunk_targets(
points: np.ndarray, times: np.ndarray, command_dims: tuple[int, ...], timing: VelocityTiming | None,
history: Sequence[np.ndarray], observed: int, latency_frames: int, mode: Mode, execute: int,
) -> ChunkTargets: # fmt: skip
"""The targets one chunk executes. `history` holds the (D,) poses commanded so far, the start pose first;
`observed` is its length when the chunk's observation was taken, so history[observed - 1] is the pose
commanded then. Every chunk but the first arrives `latency_frames` later in async mode, the robot having
executed history[observed:] meanwhile: it is rebuilt from history[-1] with the chunk time passed over the
frames that were not holding. The first chunk and sync chunks start from the observed pose, the robot
having waited `latency_frames` at it; the caller adds those waits. With velocity timing the targets begin
with the rest of any hold still owed at history[-1], less the waits. At most `execute` targets; in async
mode exactly `execute`, a chunk that runs out repeating its last target."""
first = len(history) == 1
arrival = latency_frames if mode == "async" and not first else 0
pose = history[observed - 1]
if arrival:
owed_then = timing.owed_hold(history[:observed], command_dims) if timing is not None else 0
moving = arrival - min(owed_then, arrival)
elapsed = arrival_time(points, pose, times, command_dims, timing, moving)
chunk = after_latency(points, times, history[-1], elapsed, command_dims, timing)
else:
chunk = to_frames(points, pose, times, command_dims, timing)
if timing is not None:
ahead = latency_frames if mode == "sync" or first else 0 # waited before the first target
owed = max(0, timing.owed_hold(history, command_dims) - ahead)
chunk = np.concatenate([np.repeat(np.asarray(history[-1:]), owed, axis=0), chunk])
run = chunk[:execute]
stalls = execute - len(run) if mode == "async" else 0
if stalls:
held = run[-1] if len(run) else np.asarray(history[-1])
run = np.concatenate([run, np.repeat(held[None, :], stalls, axis=0)])
return ChunkTargets(targets=np.asarray(run), stalls=stalls)
@dataclasses.dataclass(frozen=True)
class ArcInterpolator:
"""Construct once per model from the label report; call once per inference."""
span_frames: int
action_horizon: int
anchor: str
action_dim: int
command_dims: tuple[int, ...]
state_dims: tuple[int, ...] # state index of each action dim, -1 when none
rate: float = 1.0 # execution rate, chosen at deployment; the label report does not record it
timing: VelocityTiming | None = None # velocity timing capped by the rate; chosen at deployment
@staticmethod
def from_report(path: Path, rate: float = 1.0, timing: VelocityTiming | None = None) -> ArcInterpolator:
"""Everything from the arc_resample.json the label builder wrote, nothing restated by hand, the rate
and the timing."""
report = json.loads(Path(path).read_text(encoding="utf-8"))
config, robot = report["config"], report["robot"]
if "anchor" not in config:
raise ValueError(f"{path} records no anchor; pass it explicitly (datasets without it used 'action')")
return ArcInterpolator(
span_frames=int(config["span_frames"]),
action_horizon=int(config["action_horizon"]),
anchor=str(config["anchor"]),
action_dim=int(robot["action_dim"]),
command_dims=tuple(int(d) for d in robot["command_dims"]),
state_dims=tuple(int(d) for d in robot["state_dims"]),
rate=float(rate),
timing=timing,
)
def frames(self) -> np.ndarray:
return point_frames(self.span_frames, self.action_horizon, self.anchor, self.rate)
def current_pose(self, state: np.ndarray) -> np.ndarray:
"""The state expressed in the action layout; command dims without a state counterpart are 0."""
state = np.asarray(state, dtype=np.float64)
return np.array([state[s] if s >= 0 else 0.0 for s in self.state_dims])
def _checked(self, points: np.ndarray) -> np.ndarray:
points = np.asarray(points, dtype=np.float64)[:, : self.action_dim]
if points.shape != (self.action_horizon, self.action_dim):
raise ValueError(f"points {points.shape}, expected ({self.action_horizon}, {self.action_dim})")
return points
def __call__(self, points: np.ndarray, state: np.ndarray) -> np.ndarray:
"""Absolute (H, >=D) points and the state they were predicted from -> (n, D) per-frame targets.
Model padding past action_dim is dropped. `points` must be absolute targets, i.e. after the
policy has undone normalization and delta encoding.
"""
return to_frames(self._checked(points), self.current_pose(state), self.frames(), self.command_dims, self.timing)
def after_latency(
self, points: np.ndarray, observed: np.ndarray, commanded: np.ndarray, elapsed_frames: int
) -> np.ndarray:
"""The same points arriving `elapsed_frames` after the observation, rebuilt from the (D,) pose the
robot is commanded to at arrival -> (n, D) targets for the frames after the arrival; n = 0: hold the
commanded pose. `observed` is the (D,) pose commanded at the observation: the chunk timed from it
tells how much chunk time has passed (the elapsed frames with uniform timing). Poses in action
layout."""
points = self._checked(points)
commanded = np.asarray(commanded, dtype=np.float64)[: self.action_dim]
observed = np.asarray(observed, dtype=np.float64)[: self.action_dim]
times = self.frames()
elapsed = arrival_time(points, observed, times, self.command_dims, self.timing, elapsed_frames)
return after_latency(points, times, commanded, elapsed, self.command_dims, self.timing)
def chunk_targets(
self, points: np.ndarray, history: Sequence[np.ndarray], observed: int, latency_frames: int, mode: Mode,
execute: int,
) -> ChunkTargets: # fmt: skip
"""chunk_targets for absolute (H, >=D) points; `history` holds commanded poses in action layout."""
poses = [np.asarray(h, dtype=np.float64)[: self.action_dim] for h in history]
return chunk_targets(self._checked(points), self.frames(), self.command_dims, self.timing, poses, observed,
latency_frames, mode, execute) # fmt: skip