File size: 6,206 Bytes
3671737 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 | """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))
|