task57b1k / task 57 /policy_code /scripts /serve_b1k_2026.py
Luke-Xing's picture
Upload 27 files
3671737 verified
Raw History Blame Contribute Delete
6.21 kB
"""Serve a PI_BEHAVIOR checkpoint to the 2026 BEHAVIOR evaluator (omnigibson.eval.eval).
Uses the 2025 solution's B1KPolicyWrapper (rolling inpainting, stage voting, action compression)
behind openpi's 2026 websocket server, which speaks the same protocol as the 2026 evaluator client.
PYTHONPATH=src /workspace/openpi/.venv/bin/python scripts/serve_b1k_2026.py \
--policy-dir outputs/checkpoints/pi_behavior_sweep_lora/<exp>/<step>
Task mapping: the evaluator sends obs["task_id"] = 57 (sweeping_garage); the model was fine-tuned for
it in slot 36, so 57 is rewritten to 36. The 2025 "eval tricks" (hand-written per-task gripper/stage
correction rules) are OFF by default: slot 36's rules force both grippers open, which would make
holding the broom impossible.
"""
import dataclasses
import logging
import os
import socket
os.environ.setdefault("XLA_PYTHON_CLIENT_MEM_FRACTION", "0.45")
import numpy as np
import torch
import tyro
from openpi.serving import websocket_b1k_server
from b1k.policies import policy_config as _policy_config
from b1k.policies.b1k_policy import PROPRIOCEPTION_INDICES
from b1k.shared.eval_b1k_wrapper import B1KPolicyWrapper, B1KWrapperConfig
from b1k.training import config as _config
@dataclasses.dataclass
class Args:
policy_dir: str
config: str = "pi_behavior_sweep_lora"
port: int = 8000
# Evaluator task id -> model task slot.
task_map: dict[int, int] = dataclasses.field(default_factory=lambda: {57: 36})
actions_to_execute: int = 26
actions_to_keep: int = 4
execute_in_n_steps: int = 20
history_len: int = 3
votes_to_promote: int = 2
time_threshold_inpaint: float = 0.3
num_steps: int = 20
apply_eval_tricks: bool = False
# Clamp trunk commands to the envelope of the 2026 sweeping demos (see cap_torso). Off = raw model output.
torso_cap: bool = True
# If set, append per-step trunk state / commanded / capped trunk action to this CSV (diagnostics).
torso_log: str | None = None
# JoyLo teleop moves the trunk along upright -> downward -> ground (og_teleop_cfg.py R1_*_TORSO_JOINT_POS).
TORSO_PATH = np.array([[0.45, -0.4, 0.0, 0.0], [1.6, -2.5, -0.94, 0.0], [1.735, -2.57, -2.1, 0.0]])
# Per-joint range of trunk actions over all 894,606 frames of the 2026 sweeping_garage demos.
TORSO_DEMO_MIN = np.array([0.45, -2.563, -1.833, 0.0])
TORSO_DEMO_MAX = np.array([1.721, -0.4, 0.078, 0.0])
# 99.9th percentile distance of demo trunk actions from TORSO_PATH (max 0.55).
TORSO_MAX_PATH_DIST = 0.3
def _closest_on_torso_path(q: np.ndarray) -> np.ndarray:
best, best_d = None, np.inf
for a, b in zip(TORSO_PATH[:-1], TORSO_PATH[1:]):
d = b - a
p = a + np.clip((q - a) @ d / (d @ d), 0.0, 1.0) * d
if np.linalg.norm(q - p) < best_d:
best, best_d = p, np.linalg.norm(q - p)
return best
def cap_torso(q: np.ndarray) -> np.ndarray:
"""Keeps a trunk command inside the demo envelope: per-joint demo range, then at most
TORSO_MAX_PATH_DIST from the teleop trunk path. Stops the policy from leaning back/over."""
q = np.clip(q, TORSO_DEMO_MIN, TORSO_DEMO_MAX)
p = _closest_on_torso_path(q)
dist = np.linalg.norm(q - p)
if dist > TORSO_MAX_PATH_DIST:
q = p + (q - p) * (TORSO_MAX_PATH_DIST / dist)
return q
def _py(v):
return float(v) if isinstance(v, (np.floating, float)) else v
class TaskRemapWrapper:
"""Rewrites obs["task_id"] from the evaluator's task index to the model's task slot."""
def __init__(self, policy: B1KPolicyWrapper, task_map: dict[int, int], torso_cap: bool = True,
torso_log: str | None = None):
self._policy = policy
self._task_map = task_map
self._torso_cap = torso_cap
self._torso_log = open(torso_log, "a") if torso_log else None
self._episode = 0
self._step = 0
def act(self, obs: dict):
if "task_id" in obs:
task_id = int(np.asarray(obs["task_id"]).reshape(-1)[0])
obs["task_id"] = np.array([self._task_map.get(task_id, task_id)], dtype=np.int64)
action = self._policy.act(obs)
raw = action[3:7].numpy().copy()
if self._torso_cap:
action[3:7] = torch.from_numpy(cap_torso(raw.astype(np.float64))).to(action.dtype)
if self._torso_log is not None:
state = np.asarray(obs["robot_r1::proprio"]).reshape(-1)[PROPRIOCEPTION_INDICES["R1Pro"]["trunk_qpos"]]
row = [self._episode, self._step, *state, *raw, *action[3:7].numpy()]
self._torso_log.write(",".join(f"{v:.4f}" if isinstance(v, float) else str(v) for v in map(_py, row)) + "\n")
if self._step % 100 == 0:
self._torso_log.flush()
self._step += 1
return action
def reset(self):
self._policy.reset()
self._episode += 1
self._step = 0
def main(args: Args) -> None:
config = _config.get_config(args.config)
policy = _policy_config.create_trained_policy(config, args.policy_dir, sample_kwargs={"num_steps": args.num_steps})
wrapper_config = B1KWrapperConfig(
actions_to_execute=args.actions_to_execute,
actions_to_keep=args.actions_to_keep,
execute_in_n_steps=args.execute_in_n_steps,
history_len=args.history_len,
votes_to_promote=args.votes_to_promote,
time_threshold_inpaint=args.time_threshold_inpaint,
num_steps=args.num_steps,
apply_eval_tricks=args.apply_eval_tricks,
)
default_task = next(iter(args.task_map.values()))
wrapped = B1KPolicyWrapper(policy, task_id=default_task, config=wrapper_config)
wrapped = TaskRemapWrapper(wrapped, args.task_map, torso_cap=args.torso_cap, torso_log=args.torso_log)
logging.info(f"Serving {args.policy_dir} on port {args.port}; task map {args.task_map}; torso_cap={args.torso_cap}; {wrapper_config}")
logging.info("Host: %s", socket.gethostname())
websocket_b1k_server.WebsocketPolicyServer(
policy=wrapped, host="0.0.0.0", port=args.port, metadata=policy.metadata
).serve_forever()
if __name__ == "__main__":
logging.basicConfig(level=logging.INFO, force=True)
main(tyro.cli(Args))