Nadeem-Shoukath115113/opensarm / tools /patch_progress_gripper.py
Nadeem-Shoukath115113's picture
download
raw
24.5 kB
#!/usr/bin/env python
"""Patch the training dataset's reward (progress) column to allocate more
progress to gripper-close events and less to static frames.
The default LeRobot reward column is often just t/T (linear wall clock).
This script redistributes that progress so that frames where the gripper
is actively closing get a larger share of the progress delta, teaching the
reward model that grasping = meaningful progress.
Algorithm:
0. Calibrate a close/velocity threshold per gripper from its observed range
across the whole dataset. Arms are not zeroed alike (right reads ~22 fully
closed, left ~3), so one shared absolute threshold silently ignores an arm.
1. Read the parquet's state column → extract gripper positions.
2. Detect gripper-close events (velocity + threshold, same logic as the
label-time heuristic).
3. Build a per-frame importance weight: base_weight everywhere, higher
weight at gripper-close frames.
4. Re-distribute the total progress [0, 1] proportional to these weights
via cumulative-sum normalization.
5. Write the patched reward column back.
The total progress still goes from 0 → 1, but the slope is steeper during
grasps and flatter during static periods.
Usage:
python tools/patch_progress_gripper.py \\
--dataset-dir /path/to/lerobot/dataset \\
--gripper-indices 5 11 \\
--boost-weight 3.0 \\
--dry-run # preview without writing
# thresholds are auto-calibrated per arm by default; override explicitly with
# --close-threshold 5.0 25.0 --velocity-threshold 2.0
# or disable calibration with --no-auto-threshold
"""
from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
import numpy as np
import pyarrow as pa
import pyarrow.parquet as pq
def _broadcast_per_arm(flag: str, values: float | list[float], n: int) -> list[float]:
"""Expand a shared scalar or a one-per-arm list into exactly n values."""
vals = [float(values)] if isinstance(values, (int, float)) else [float(v) for v in values]
if len(vals) == 1:
vals = vals * n
if len(vals) != n:
raise ValueError(
f"{flag} has {len(vals)} values but --gripper-indices has {n}; "
"pass one shared value or one per index"
)
return vals
def _was_open_before(
grip: np.ndarray,
t: int,
thr: float,
min_open_frames: int,
lookback: int,
) -> bool:
"""True if the gripper held open for min_open_frames in a row shortly before t.
Rejects chatter around the threshold, which otherwise fires a fresh "grasp"
on every oscillation without the gripper ever having opened.
"""
lo = max(0, t - lookback)
if t - lo < min_open_frames:
return False
run = 0
for is_open in grip[lo:t] >= thr:
run = run + 1 if is_open else 0
if run >= min_open_frames:
return True
return False
def detect_gripper_close_frames(
states: np.ndarray,
gripper_indices: list[int],
close_threshold: float | list[float],
velocity_threshold: float | list[float],
window_half: int = 3,
act_pri_gate: np.ndarray | None = None,
hold_frames: int = 0,
arm_event_counts: dict[int, int] | None = None,
warmup_frames: int = 0,
refractory_frames: int = 0,
min_open_frames: int = 0,
open_lookback: int = 30,
) -> np.ndarray:
"""Return per-frame float weight in [0, 1] for gripper-close intensity."""
T = states.shape[0]
mask = np.zeros(T, dtype=np.float64)
n_arms = len(gripper_indices)
thresholds = _broadcast_per_arm("--close-threshold", close_threshold, n_arms)
velocities = _broadcast_per_arm("--velocity-threshold", velocity_threshold, n_arms)
for gi, thr, vel_thr in zip(gripper_indices, thresholds, velocities):
if gi >= states.shape[1]:
continue
grip = states[:, gi].astype(np.float64)
vel = np.zeros_like(grip)
vel[1:] = grip[1:] - grip[:-1]
last_event = -(10**9)
for t in range(max(1, warmup_frames), T):
if act_pri_gate is not None and not bool(act_pri_gate[t]):
continue
if t - last_event < refractory_frames:
continue
if grip[t] < thr and vel[t] < -vel_thr:
if hold_frames > 0:
hi = min(T, t + hold_frames)
if hi <= t:
continue
# Require the gripper to remain in the closed region for a short horizon.
if not np.all(grip[t:hi] < thr):
continue
if min_open_frames > 0 and not _was_open_before(
grip, t, thr, min_open_frames, open_lookback
):
continue
last_event = t
if arm_event_counts is not None:
arm_event_counts[gi] = arm_event_counts.get(gi, 0) + 1
lo = max(0, t - window_half)
hi = min(T, t + window_half + 1)
for k in range(lo, hi):
dist = abs(k - t)
w = 0.5 * (1.0 + np.cos(np.pi * dist / (window_half + 1)))
mask[k] = max(mask[k], w)
# A boost inside the warmup would put a step at the very start of the episode.
if warmup_frames > 0:
mask[:warmup_frames] = 0.0
return mask
def redistribute_progress(
n_frames: int,
gripper_mask: np.ndarray,
base_weight: float = 1.0,
boost_weight: float = 3.0,
) -> np.ndarray:
"""Build a new progress curve [0, 1] with more slope at gripper events.
Each frame gets weight = base_weight + boost_weight * gripper_mask[t].
Progress is the normalized cumulative sum of these weights.
"""
weights = base_weight + boost_weight * gripper_mask
cumw = np.cumsum(weights)
# Anchor on cumw[0] so frame 0 is exactly 0.0; plain cumw/cumw[-1] starts at w[0]/total,
# which is a visible jump out of the gate.
span = cumw[-1] - cumw[0]
if span <= 0:
return np.linspace(0.0, 1.0, len(cumw), dtype=np.float32)
progress = (cumw - cumw[0]) / span
return progress.astype(np.float32)
def _choose_col(candidates: list[str], cols: list[str], required: bool = True) -> str | None:
for c in candidates:
if c in cols:
return c
if required:
raise KeyError(f"None of required columns found: {candidates}. Available: {cols}")
return None
def _to_scalar_array(values: list) -> np.ndarray:
out = []
for v in values:
if isinstance(v, (list, tuple, np.ndarray)):
out.append(float(v[0]))
else:
out.append(float(v))
return np.asarray(out, dtype=np.float64)
def _load_act_pri_per_frame(labels_dir: Path, episode_index: int) -> np.ndarray | None:
p = labels_dir / "per_frame" / f"episode_{episode_index:06d}.npy"
if not p.exists():
return None
return np.load(p).astype(np.int64).reshape(-1)
def calibrate_thresholds(
parquets: list[Path],
gripper_indices: list[int],
closed_frac: float,
velocity_frac: float,
) -> tuple[list[float], list[float], dict[int, tuple[float, float]]]:
"""Derive a close/velocity threshold per arm from its range over the whole dataset.
Absolute thresholds are not portable between arms: a servo that reads ~22 when
fully closed never trips a shared threshold of 20 and contributes zero events.
"""
lo: dict[int, float] = {gi: np.inf for gi in gripper_indices}
hi: dict[int, float] = {gi: -np.inf for gi in gripper_indices}
for pf in parquets:
table = pq.read_table(pf)
state_col = _choose_col(["state", "observation.state"], table.column_names, required=True)
states = np.asarray(table[state_col].to_pylist(), dtype=np.float64)
for gi in gripper_indices:
if gi >= states.shape[1]:
continue
lo[gi] = min(lo[gi], float(states[:, gi].min()))
hi[gi] = max(hi[gi], float(states[:, gi].max()))
thresholds: list[float] = []
velocities: list[float] = []
ranges: dict[int, tuple[float, float]] = {}
for gi in gripper_indices:
if not np.isfinite(lo[gi]) or not np.isfinite(hi[gi]):
raise SystemExit(f"gripper index {gi} not present in the state vector")
span = hi[gi] - lo[gi]
if span <= 0:
raise SystemExit(f"gripper index {gi} never moves (min == max == {lo[gi]:.2f})")
thresholds.append(lo[gi] + closed_frac * span)
velocities.append(velocity_frac * span)
ranges[gi] = (lo[gi], hi[gi])
return thresholds, velocities, ranges
def patch_parquet(
parquet_path: Path,
gripper_indices: list[int],
close_threshold: float | list[float],
velocity_threshold: float | list[float],
boost_weight: float,
window_half: int,
dry_run: bool,
act_pri_labels_dir: Path | None,
grasp_task_indices: set[int] | None,
hold_frames: int,
warmup_frames: int = 0,
refractory_frames: int = 0,
min_open_frames: int = 0,
) -> dict:
"""Patch one parquet file. Returns stats dict.
Supports both:
- per-episode files: data/chunk-*/episode_XXXXXX.parquet
- aggregated files: data/chunk-*/file-XXX.parquet
"""
table = pq.read_table(parquet_path)
cols = table.column_names
state_col = _choose_col(["state", "observation.state"], cols, required=True)
episode_col = _choose_col(["episode_index"], cols, required=True)
frame_col = _choose_col(["frame_index"], cols, required=False)
reward_col = _choose_col(["reward", "progress"], cols, required=False)
states = np.asarray(table[state_col].to_pylist(), dtype=np.float64)
episode_idx = _to_scalar_array(table[episode_col].to_pylist()).astype(np.int64)
if frame_col is not None:
frame_idx = _to_scalar_array(table[frame_col].to_pylist()).astype(np.int64)
else:
frame_idx = np.arange(table.num_rows, dtype=np.int64)
old_progress_all = None
if reward_col is not None:
old_progress_all = _to_scalar_array(table[reward_col].to_pylist())
new_progress_all = np.zeros(table.num_rows, dtype=np.float32)
per_ep_stats = []
total_grasp_frames = 0
clock_sse = 0.0
clock_n = 0
arm_event_counts: dict[int, int] = {}
all_eps = np.unique(episode_idx)
for ep in all_eps:
rows = np.where(episode_idx == ep)[0]
if rows.size == 0:
continue
order = np.argsort(frame_idx[rows], kind="stable")
rows_sorted = rows[order]
ep_states = states[rows_sorted]
act_pri_gate = None
if act_pri_labels_dir is not None and grasp_task_indices is not None and len(grasp_task_indices) > 0:
per_frame = _load_act_pri_per_frame(act_pri_labels_dir, int(ep))
if per_frame is not None and per_frame.size > 0:
max_f = int(frame_idx[rows_sorted].max())
if max_f < per_frame.size:
picked = per_frame[frame_idx[rows_sorted]]
act_pri_gate = np.isin(picked, list(grasp_task_indices))
ep_mask = detect_gripper_close_frames(
ep_states,
gripper_indices,
close_threshold,
velocity_threshold,
window_half,
act_pri_gate=act_pri_gate,
hold_frames=hold_frames,
arm_event_counts=arm_event_counts,
warmup_frames=warmup_frames,
refractory_frames=refractory_frames,
min_open_frames=min_open_frames,
)
n_grasp_frames = int((ep_mask > 0.5).sum())
total_grasp_frames += n_grasp_frames
ep_new_progress = redistribute_progress(
len(rows_sorted), ep_mask, base_weight=1.0, boost_weight=boost_weight
)
new_progress_all[rows_sorted] = ep_new_progress
if old_progress_all is not None:
ep_old_progress = old_progress_all[rows_sorted]
else:
n = len(rows_sorted)
ep_old_progress = np.arange(n, dtype=np.float64) / max(1, n - 1)
old_delta = np.diff(ep_old_progress)
new_delta = np.diff(ep_new_progress)
grasp_frames_idx = np.where(ep_mask[1:] > 0.5)[0]
if len(grasp_frames_idx) > 0:
old_slope_at_grasp = float(old_delta[grasp_frames_idx].mean())
new_slope_at_grasp = float(new_delta[grasp_frames_idx].mean())
else:
old_slope_at_grasp = 0.0
new_slope_at_grasp = 0.0
lin = np.linspace(0.0, 1.0, len(rows_sorted), dtype=np.float64)
resid = ep_new_progress.astype(np.float64) - lin
clock_sse += float((resid**2).sum())
clock_n += int(len(rows_sorted))
per_ep_stats.append(
{
"episode_index": int(ep),
"frames": int(len(rows_sorted)),
"grasp_frames": n_grasp_frames,
"old_slope_at_grasp": old_slope_at_grasp,
"new_slope_at_grasp": new_slope_at_grasp,
"max_dev_from_linear": float(np.abs(resid).max()),
"mean_dev_from_linear": float(np.abs(resid).mean()),
"max_step": float(np.diff(ep_new_progress).max()) if len(rows_sorted) > 1 else 0.0,
"first_step": float(ep_new_progress[1] - ep_new_progress[0]) if len(rows_sorted) > 1 else 0.0,
}
)
# Per-episode diff so the delta at frame 0 of each episode is 0 (no cross-episode jumps).
new_delta_all = np.zeros(table.num_rows, dtype=np.float32)
for ep in all_eps:
rows = np.where(episode_idx == ep)[0]
order = np.argsort(frame_idx[rows], kind="stable")
rows_sorted = rows[order]
ep_prog = new_progress_all[rows_sorted]
ep_delta = np.diff(ep_prog, prepend=ep_prog[0]).astype(np.float32)
new_delta_all[rows_sorted] = ep_delta
if not dry_run:
new_reward_col = pa.FixedSizeListArray.from_arrays(
pa.array(new_progress_all.astype(np.float32), type=pa.float32()), 1
)
new_delta_col = pa.FixedSizeListArray.from_arrays(
pa.array(new_delta_all.astype(np.float32), type=pa.float32()), 1
)
if reward_col is None:
table = table.append_column("reward", new_reward_col)
else:
col_idx = table.column_names.index(reward_col)
table = table.set_column(col_idx, "reward", new_reward_col)
if reward_col == "progress":
# Keep downstream readers consistent on a single field name.
names = table.column_names
col_idx = names.index("reward")
table = table.remove_column(col_idx)
table = table.append_column("reward", new_reward_col)
# SPIRAL's TD path wants a per-step signal in the `reward` slot; SARM2 wants
# the cumulative curve. Keep `reward` cumulative here and expose the delta as
# `reward_delta` so downstream trainers can pick whichever fits.
if "reward_delta" in table.column_names:
di = table.column_names.index("reward_delta")
table = table.set_column(di, "reward_delta", new_delta_col)
else:
table = table.append_column("reward_delta", new_delta_col)
pq.write_table(table, parquet_path)
return {
"file": str(parquet_path.name),
"rows": int(table.num_rows),
"episodes": int(len(all_eps)),
"grasp_frames": int(total_grasp_frames),
"arm_events": arm_event_counts,
"patched": not dry_run,
"had_reward_col": reward_col is not None,
"clock_sse": clock_sse,
"clock_n": clock_n,
"per_episode": per_ep_stats,
}
def main():
parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument("--dataset-dir", type=str, required=True, help="Root of the LeRobot dataset")
parser.add_argument("--gripper-indices", type=int, nargs="+", default=[5, 11])
parser.add_argument("--close-threshold", type=float, nargs="+", default=None,
help="One shared value or one per --gripper-indices. Overrides auto-calibration.")
parser.add_argument("--velocity-threshold", type=float, nargs="+", default=None,
help="One shared value or one per --gripper-indices. Overrides auto-calibration.")
parser.add_argument("--auto-threshold", action=argparse.BooleanOptionalAction, default=True,
help="Derive per-arm thresholds from each gripper's range over the dataset")
parser.add_argument("--closed-frac", type=float, default=0.20,
help="Auto mode: closed region is the bottom this fraction of an arm's range")
parser.add_argument("--velocity-frac", type=float, default=0.03,
help="Auto mode: minimum per-frame closing speed as a fraction of an arm's range")
parser.add_argument("--boost-weight", type=float, default=3.0,
help="How much extra weight gripper-close frames get (1.0 = double, 3.0 = 4x)")
parser.add_argument("--window-half", type=int, default=3)
parser.add_argument("--act-pri-labels-dir", type=str, default=None,
help="Path to one act_pri prediction store session (contains per_frame/episode_*.npy)")
parser.add_argument("--grasp-task-indices", type=int, nargs="+", default=[7],
help="Act_pri task indices treated as grasp primitives (default: 7=pick_grab)")
parser.add_argument("--hold-frames", type=int, default=5,
help="Require close condition to hold for this many future frames")
parser.add_argument("--warmup-frames", type=int, default=15,
help="Suppress boosts in the first N frames so progress cannot jump at episode start")
parser.add_argument("--refractory-frames", type=int, default=15,
help="Minimum gap between accepted events on one arm; debounces repeat triggers")
parser.add_argument("--min-open-frames", type=int, default=3,
help="Require the gripper to be open this many frames in a row before a close counts")
parser.add_argument("--dry-run", action="store_true", help="Preview without writing")
parser.add_argument("--summary-max-episodes", type=int, default=8,
help="Max per-episode lines to print per parquet file")
args = parser.parse_args()
dataset_dir = Path(args.dataset_dir)
parquets = sorted(dataset_dir.glob("data/chunk-*/*.parquet"))
if not parquets:
print(f"No parquet files found under {dataset_dir}/data/chunk-*")
sys.exit(1)
meta_info = dataset_dir / "meta" / "info.json"
if meta_info.exists():
try:
info = json.loads(meta_info.read_text())
print(f"dataset_name={info.get('dataset_name')} fps={info.get('fps')}")
except Exception:
pass
print(f"{'DRY RUN — ' if args.dry_run else ''}Processing {len(parquets)} parquet file(s)")
n_arms = len(args.gripper_indices)
close_threshold: list[float]
velocity_threshold: list[float]
if args.auto_threshold and (args.close_threshold is None or args.velocity_threshold is None):
auto_close, auto_vel, ranges = calibrate_thresholds(
parquets, args.gripper_indices, args.closed_frac, args.velocity_frac
)
for gi, thr, vel in zip(args.gripper_indices, auto_close, auto_vel):
lo, hi = ranges[gi]
print(f" [calib] gripper idx {gi}: range [{lo:.2f}, {hi:.2f}] "
f"-> close<{thr:.2f}, closing_speed>{vel:.2f}")
else:
auto_close, auto_vel = [20.0] * n_arms, [2.0] * n_arms
close_threshold = (_broadcast_per_arm("--close-threshold", args.close_threshold, n_arms)
if args.close_threshold is not None else auto_close)
velocity_threshold = (_broadcast_per_arm("--velocity-threshold", args.velocity_threshold, n_arms)
if args.velocity_threshold is not None else auto_vel)
print(f" gripper_indices={args.gripper_indices}, "
f"close_threshold={[round(v, 2) for v in close_threshold]}, "
f"velocity_threshold={[round(v, 2) for v in velocity_threshold]}, "
f"boost_weight={args.boost_weight}")
print()
total_grasp = 0
total_patched = 0
total_episodes = 0
clock_sse_all = 0.0
clock_n_all = 0
all_ep_rows: list[dict] = []
arm_totals: dict[int, int] = {gi: 0 for gi in args.gripper_indices}
act_pri_labels_dir = Path(args.act_pri_labels_dir).expanduser().resolve() if args.act_pri_labels_dir else None
grasp_task_indices = set(int(x) for x in args.grasp_task_indices) if args.grasp_task_indices else None
for pf in parquets:
stats = patch_parquet(
pf,
gripper_indices=args.gripper_indices,
close_threshold=close_threshold,
velocity_threshold=velocity_threshold,
boost_weight=args.boost_weight,
window_half=args.window_half,
dry_run=args.dry_run,
act_pri_labels_dir=act_pri_labels_dir,
grasp_task_indices=grasp_task_indices,
hold_frames=args.hold_frames,
warmup_frames=args.warmup_frames,
refractory_frames=args.refractory_frames,
min_open_frames=args.min_open_frames,
)
total_grasp += int(stats["grasp_frames"])
total_episodes += int(stats["episodes"])
clock_sse_all += float(stats["clock_sse"])
clock_n_all += int(stats["clock_n"])
all_ep_rows.extend(stats["per_episode"])
for gi, n in stats["arm_events"].items():
arm_totals[gi] = arm_totals.get(gi, 0) + int(n)
if int(stats["grasp_frames"]) > 0:
total_patched += 1
print(f" {stats['file']}: rows={stats['rows']} episodes={stats['episodes']} "
f"grasp_frames={stats['grasp_frames']} "
f"existing_reward={'yes' if stats['had_reward_col'] else 'no'}")
ep_stats = stats["per_episode"]
ep_stats = sorted(ep_stats, key=lambda x: x["grasp_frames"], reverse=True)
for row in ep_stats[: args.summary_max_episodes]:
if row["grasp_frames"] <= 0:
continue
print(
f" ep {row['episode_index']:06d}: frames={row['frames']} "
f"grasp_frames={row['grasp_frames']} "
f"slope {row['old_slope_at_grasp']:.6f} -> {row['new_slope_at_grasp']:.6f} "
f"max_dev={row['max_dev_from_linear']:.4f}"
)
print(f"\nDone. {total_patched}/{len(parquets)} parquet files had grasp events "
f"covering {total_episodes} episodes and {total_grasp} total grasp frames")
print(" events per arm:")
for gi in args.gripper_indices:
n = arm_totals.get(gi, 0)
flag = " <-- NO EVENTS, threshold likely wrong for this arm" if n == 0 else ""
print(f" gripper idx {gi}: {n}{flag}")
if all_ep_rows:
max_dev = np.array([r["max_dev_from_linear"] for r in all_ep_rows])
mean_dev = np.array([r["mean_dev_from_linear"] for r in all_ep_rows])
first_step = np.array([r["first_step"] for r in all_ep_rows])
max_step = np.array([r["max_step"] for r in all_ep_rows])
zero_ev = sum(1 for r in all_ep_rows if r["grasp_frames"] == 0)
clock_mse = clock_sse_all / max(1, clock_n_all)
print("\n deviation from the linear ramp (this is what stops the model telling time):")
print(f" max |new - linear|: mean {max_dev.mean():.4f} min {max_dev.min():.4f} max {max_dev.max():.4f}")
print(f" mean |new - linear|: mean {mean_dev.mean():.4f}")
print(f" CLOCK BASELINE MSE : {clock_mse:.6f} <-- SARM2 must beat this")
print(f" episodes with no events: {zero_ev}/{len(all_ep_rows)}")
print(" start/step sanity:")
print(f" first step (frame 0->1): max {first_step.max():.6f}")
print(f" largest single step : max {max_step.max():.6f}")
if args.dry_run:
print("(dry run — no files modified)")
if __name__ == "__main__":
main()

Xet Storage Details

Size:
24.5 kB
·
Xet hash:
fe863ee789191b09e14a395a70c2aa8118b5eb1c19d70cf2b03a1295cf3b89ae

Xet efficiently stores files, intelligently splitting them into unique chunks and accelerating uploads and downloads. More info.