Download deploy/arc_interp.py from JingwuLuo/ur3-optical-resample: direct link, hf CLI and curl.
- Browser
- Download file 17.9 kB
-
https://huggingface.co/JingwuLuo/ur3-optical-resample/resolve/main/deploy/arc_interp.py
- Command line
-
hf download hf://JingwuLuo/ur3-optical-resample/deploy/arc_interp.py
-
curl -L -o arc_interp.py https://huggingface.co/JingwuLuo/ur3-optical-resample/resolve/main/deploy/arc_interp.py
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 | |
| 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) | |
| 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) | |
| 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 | |
| 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 | |