refactor(pi052): introduce generic language runtime

This commit is contained in:
Pepijn
2026-06-23 12:00:25 +02:00
parent 6f0c776017
commit 020dbab8f9
15 changed files with 2723 additions and 3082 deletions
@@ -0,0 +1,35 @@
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Generic runtime primitives for language-conditioned policies."""
from .runtime import (
LanguageConditionedPolicyAdapter,
LanguageConditionedRuntime,
RuntimeState,
Tick,
TickClock,
ToolCall,
VQAResult,
)
__all__ = [
"LanguageConditionedPolicyAdapter",
"LanguageConditionedRuntime",
"RuntimeState",
"Tick",
"TickClock",
"ToolCall",
"VQAResult",
]
@@ -0,0 +1,432 @@
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Small reusable runtime for language-conditioned robot policies."""
from __future__ import annotations
import logging
import time
from collections import deque
from collections.abc import Callable
from dataclasses import dataclass, field
from typing import Any, Protocol
logger = logging.getLogger(__name__)
@dataclass
class ToolCall:
"""A pending runtime tool invocation."""
name: str
arguments: dict[str, Any] = field(default_factory=dict)
@dataclass
class VQAResult:
"""Text answer plus optional parsed spatial payload."""
answer: str
parsed: dict[str, Any] | None = None
camera: str | None = None
@dataclass
class RuntimeState:
"""Explicit state shared by the runtime and policy adapter."""
task: str = ""
language_context: dict[str, str] = field(default_factory=dict)
action_queue: deque[Any] = field(default_factory=deque)
events: set[str] = field(default_factory=set)
pending_tools: list[ToolCall] = field(default_factory=list)
log_lines: list[str] = field(default_factory=list)
mode: str = "action"
stop: bool = False
tick: Tick | None = None
actions_dispatched: int = 0
action_deadline: float | None = None
extra: dict[str, Any] = field(default_factory=dict)
_ALIASES = {
"current_plan": ("language_context", "plan"),
"current_subtask": ("language_context", "subtask"),
"current_memory": ("language_context", "memory"),
"tool_calls_pending": ("pending_tools", None),
"events_this_tick": ("events", None),
"_tick": ("tick", None),
}
def emit(self, event_name: str) -> None:
self.events.add(event_name)
def take_event(self, event_name: str) -> bool:
if event_name not in self.events:
return False
self.events.remove(event_name)
return True
def log(self, line: str) -> None:
self.log_lines.append(line)
def set_context(self, key: str, value: str | None, *, label: str | None = None) -> bool:
previous = self.language_context.get(key)
if previous == value:
return False
if value is None:
self.language_context.pop(key, None)
else:
self.language_context[key] = value
if label is not None and value:
self.log(f" {label}: {value}")
return True
def get(self, key: str, default: Any = None) -> Any:
try:
return self[key]
except KeyError:
return default
def setdefault(self, key: str, default: Any = None) -> Any:
current = self.get(key, None)
if current is not None:
return current
self[key] = default
return default
def __getitem__(self, key: str) -> Any:
alias = self._ALIASES.get(key)
if alias is not None:
target, subkey = alias
value = getattr(self, target)
return value if subkey is None else value.get(subkey)
if hasattr(self, key):
return getattr(self, key)
if key in self.extra:
return self.extra[key]
raise KeyError(key)
def __setitem__(self, key: str, value: Any) -> None:
alias = self._ALIASES.get(key)
if alias is not None:
target, subkey = alias
if subkey is None:
setattr(self, target, value)
elif value is None:
getattr(self, target).pop(subkey, None)
else:
getattr(self, target)[subkey] = value
return
if hasattr(self, key):
setattr(self, key, value)
else:
self.extra[key] = value
class LanguageConditionedPolicyAdapter(Protocol):
"""Policy-specific bridge used by :class:`LanguageConditionedRuntime`."""
def select_action(self, observation: dict[str, Any], state: RuntimeState) -> Any: ...
def select_text(
self,
kind: str,
observation: dict[str, Any] | None,
state: RuntimeState,
user_text: str | None = None,
) -> str: ...
def parse_tool_calls(self, text: str) -> list[ToolCall]: ...
def answer_vqa(
self,
question: str,
camera: str | None,
observation: dict[str, Any] | None,
state: RuntimeState,
) -> VQAResult: ...
@dataclass
class Tick:
index: int
monotonic_seconds: float
@dataclass
class TickClock:
max_rate_hz: float = 50.0
_index: int = field(default=0, init=False)
_last_seconds: float | None = field(default=None, init=False)
def advance(self) -> Tick:
period = 1.0 / max(self.max_rate_hz, 0.1)
now = time.monotonic()
if self._last_seconds is not None:
sleep_for = (self._last_seconds + period) - now
if sleep_for > 0:
time.sleep(sleep_for)
now = time.monotonic()
self._last_seconds = now
self._index += 1
return Tick(index=self._index, monotonic_seconds=now)
@dataclass
class _RateGate:
hz: float
_last_seconds: float | None = None
def due(self, tick: Tick, *, force: bool = False) -> bool:
if force:
self._last_seconds = tick.monotonic_seconds
return True
period = 1.0 / max(self.hz, 1e-6)
if self._last_seconds is None or tick.monotonic_seconds - self._last_seconds >= period:
self._last_seconds = tick.monotonic_seconds
return True
return False
def rearm(self) -> None:
self._last_seconds = None
@dataclass
class LanguageConditionedRuntime:
"""Generic tick loop for language-conditioned robot policies."""
policy_adapter: LanguageConditionedPolicyAdapter
observation_provider: Callable[[], dict[str, Any] | None] | None = None
action_executor: Callable[[Any], None] | None = None
tools: dict[str, Any] = field(default_factory=dict)
event_collector: Callable[[RuntimeState], None] | None = None
chunk_hz: float = 4.0
ctrl_hz: float = 50.0
high_level_hz: float = 1.0
max_rate_hz: float = 50.0
state: RuntimeState = field(default_factory=RuntimeState)
_chunk_gate: _RateGate = field(init=False)
_ctrl_gate: _RateGate = field(init=False)
_language_gate: _RateGate = field(init=False)
_stop: bool = field(default=False, init=False)
_last_dispatch_seconds: float | None = field(default=None, init=False)
def __post_init__(self) -> None:
self._chunk_gate = _RateGate(self.chunk_hz)
self._ctrl_gate = _RateGate(self.ctrl_hz)
self._language_gate = _RateGate(self.high_level_hz)
@property
def policy(self) -> Any:
return getattr(self.policy_adapter, "policy", self.policy_adapter)
def set_task(self, task: str) -> None:
self.state.task = task
self.state.log(f"Task: {task}")
def stop(self) -> None:
self._stop = True
self.state.stop = True
def run(self, *, max_ticks: int | None = None) -> None:
clock = TickClock(max_rate_hz=self.max_rate_hz)
while not self._stop:
tick = clock.advance()
self._run_tick(tick)
self._flush_logs()
if self.state.stop:
self._stop = True
if max_ticks is not None and tick.index >= max_ticks:
break
self._on_shutdown()
def step_once(self) -> list[str]:
previous = self.state.tick.index if self.state.tick is not None else 0
tick = Tick(index=previous + 1, monotonic_seconds=time.monotonic())
self._run_tick(tick, force_rates=True)
return list(self.state.log_lines)
def _run_tick(self, tick: Tick, *, force_rates: bool = False) -> None:
self.state.tick = tick
self.state.log_lines = []
if self.event_collector is not None:
self.event_collector(self.state)
self._handle_action_deadline()
if self.state.stop:
return
self.maybe_update_language_state(force=force_rates)
self.maybe_handle_user_events()
self.maybe_enqueue_action_chunk(force=force_rates)
self.dispatch_action(force=force_rates)
self.dispatch_tools()
self.state.events.clear()
def _current_observation(self) -> dict[str, Any] | None:
if self.observation_provider is None:
return None
try:
return self.observation_provider()
except Exception as exc: # noqa: BLE001
logger.debug("observation_provider failed: %s", exc)
return None
def maybe_update_language_state(self, *, force: bool = False) -> None:
if self.state.mode != "action" or not self.state.task:
return
if self.state.action_queue:
self._language_gate.rearm()
return
if self.state.tick is None or not self._language_gate.due(self.state.tick, force=force):
return
update = getattr(self.policy_adapter, "update_language_state", None)
if update is None:
return
observation = self._current_observation()
try:
update(observation, self.state)
except Exception as exc: # noqa: BLE001
logger.warning("language update failed: %s", exc, exc_info=logger.isEnabledFor(logging.DEBUG))
self.state.log(f" [warn] language update failed: {type(exc).__name__}: {exc}")
def maybe_handle_user_events(self) -> None:
if self.state.take_event("user_interjection"):
self._handle_user_interjection()
if self.state.take_event("user_vqa_query"):
self._handle_vqa_query()
def _handle_user_interjection(self) -> None:
text = str(self.state.extra.get("recent_interjection") or "")
if not text:
return
observation = self._current_observation()
out = self.policy_adapter.select_text("interjection", observation, self.state, user_text=text)
if not out:
return
calls = self.policy_adapter.parse_tool_calls(out)
for call in calls:
self.state.pending_tools.append(call)
if calls:
self.state.emit("tool_call_pending")
for call in calls:
if call.name == "say" and call.arguments.get("text"):
self.state.log(f" speech: {call.arguments['text']}")
plan = getattr(self.policy_adapter, "plan_from_text", lambda value: value)(out)
if plan:
self.state.set_context("plan", plan, label="plan")
self.state.extra["recent_interjection"] = None
def _handle_vqa_query(self) -> None:
question = str(self.state.extra.get("recent_vqa_query") or "")
if not question:
return
observation = self._current_observation()
result = self.policy_adapter.answer_vqa(question, None, observation, self.state)
if result.answer:
self.state.log(f" vqa: {result.answer}")
self.state.extra["recent_vqa_query"] = None
def maybe_enqueue_action_chunk(self, *, force: bool = False) -> None:
if self.state.mode != "action" or not self.state.task:
return
if self.state.action_queue:
return
if self.state.tick is None or not self._chunk_gate.due(self.state.tick, force=force):
return
observation = self._current_observation()
if observation is None:
return
try:
chunk = self.policy_adapter.select_action(observation, self.state)
except Exception as exc: # noqa: BLE001
logger.warning("select_action failed: %s", exc, exc_info=logger.isEnabledFor(logging.DEBUG))
self.state.log(f" [warn] select_action failed: {type(exc).__name__}: {exc}")
return
self._enqueue_chunk(chunk)
def _enqueue_chunk(self, chunk: Any) -> None:
if chunk is None:
return
chunk_iter = chunk[0] if getattr(chunk, "ndim", None) == 3 else chunk
if getattr(chunk_iter, "ndim", None) == 1:
chunk_iter = chunk_iter.unsqueeze(0)
for step in chunk_iter:
self.state.action_queue.append(step.unsqueeze(0) if hasattr(step, "unsqueeze") else step)
try:
self.state.extra["last_chunk_size"] = int(chunk_iter.shape[0])
except Exception: # noqa: BLE001
self.state.extra["last_chunk_size"] = len(self.state.action_queue)
def dispatch_action(self, *, force: bool = False) -> None:
if self.state.mode != "action":
self._last_dispatch_seconds = None
return
if self.state.tick is None or not self._ctrl_gate.due(self.state.tick, force=force):
return
queue = self.state.action_queue
if not queue:
self._last_dispatch_seconds = None
return
now = time.monotonic()
if self._last_dispatch_seconds is None or self.ctrl_hz <= 0:
n_to_pop = 1
else:
n_to_pop = max(1, min(len(queue), int(round((now - self._last_dispatch_seconds) * self.ctrl_hz))))
self._last_dispatch_seconds = now
latest = None
for _ in range(n_to_pop):
if not queue:
break
latest = queue.popleft()
self.state.actions_dispatched += 1
if latest is not None and self.action_executor is not None:
self.action_executor(latest)
def dispatch_tools(self) -> None:
if not (self.state.take_event("tool_call_pending") or self.state.pending_tools):
return
pending = list(self.state.pending_tools)
self.state.pending_tools = []
for call in pending:
name = call.name if isinstance(call, ToolCall) else (call.get("function") or {}).get("name")
args = (
call.arguments
if isinstance(call, ToolCall)
else (call.get("function") or {}).get("arguments", {})
)
tool = self.tools.get(name)
if tool is None:
self.state.log(f" [warn] tool {name!r} not registered — skipping call")
continue
try:
tool.call(args)
except Exception as exc: # noqa: BLE001
self.state.log(f" [error] tool dispatch failed: {exc}")
def _handle_action_deadline(self) -> None:
deadline = self.state.action_deadline
if self.state.mode == "action" and deadline is not None and time.monotonic() >= deadline:
self.state.mode = "paused"
self.state.action_deadline = None
self.state.action_queue.clear()
self.state.log("timed action elapsed — paused")
def _flush_logs(self) -> None:
for line in self.state.log_lines:
print(f"[runtime] {line}", flush=True)
def _on_shutdown(self) -> None:
self.state.action_queue.clear()
print("[runtime] stopped", flush=True)
@@ -11,62 +11,33 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""PI052 inference / runtime orchestration.
Multi-rate runtime that mirrors the recipe-time training shape:
"""PI052 runtime adapter and CLI helpers."""
low_level_execution → LowLevelForward + DispatchAction (high Hz)
high_level_subtask → HighLevelSubtaskFwd (~1 Hz)
memory_update → MemoryUpdateFwd (event: subtask_change)
user_interjection_response → UserInterjectionFwd (event: stdin)
ask_vqa_* → AskVQAFwd (event: stdin question)
speech tool calls → DispatchToolCalls (event: tool_call_pending)
The CLI ``lerobot-pi052-runtime`` builds a ``PI052Runtime`` and calls
``run()``.
"""
from lerobot.policies.language_conditioned import (
LanguageConditionedRuntime,
RuntimeState,
Tick,
TickClock,
ToolCall,
VQAResult,
)
from .pi052_adapter import PI052PolicyAdapter
from .repl import StdinReader
from .runtime import PI052Runtime
from .runtime_state import initial_runtime_state, push_log, set_if_changed, take_event
from .steps import (
AskVQAFwd,
DispatchAction,
DispatchToolCalls,
HighLevelSubtaskFwd,
InferenceStep,
LowLevelForward,
MemoryUpdateFwd,
UserInterjectionFwd,
)
from .triggers import EventTrigger, HzTrigger, Tick, TickClock, Trigger
from .ui import make_state_panel, print_robot_lines, print_user_line
__all__ = [
# runtime
"LanguageConditionedRuntime",
"PI052PolicyAdapter",
"PI052Runtime",
"RuntimeState",
"StdinReader",
# state helpers
"initial_runtime_state",
"push_log",
"set_if_changed",
"take_event",
# triggers
"Trigger",
"Tick",
"TickClock",
"HzTrigger",
"EventTrigger",
# steps
"InferenceStep",
"LowLevelForward",
"DispatchAction",
"HighLevelSubtaskFwd",
"MemoryUpdateFwd",
"UserInterjectionFwd",
"AskVQAFwd",
"DispatchToolCalls",
# UI
"ToolCall",
"VQAResult",
"make_state_panel",
"print_robot_lines",
"print_user_line",
@@ -0,0 +1,311 @@
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""PI052 adapter for the generic language-conditioned runtime."""
from __future__ import annotations
import logging
import re
from dataclasses import dataclass
from typing import Any
from lerobot.policies.language_conditioned import RuntimeState, ToolCall, VQAResult
logger = logging.getLogger(__name__)
_LOC_TOKENIZER_CACHE: dict[str, Any] = {}
_SAY_RE = re.compile(r"<\s*say\s*>(.*?)<\s*/\s*say\s*>", re.IGNORECASE | re.DOTALL)
@dataclass
class PI052PolicyAdapter:
"""Runtime bridge for PI052 policies."""
policy: Any
def select_action(self, observation: dict[str, Any], state: RuntimeState) -> Any:
subtask = state.language_context.get("subtask") or state.task or ""
text_batch = _build_text_batch(
self.policy,
[{"role": "user", "content": subtask}],
add_generation_prompt=False,
)
from lerobot.utils.constants import OBS_LANGUAGE_ATTENTION_MASK, OBS_LANGUAGE_TOKENS # noqa: PLC0415
batch = dict(observation)
batch[OBS_LANGUAGE_TOKENS] = text_batch["lang_tokens"]
batch[OBS_LANGUAGE_ATTENTION_MASK] = text_batch["lang_masks"]
return self.policy.predict_action_chunk(batch)
def select_text(
self,
kind: str,
observation: dict[str, Any] | None,
state: RuntimeState,
user_text: str | None = None,
) -> str:
messages = self.messages_for(kind, state, user_text=user_text)
return _generate_with_policy(
self.policy,
messages,
observation=observation,
state=state,
label=f"{kind} gen",
min_new_tokens=int(state.extra.get("text_gen_min_new_tokens") or 0),
temperature=float(state.extra.get("text_gen_temperature") or 0.0),
top_p=float(state.extra.get("text_top_p") or 1.0),
suppress_loc_tokens=kind in {"subtask", "memory", "interjection"},
)
def parse_tool_calls(self, text: str) -> list[ToolCall]:
_plan, speech = split_plan_and_say(text)
return [ToolCall("say", {"text": speech})] if speech else []
def plan_from_text(self, text: str) -> str:
plan, _speech = split_plan_and_say(text)
return "" if looks_like_gibberish(plan) else plan
def answer_vqa(
self,
question: str,
camera: str | None,
observation: dict[str, Any] | None,
state: RuntimeState,
) -> VQAResult:
answer = self.select_text("vqa", observation, state, user_text=question)
from .vqa import parse_vqa_answer # noqa: PLC0415
return VQAResult(answer=answer, parsed=parse_vqa_answer(answer), camera=camera)
def update_language_state(self, observation: dict[str, Any] | None, state: RuntimeState) -> None:
chunks_per_gen = max(1, int(state.extra.get("subtask_chunks_per_gen", 1) or 1))
if "_hl_chunks_until_gen" not in state.extra:
state.extra["_hl_chunks_until_gen"] = 0
if state.extra["_hl_chunks_until_gen"] > 0:
state.extra["_hl_chunks_until_gen"] -= 1
return
state.extra["_hl_chunks_until_gen"] = chunks_per_gen - 1
msg = self.select_text("subtask", observation, state)
state.extra["last_subtask_raw"] = msg or ""
if not msg:
empties = int(state.extra.get("subtask_empty_count") or 0) + 1
state.extra["subtask_empty_count"] = empties
if empties == 1 or empties % 5 == 0:
debug = getattr(self.policy, "_last_select_message_debug", "") or ""
state.log(
f" [info] subtask gen empty (x{empties}); {debug}"
if debug
else f" [info] subtask gen returned empty (x{empties})"
)
return
if looks_like_gibberish(msg):
count = int(state.extra.get("subtask_gibberish_count") or 0) + 1
state.extra["subtask_gibberish_count"] = count
if count == 1 or count % 30 == 0:
state.log(f" [info] subtask gen rejected (gibberish x{count}): {msg[:60]!r}")
return
previous = state.language_context.get("subtask")
changed = state.set_context("subtask", msg, label="subtask")
if not changed:
state.extra["subtask_repeat_count"] = int(state.extra.get("subtask_repeat_count") or 0) + 1
return
state.extra["subtask_repeat_count"] = 0
if previous:
state.extra["prior_subtask"] = previous
self._update_memory(observation, state)
def _update_memory(self, observation: dict[str, Any] | None, state: RuntimeState) -> None:
new_memory = self.select_text("memory", observation, state)
state.extra["last_memory_raw"] = new_memory or ""
if not new_memory:
return
if looks_like_gibberish(new_memory):
count = int(state.extra.get("memory_gibberish_count") or 0) + 1
state.extra["memory_gibberish_count"] = count
state.log(f" [info] memory gen rejected (gibberish x{count}): {new_memory[:60]!r}")
return
state.set_context("memory", new_memory, label="memory")
def messages_for(
self,
kind: str,
state: RuntimeState,
*,
user_text: str | None = None,
) -> list[dict[str, Any]]:
if kind == "subtask":
return [{"role": "user", "content": state.task or ""}]
if kind == "memory":
messages = [{"role": "user", "content": state.task or ""}]
if state.language_context.get("memory"):
messages.append(
{
"role": "assistant",
"content": f"Previous memory: {state.language_context['memory']}",
}
)
if state.extra.get("prior_subtask"):
messages.append(
{"role": "user", "content": f"Completed subtask: {state.extra['prior_subtask']}"}
)
return messages
if kind == "interjection":
messages = [{"role": "user", "content": state.task or ""}]
if state.language_context.get("plan"):
messages.append(
{"role": "assistant", "content": f"Previous plan:\n{state.language_context['plan']}"}
)
if user_text:
messages.append({"role": "user", "content": user_text})
return messages
if kind == "plan":
return [{"role": "user", "content": state.task or ""}]
if kind == "vqa":
return [{"role": "user", "content": user_text or ""}]
raise ValueError(f"Unknown PI052 text kind: {kind}")
def _get_loc_tokenizer(tok_name: str, auto_tokenizer_cls: Any, register_loc_fn: Any) -> Any:
tokenizer = _LOC_TOKENIZER_CACHE.get(tok_name)
if tokenizer is None:
tokenizer = register_loc_fn(auto_tokenizer_cls.from_pretrained(tok_name))
_LOC_TOKENIZER_CACHE[tok_name] = tokenizer
return tokenizer
def _build_text_batch(
policy: Any,
prompt_messages: list[dict[str, Any]],
*,
add_generation_prompt: bool = True,
) -> dict[str, Any]:
import torch # noqa: PLC0415
from transformers import AutoTokenizer # noqa: PLC0415
from lerobot.policies.pi052.text_processor_pi052 import ( # noqa: PLC0415
_flatten_say_tool_calls,
_format_messages,
_strip_blocks,
register_paligemma_loc_tokens,
)
tok_name = getattr(policy.config, "tokenizer_name", None) or "google/paligemma-3b-pt-224"
tokenizer = _get_loc_tokenizer(tok_name, AutoTokenizer, register_paligemma_loc_tokens)
messages = [_strip_blocks(_flatten_say_tool_calls(m)) for m in prompt_messages]
prompt, _spans = _format_messages(messages)
if add_generation_prompt:
prompt = prompt + "Assistant: "
encoded = tokenizer(prompt, return_tensors="pt")
ids = encoded["input_ids"]
attn = encoded.get("attention_mask")
if attn is None and tokenizer.pad_token_id is not None:
attn = ids != tokenizer.pad_token_id
if attn is not None and hasattr(attn, "dtype") and attn.dtype != torch.bool:
attn = attn.bool()
device = getattr(getattr(policy, "config", None), "device", None)
if device is not None:
try:
ids = ids.to(device)
if attn is not None and hasattr(attn, "to"):
attn = attn.to(device)
except Exception as exc: # noqa: BLE001
logger.debug("could not move pi052 lang tokens to %s: %s", device, exc)
return {"lang_tokens": ids, "lang_masks": attn, "tokenizer": tokenizer}
def _generate_with_policy(
policy: Any,
messages: list[dict[str, Any]],
*,
observation: dict[str, Any] | None = None,
state: RuntimeState | None = None,
label: str = "select_message",
min_new_tokens: int = 0,
temperature: float = 0.0,
top_p: float = 1.0,
suppress_loc_tokens: bool = False,
) -> str:
if not hasattr(policy, "select_message"):
if state is not None:
state.log(f" [warn] policy has no select_message — skipping {label}")
return ""
text_batch = _build_text_batch(policy, messages)
try:
from lerobot.utils.constants import OBS_LANGUAGE_ATTENTION_MASK, OBS_LANGUAGE_TOKENS # noqa: PLC0415
batch: dict[str, Any] = {
OBS_LANGUAGE_TOKENS: text_batch["lang_tokens"],
OBS_LANGUAGE_ATTENTION_MASK: text_batch["lang_masks"],
}
if observation:
for k, v in observation.items():
if isinstance(k, str) and k.startswith("observation.") and k not in batch:
batch[k] = v
return policy.select_message(
batch,
tokenizer=text_batch["tokenizer"],
min_new_tokens=min_new_tokens,
temperature=temperature,
top_p=top_p,
suppress_loc_tokens=suppress_loc_tokens,
)
except Exception as exc: # noqa: BLE001
logger.warning("%s failed: %s", label, exc, exc_info=logger.isEnabledFor(logging.DEBUG))
if state is not None:
state.log(f" [warn] {label} failed: {type(exc).__name__}: {exc}")
return ""
def looks_like_gibberish(text: str) -> bool:
if not text or not text.strip():
return True
stripped = text.strip()
alpha = sum(1 for c in stripped if c.isalpha())
if alpha < max(3, len(stripped) // 8):
return True
if stripped.startswith('":') and stripped.count('"') > stripped.count(" "):
return True
if len(set(stripped)) <= 2 and len(stripped) > 4:
return True
cleaned = stripped.replace("\n", " ").replace(":", " ")
for marker in ("Assistant", "User", "Ass "):
if marker in cleaned and len(cleaned.split()) < 4:
return True
tokens = [t for t in cleaned.split() if any(c.isalpha() for c in t)]
unique_alpha = {t.lower() for t in tokens}
if len(unique_alpha) < 3 and len(stripped) < 80:
return True
return len(tokens) >= 8 and len(unique_alpha) <= max(3, len(tokens) // 10)
def split_plan_and_say(text: str) -> tuple[str, str]:
if not text:
return "", ""
match = _SAY_RE.search(text)
if not match:
return text.strip(), ""
speech = match.group(1).strip().strip('"').strip("'")
plan = (text[: match.start()] + text[match.end() :]).strip()
return plan, speech
def messages_for_vqa(question: str) -> list[dict[str, Any]]:
return [{"role": "user", "content": question}]
+9 -2
View File
@@ -99,7 +99,14 @@ class StdinReader:
# Question → VQA; statement → interjection.
if lower.endswith("?"):
state["recent_vqa_query"] = line
state.setdefault("events_this_tick", []).append("user_vqa_query")
_emit(state, "user_vqa_query")
else:
state["recent_interjection"] = line
state.setdefault("events_this_tick", []).append("user_interjection")
_emit(state, "user_interjection")
def _emit(state: Any, event_name: str) -> None:
if hasattr(state, "emit"):
state.emit(event_name)
else:
state.setdefault("events_this_tick", []).append(event_name)
+47 -180
View File
@@ -11,195 +11,62 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""PI052 runtime loop.
Threads the multi-rate inference pipeline together with a stdin REPL
event collector, drives ticks through :class:`TickClock`, and prints
state-change updates to the user.
"""
"""PI052 compatibility wrapper for the generic language-conditioned runtime."""
from __future__ import annotations
import logging
from collections import deque
from dataclasses import dataclass, field
from typing import Any, Callable
from collections.abc import Callable
from typing import Any
from .runtime_state import initial_runtime_state, push_log
from .steps import (
AskVQAFwd,
DispatchAction,
DispatchToolCalls,
HighLevelSubtaskFwd,
InferenceStep,
LowLevelForward,
MemoryUpdateFwd,
from lerobot.policies.language_conditioned import (
LanguageConditionedRuntime,
RuntimeState,
Tick,
TickClock,
ToolCall,
VQAResult,
)
from .triggers import EventTrigger, HzTrigger, TickClock
logger = logging.getLogger(__name__)
from .pi052_adapter import PI052PolicyAdapter
@dataclass
class PI052Runtime:
"""Compose the inference pipeline and drive it tick-by-tick."""
class PI052Runtime(LanguageConditionedRuntime):
"""Backwards-compatible PI052 runtime constructor."""
policy: Any
tools: dict[str, Any] = field(default_factory=dict)
"""Name → tool-instance dict, e.g. ``{"say": SayTool(...)}``. Read
from :func:`lerobot.tools.get_tools(meta)` when wiring the
runtime."""
observation_provider: Callable[[], dict | None] | None = None
"""Closure returning the current preprocessed observation batch.
``None`` for dry-run / language-only sessions."""
robot_executor: Callable[[Any], None] | None = None
"""Closure that takes one action chunk and forwards it to the
robot. ``None`` for dry-run."""
event_collector: Callable[[dict], None] | None = None
"""Per-tick hook that polls external sources (stdin, network) and
appends event names to ``state["events_this_tick"]``."""
chunk_hz: float = 4.0
ctrl_hz: float = 50.0
high_level_hz: float = 1.0
max_rate_hz: float = 50.0
def __init__(
self,
policy: Any,
*,
tools: dict[str, Any] | None = None,
observation_provider: Callable[[], dict | None] | None = None,
robot_executor: Callable[[Any], None] | None = None,
event_collector: Callable[[RuntimeState], None] | None = None,
chunk_hz: float = 4.0,
ctrl_hz: float = 50.0,
high_level_hz: float = 1.0,
max_rate_hz: float = 50.0,
) -> None:
super().__init__(
policy_adapter=policy if isinstance(policy, PI052PolicyAdapter) else PI052PolicyAdapter(policy),
observation_provider=observation_provider,
action_executor=robot_executor,
tools=tools or {},
event_collector=event_collector,
chunk_hz=chunk_hz,
ctrl_hz=ctrl_hz,
high_level_hz=high_level_hz,
max_rate_hz=max_rate_hz,
)
pipeline: list[InferenceStep] = field(init=False)
state: dict[str, Any] = field(init=False)
_stop: bool = field(default=False, init=False)
def __post_init__(self) -> None:
# Subtask + memory + VQA configuration. Pipeline:
#
# HighLevelSubtaskFwd → generate the next subtask via the LM
# head at ~``high_level_hz``; writes
# ``current_subtask`` and emits
# ``subtask_change`` on a transition.
# MemoryUpdateFwd → on ``subtask_change``, refresh
# ``current_memory`` from the
# ``memory_update`` head.
# AskVQAFwd → answer camera-grounded stdin questions.
# LowLevelForward → action chunk conditioned on the
# generated ``current_subtask``.
# DispatchAction → drain the chunk to the robot.
# DispatchToolCalls → fire any pending tool calls.
#
# Order matters: ``HighLevelSubtaskFwd`` must run before
# ``MemoryUpdateFwd`` so the event is visible the same tick, and
# both must run before ``LowLevelForward`` (which is gated on
# "action queue empty") so the chunk consumes the freshest
# subtask. ``UserInterjectionFwd`` is still importable but
# disabled until plan generation is wired in.
self.pipeline = [
HighLevelSubtaskFwd(
trigger=HzTrigger(self.high_level_hz),
policy=self.policy,
observation_provider=self.observation_provider,
),
# Listens for the ``subtask_change`` event raised by
# ``HighLevelSubtaskFwd`` and refreshes ``current_memory``.
MemoryUpdateFwd(
trigger=EventTrigger("subtask_change"),
policy=self.policy,
observation_provider=self.observation_provider,
),
AskVQAFwd(
policy=self.policy,
observation_provider=self.observation_provider,
),
LowLevelForward(
trigger=HzTrigger(self.chunk_hz),
policy=self.policy,
observation_provider=self.observation_provider,
),
DispatchAction(
trigger=HzTrigger(self.ctrl_hz),
robot_executor=self.robot_executor,
),
DispatchToolCalls(tools=self.tools),
]
self.state = initial_runtime_state()
# ------------------------------------------------------------------
# Lifecycle
# ------------------------------------------------------------------
def set_task(self, task: str) -> None:
"""Set or replace the active task. Logged for the REPL."""
self.state["task"] = task
push_log(self.state, f"Task: {task}")
def stop(self) -> None:
self._stop = True
def run(self, *, max_ticks: int | None = None) -> None:
"""Main loop. Returns when ``stop()`` is called or after
``max_ticks`` ticks (useful for tests / dry-run)."""
clock = TickClock(max_rate_hz=self.max_rate_hz)
while not self._stop:
tick = clock.advance()
self.state["_tick"] = tick
self.state["events_this_tick"] = []
self.state["log_lines"] = []
if self.event_collector is not None:
self.event_collector(self.state)
if self.state.get("stop"):
self._stop = True
break
for step in self.pipeline:
self.state = step(self.state)
self._flush_logs()
if max_ticks is not None and tick.index >= max_ticks:
break
self._on_shutdown()
# ------------------------------------------------------------------
# REPL helper: drive one full pipeline pass and return its logs
# ------------------------------------------------------------------
def step_once(self) -> list[str]:
"""Run one tick of the pipeline and return the log lines.
Used by the interactive REPL: instead of a background thread,
the CLI drives ticks synchronously after each user input. Logs
are returned (not printed) so the caller can route them into
the rich-Live chat scrollback.
"""
from .triggers import Tick # noqa: PLC0415
# Synthesize a tick. We don't need the real wall-clock pacing
# here — the REPL drives the runtime, not vice versa — but
# ``HzTrigger`` uses ``tick.monotonic_seconds`` to gate, so we
# bump it generously so every Hz-triggered step considers
# itself due.
import time as _time # noqa: PLC0415
prev_index = self.state.get("_tick").index if isinstance(self.state.get("_tick"), Tick) else 0
self.state["_tick"] = Tick(index=prev_index + 1, monotonic_seconds=_time.monotonic())
self.state["log_lines"] = []
# ``events_this_tick`` is set up by the caller before
# ``step_once`` (the REPL pushes user-driven events first).
self.state.setdefault("events_this_tick", [])
for step in self.pipeline:
self.state = step(self.state)
return list(self.state.get("log_lines") or [])
# ------------------------------------------------------------------
# I/O
# ------------------------------------------------------------------
def _flush_logs(self) -> None:
for line in self.state.get("log_lines") or []:
print(f"[pi052] {line}", flush=True)
def _on_shutdown(self) -> None:
# Drain any queued action chunks safely.
queue = self.state.get("action_queue")
if isinstance(queue, deque):
queue.clear()
print("[pi052] runtime stopped", flush=True)
__all__ = [
"LanguageConditionedRuntime",
"PI052PolicyAdapter",
"PI052Runtime",
"RuntimeState",
"Tick",
"TickClock",
"ToolCall",
"VQAResult",
]
File diff suppressed because it is too large Load Diff
@@ -1,95 +0,0 @@
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Runtime state passed between inference steps each tick.
The runtime threads a single dict through the pipeline; this module
documents the shape and provides factories. We use a plain ``dict``
rather than a frozen dataclass because steps freely add and remove
keys (``events_this_tick``, ``messages_pending``, ``tool_calls_pending``,
…) and dataclass field churn would just get in the way.
Stable keys (read by multiple steps):
task str the current top-level task
current_plan str | None latest plan emitted by the planner
current_subtask str | None latest subtask the policy is executing
current_memory str | None latest compressed memory
recent_interjection str | None most recent user interjection text (consumed)
action_queue collections.deque[Tensor] pending action chunks
tool_calls_pending list[dict] parsed but not-yet-dispatched tool calls
events_this_tick list[str] triggers consumed this tick
_tick Tick current tick (set by the loop)
mode str "action" (run the robot) | "paused"
(action loop stopped — robot holds)
log_lines list[str] human-readable status lines printed each tick
"""
from __future__ import annotations
from collections import deque
from typing import Any
def initial_runtime_state(task: str | None = None) -> dict[str, Any]:
"""Build a fresh runtime state dict with sensible defaults."""
return {
"task": task,
"current_plan": None,
"current_subtask": None,
"current_memory": None,
"recent_interjection": None,
"action_queue": deque(),
"tool_calls_pending": [],
"events_this_tick": [],
"log_lines": [],
"mode": "action",
"stop": False,
}
def take_event(state: dict[str, Any], event_name: str) -> bool:
"""Pop ``event_name`` from ``events_this_tick`` if present.
Steps that consume an event call this so the same event doesn't
re-fire on a sibling step within the same tick.
"""
events: list[str] = state.get("events_this_tick") or []
if event_name in events:
events.remove(event_name)
return True
return False
def push_log(state: dict[str, Any], line: str) -> None:
"""Append ``line`` to the per-tick log buffer; the runtime prints
it at the end of the tick."""
state.setdefault("log_lines", []).append(line)
def set_if_changed(state: dict[str, Any], key: str, value: Any, label: str | None = None) -> bool:
"""Update ``state[key]`` and log a diff line if the value changed.
Returns ``True`` if the value actually changed.
"""
prev = state.get(key)
if prev == value:
return False
state[key] = value
if label is not None:
push_log(state, f" {label}: {value}")
return True
+15 -926
View File
@@ -11,930 +11,19 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Inference steps for the PI052 multi-rate runtime.
Each step is a tiny class with a ``trigger`` and an ``__call__(state)``;
the runtime applies them in order each tick. When a step's trigger
doesn't fire, the step is a no-op and the runtime moves on.
Stream-to-step mapping mirrors the PI052 training recipe:
* ``LowLevelForward`` — calls ``policy.select_action`` for the
action chunk; trained by
``low_level_execution``
* ``EnqueueChunk`` — pushes the chunk to ``action_queue``
* ``DispatchAction`` — pops one action per control tick and
forwards to the robot
* ``HighLevelSubtaskFwd`` — calls ``policy.select_message`` for the
next subtask; trained by
``high_level_subtask``
* ``MemoryUpdateFwd`` — fires on subtask boundary; trained by
``memory_update``
* ``UserInterjectionFwd`` — fires on stdin interjection; trained by
``user_interjection_response``
* ``AskVQAFwd`` — fires on stdin question; trained by
``ask_vqa_*``
* ``DispatchToolCalls`` — pops ``tool_calls_pending`` and calls
the matching ``Tool`` instance
"""
from __future__ import annotations
import logging
import re
from dataclasses import dataclass, field
from typing import Any
from .runtime_state import push_log, set_if_changed, take_event
from .triggers import EventTrigger, HzTrigger, Trigger
logger = logging.getLogger(__name__)
# ---------------------------------------------------------------------------
# Step base + runner
# ---------------------------------------------------------------------------
@dataclass
class InferenceStep:
"""A trigger-gated callable. Subclasses override :meth:`run`."""
trigger: Trigger
def __call__(self, state: dict[str, Any]) -> dict[str, Any]:
if not self.trigger.should_fire(state["_tick"], state):
return state
return self.run(state) or state
def run(self, state: dict[str, Any]) -> dict[str, Any] | None: # pragma: no cover
raise NotImplementedError
# ---------------------------------------------------------------------------
# Low-level (action) path
# ---------------------------------------------------------------------------
@dataclass
class LowLevelForward(InferenceStep):
"""Run the policy's action head and produce one action chunk."""
policy: Any = None
observation_provider: Any = None
"""Callable ``() -> dict``: returns the current observation batch
(already preprocessed). Typically wraps the robot's camera /
proprio reads. ``None`` in dry-run mode → step skips."""
trigger: Trigger = field(default_factory=lambda: HzTrigger(hz=4.0))
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
if self.policy is None or self.observation_provider is None:
return None
# ``/vlm`` mode pauses the whole action loop so the robot holds
# position while the operator probes the VLM with VQA.
if state.get("mode", "action") != "action":
return None
if not state.get("task"):
return None
# PI052 produces *action chunks* (typically 50 steps via
# flow-matching). Every step gets dispatched to the robot;
# popping one per dispatch tick is essentially free. Only
# generate a new chunk once the previous one has fully
# drained — this is the canonical "sense → think → act"
# loop. Refreshing while a chunk is still queued causes the
# new chunk to "telescope" past the old one (planned from an
# observation that's already 25+ steps stale by the time it
# starts dispatching).
queue = state.setdefault("action_queue", [])
if len(queue) > 0:
return None
observation = self.observation_provider()
if observation is None:
return None
# The action expert is conditioned on the SUBTASK generated by
# the high-level loop (``HighLevelSubtaskFwd`` runs earlier in
# the pipeline and writes ``current_subtask``). Matches the
# training-time ``low_level_execution`` recipe — ``user(${subtask})``.
# Falls back to the task string only on the very first frame,
# before the high-level loop has produced a subtask.
subtask = state.get("current_subtask") or state.get("task") or ""
ctx = [{"role": "user", "content": subtask}]
# ``add_generation_prompt=False`` to match the training-time
# prefix shape: at training the action expert sees the rendered
# user turn ending at ``<|im_end|>`` (no trailing
# ``<|im_start|>assistant\n``). Passing True here would append
# extra role-marker tokens the action expert never saw during
# training.
text_batch = _build_text_batch(self.policy, ctx, add_generation_prompt=False)
from lerobot.utils.constants import ( # noqa: PLC0415
OBS_LANGUAGE_ATTENTION_MASK,
OBS_LANGUAGE_TOKENS,
)
observation = dict(observation)
observation[OBS_LANGUAGE_TOKENS] = text_batch["lang_tokens"]
observation[OBS_LANGUAGE_ATTENTION_MASK] = text_batch["lang_masks"]
try:
# ``predict_action_chunk`` returns the *full* chunk shape
# ``(batch, n_action_steps, action_dim)``. Enqueue every
# step so DispatchAction at ctrl_hz can drain them
# smoothly until the next refresh.
chunk = self.policy.predict_action_chunk(observation)
except Exception as exc: # noqa: BLE001
logger.warning(
"predict_action_chunk failed: %s",
exc,
exc_info=logger.isEnabledFor(logging.DEBUG),
)
push_log(
state,
f" [warn] predict_action_chunk failed: {type(exc).__name__}: {exc}",
)
return None
# ``chunk`` shape: ``(batch, n_action_steps, action_dim)``. Push
# each step as a ``(1, action_dim)`` tensor so the existing
# action executor's batch-squeeze logic works unchanged.
if chunk.ndim == 3:
chunk_iter = chunk[0] # ``(n_action_steps, action_dim)``
elif chunk.ndim == 2:
chunk_iter = chunk
else:
chunk_iter = chunk.unsqueeze(0)
for step in chunk_iter:
queue.append(step.unsqueeze(0))
state["last_chunk_size"] = int(chunk_iter.shape[0])
return None
@dataclass
class DispatchAction(InferenceStep):
"""Pop one action per tick and hand it to the robot.
In dry-run mode (``robot_executor=None``) the step still pops the
queue so it doesn't grow unbounded — the popped tensor is logged
instead of executed.
Wall-clock catch-up: the action queue represents an open-loop
trajectory at a fixed step rate (``trigger.hz`` ≈ ``ctrl_hz``).
When the main loop stalls — e.g. an LLM call for the high-level
subtask blocks for ~2 s on MPS — the dispatch trigger fires only
once over that whole interval. Naively popping a single entry per
fire makes the robot lag further and further behind the planned
timeline, and a 50-step chunk would take ~125 s to drain instead
of ~1.7 s. Track real elapsed time between dispatches and pop
``round(elapsed * hz)`` entries, sending the most recent one. The
skipped intermediate joint targets are stale anyway — the dynamixel
will smooth toward the latest goal position.
"""
robot_executor: Any = None
trigger: Trigger = field(default_factory=lambda: HzTrigger(hz=50.0))
_last_dispatch_t: float | None = field(default=None, init=False)
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
import time as _time # noqa: PLC0415
# ``/vlm`` mode pauses dispatch — the robot holds its last
# commanded position while the operator runs VQA.
if state.get("mode", "action") != "action":
self._last_dispatch_t = None
return None
queue = state.get("action_queue")
if not queue:
# Reset wall-clock anchor when the queue is empty so the
# next chunk doesn't see a huge fake "elapsed" window.
self._last_dispatch_t = None
return None
now = _time.monotonic()
hz = getattr(self.trigger, "hz", 30.0)
if self._last_dispatch_t is None or hz <= 0:
n_to_pop = 1
else:
elapsed = now - self._last_dispatch_t
# ``max(1, ...)`` so we always pop at least one when the
# trigger fires; ``min(len(queue), ...)`` so we don't run
# off the end of the chunk.
n_to_pop = max(1, min(len(queue), int(round(elapsed * hz))))
self._last_dispatch_t = now
# Drain ``n_to_pop`` stale entries, keep only the latest as the
# action actually sent. The intermediate joint targets would
# all be ~1030 ms apart in chunk time — the robot can't track
# them individually anyway when the host loop is slow.
latest = None
for _ in range(n_to_pop):
if not queue:
break
latest = queue.popleft() if hasattr(queue, "popleft") else queue.pop(0)
state["actions_dispatched"] = state.get("actions_dispatched", 0) + 1
if latest is not None and self.robot_executor is not None:
self.robot_executor(latest)
return None
# ---------------------------------------------------------------------------
# High-level (text) paths — all use policy.select_message
# ---------------------------------------------------------------------------
_LOC_TOKENIZER_CACHE: dict[str, Any] = {}
def _get_loc_tokenizer(tok_name: str, auto_tokenizer_cls: Any, register_loc_fn: Any) -> Any:
"""Return a loc-token-registered tokenizer, loading from disk only once.
``AutoTokenizer.from_pretrained`` + loc-token registration is expensive and
the result is immutable, so cache per ``tok_name``.
"""
tokenizer = _LOC_TOKENIZER_CACHE.get(tok_name)
if tokenizer is None:
tokenizer = register_loc_fn(auto_tokenizer_cls.from_pretrained(tok_name))
_LOC_TOKENIZER_CACHE[tok_name] = tokenizer
return tokenizer
def _build_text_batch(
policy: Any,
prompt_messages: list[dict[str, Any]],
*,
add_generation_prompt: bool = True,
) -> dict[str, Any]:
"""Tokenize chat messages into the batch ``select_message`` expects.
PI052's backbone (PaliGemma) ships no chat template, so we train on
a plain role-prefixed concatenation built by
``PI052TextTokenizerStep``. We reuse that exact formatter so the
inference prefix matches training; ``add_generation_prompt`` appends
the bare ``Assistant: `` header the LM head continues from.
"""
import torch # noqa: PLC0415
from transformers import AutoTokenizer # noqa: PLC0415
from lerobot.policies.pi052.text_processor_pi052 import ( # noqa: PLC0415
_flatten_say_tool_calls,
_format_messages,
_strip_blocks,
register_paligemma_loc_tokens,
)
tok_name = getattr(policy.config, "tokenizer_name", None) or "google/paligemma-3b-pt-224"
# Register PaliGemma's <locDDDD> tokens so inference encoding /
# decoding sees them as single vocab ids — must match training.
# The tokenizer is read-only after registration, so cache it: rebuilding it
# from disk on every call dominated eval runtime (this runs twice per env
# per replan — subtask gen + action prompt).
tokenizer = _get_loc_tokenizer(tok_name, AutoTokenizer, register_paligemma_loc_tokens)
messages = [_strip_blocks(_flatten_say_tool_calls(m)) for m in prompt_messages]
prompt, _spans = _format_messages(messages)
if add_generation_prompt:
prompt = prompt + "Assistant: "
encoded = tokenizer(prompt, return_tensors="pt")
ids = encoded["input_ids"]
attn = encoded.get("attention_mask")
if attn is None and tokenizer.pad_token_id is not None:
attn = ids != tokenizer.pad_token_id
if attn is not None and hasattr(attn, "dtype") and attn.dtype != torch.bool:
attn = attn.bool()
# Move tokens onto the policy's device — otherwise prefix embedding
# raises a device-mismatch on every forward (CPU tensor vs MPS / CUDA
# model), which the caller's broad except would swallow silently.
device = getattr(getattr(policy, "config", None), "device", None)
if device is not None:
try:
ids = ids.to(device)
if attn is not None and hasattr(attn, "to"):
attn = attn.to(device)
except Exception as exc: # noqa: BLE001
logger.debug("could not move pi052 lang tokens to %s: %s", device, exc)
return {"lang_tokens": ids, "lang_masks": attn, "tokenizer": tokenizer}
def _strip_recipe_keys(m: dict[str, Any]) -> dict[str, Any]:
new = dict(m)
new.pop("stream", None)
new.pop("target", None)
return new
@dataclass
class HighLevelSubtaskFwd(InferenceStep):
"""At ~1 Hz, ask the policy for the next subtask.
Mirrors the ``high_level_subtask`` recipe layout exactly:
user: "${task}\\nPlan: ${plan}\\nMemory: ${memory}"
user: "Current subtask: ${subtask}" (if subtask present)
↓ generate ↓
assistant: <next subtask>
"""
policy: Any = None
observation_provider: Any = None
"""Same shape as ``LowLevelForward.observation_provider``. When
set, the resulting observation is merged into ``select_message``'s
batch so text generation runs against real video + state."""
trigger: Trigger = field(default_factory=lambda: HzTrigger(hz=1.0))
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
if self.policy is None or not state.get("task"):
return None
# ``/vlm`` mode pauses subtask generation along with the rest of
# the action loop.
if state.get("mode", "action") != "action":
return None
# Gate to chunk boundaries: only generate a fresh subtask when
# the action queue is empty (i.e. right before LowLevelForward
# refreshes the chunk). ``select_message`` takes ~2 s on MPS,
# and running it every loop iteration starves DispatchAction
# at ctrl_hz=30 — the queue drains at ~0.4 actions/sec instead
# of 30/sec and the robot barely moves. Tying it to the same
# "queue empty" condition as the chunk refresh produces a
# clean sense → think → act cycle.
#
# Rearm the trigger when skipping so a low-hz schedule
# (e.g. ``--high_level_hz=0.2`` = once per 5 s) doesn't lose
# the slot: the trigger fires once on the timer but the brief
# queue-empty window almost never coincides, so without rearm
# HL would effectively never run.
queue = state.get("action_queue") or []
if len(queue) > 0:
if hasattr(self.trigger, "rearm"):
self.trigger.rearm()
return None
# Per-chunk-boundary throttle: at each "queue empty" moment we
# increment a counter; subtask gen only fires once the counter
# reaches ``subtask_chunks_per_gen``. Lets the operator run e.g.
# 5 action chunks per subtask-gen so the LM head doesn't churn
# every 1.7 s (a fresh subtask while the previous one is still
# being executed is wasted compute *and* causes the action
# expert's flow trajectory to be re-planned mid-grasp).
chunks_per_gen = max(1, int(state.get("subtask_chunks_per_gen", 1) or 1))
# Initialise so the first chunk boundary fires immediately
# (counter starts at chunks_per_gen, decrements per skip,
# generates and resets when it hits 0).
if "_hl_chunks_until_gen" not in state:
state["_hl_chunks_until_gen"] = 0
if state["_hl_chunks_until_gen"] > 0:
state["_hl_chunks_until_gen"] -= 1
if hasattr(self.trigger, "rearm"):
self.trigger.rearm()
return None
state["_hl_chunks_until_gen"] = chunks_per_gen - 1
ctx = _msgs_for_subtask(state)
observation = _maybe_observation(self.observation_provider)
# Default: greedy argmax, no min_new_tokens, no special-token
# suppression — matches training. Operator can override via
# ``--text_min_new_tokens=N --text_temperature=T --text_top_p=P``
# on the CLI; useful for under-trained checkpoints whose LM
# head still favours EOS at position 0 (pre-trained chat
# backbone's short-turn prior hasn't been fully overridden
# by the fine-tuning supervision yet).
msg = _generate_with_policy(
self.policy,
ctx,
observation=observation,
state=state,
label="subtask gen",
min_new_tokens=int(state.get("text_gen_min_new_tokens") or 0),
temperature=float(state.get("text_gen_temperature") or 0.0),
top_p=float(state.get("text_gen_top_p") or 1.0),
# Subtasks never legitimately contain PaliGemma ``<loc>``
# tokens — suppress them so a checkpoint whose LM head
# has drifted toward the pretrained loc-prior falls back
# to its (still-correct) text mass.
suppress_loc_tokens=True,
)
# Diagnostics: surface what the model is *actually* producing
# at chunk boundaries, even when the output gets rejected or
# repeats. Memorisation collapse looks like "same accepted
# subtask N times in a row" or "gibberish_count rising while
# current_subtask is stuck". The state panel renders these.
state["last_subtask_raw"] = msg or ""
# Persistent empty completion is its own failure mode (model
# immediately EOS-es from the chat-template generation
# prompt) — surface it once every N occurrences so the
# operator can distinguish "generation failing silently"
# from "generating fine but filter rejecting".
if not msg:
empties = state.get("subtask_empty_count", 0) + 1
state["subtask_empty_count"] = empties
if empties == 1 or empties % 5 == 0:
debug = getattr(self.policy, "_last_select_message_debug", "") or ""
if debug:
push_log(
state,
f" [info] subtask gen empty (×{empties}); {debug}",
)
else:
push_log(
state,
f" [info] subtask gen returned empty (×{empties}) — "
"no tokens generated (head EOS-ing before any "
"non-special token).",
)
if msg and _looks_like_gibberish(msg):
# Bump a counter so the operator can see the model is
# struggling without spamming the log every tick. A first
# rejection still logs once so the failure is visible.
count = state.get("subtask_gibberish_count", 0) + 1
state["subtask_gibberish_count"] = count
if count == 1 or count % 30 == 0:
push_log(
state,
f" [info] subtask gen rejected (gibberish ×{count}): {msg[:60]!r}",
)
return None
if msg:
prev_subtask = state.get("current_subtask")
changed = set_if_changed(state, "current_subtask", msg, label="subtask")
if changed:
# Stash the just-completed subtask so ``MemoryUpdateFwd``
# can drop it into its prompt as ``Completed subtask:``
# — the recipe binds ``completed_subtask`` to
# ``nth_prev(style=subtask, offset=1)``, i.e. the subtask
# that was active *before* the change.
if prev_subtask:
state["prior_subtask"] = prev_subtask
# Subtask change is a downstream trigger.
state.setdefault("events_this_tick", []).append("subtask_change")
state["subtask_repeat_count"] = 0
else:
# Same accepted string regenerated — memorisation tell.
# Once this counter climbs past a few, you're seeing
# the model unable to move past the current subtask
# despite the chunk having drained (visual scene may
# have changed but the LM is replaying training
# tokens).
state["subtask_repeat_count"] = state.get("subtask_repeat_count", 0) + 1
# Silently skip empty completions — common when the model
# warms up or generates only EOS; logging it every tick at
# ctrl_hz is just noise.
return None
@dataclass
class MemoryUpdateFwd(InferenceStep):
"""On subtask boundary, refresh the compressed memory.
Mirrors the ``memory_update`` recipe layout exactly:
user: "${task}"
assistant: "Previous memory: ${prior_memory}" (if prior memory)
user: "Completed subtask: ${completed_subtask}" (if subtask)
↓ generate ↓
assistant: <new memory>
"""
policy: Any = None
observation_provider: Any = None
trigger: Trigger = field(default_factory=lambda: EventTrigger("subtask_change"))
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
# Don't consume the event — multiple steps may want to react.
if self.policy is None:
return None
ctx = _msgs_for_memory(state)
observation = _maybe_observation(self.observation_provider)
new_memory = _generate_with_policy(
self.policy,
ctx,
observation=observation,
state=state,
label="memory gen",
suppress_loc_tokens=True,
)
state["last_memory_raw"] = new_memory or ""
if new_memory and _looks_like_gibberish(new_memory):
count = state.get("memory_gibberish_count", 0) + 1
state["memory_gibberish_count"] = count
push_log(
state,
f" [info] memory gen rejected (gibberish ×{count}): {new_memory[:60]!r}",
)
return None
if new_memory:
set_if_changed(state, "current_memory", new_memory, label="memory")
return None
@dataclass
class UserInterjectionFwd(InferenceStep):
"""On stdin interjection, refresh the plan + emit a paired ``say``.
Mirrors the ``user_interjection_response`` recipe layout exactly:
user: "${task}"
assistant: "Previous plan:\\n${prior_plan}" (if prior plan)
user: "${interjection}" (the new utterance)
↓ generate ↓
assistant: <plan + <say>...</say>>
"""
policy: Any = None
observation_provider: Any = None
trigger: Trigger = field(default_factory=lambda: EventTrigger("user_interjection"))
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
if self.policy is None or not take_event(state, "user_interjection"):
return None
ctx = _msgs_for_interjection(state)
observation = _maybe_observation(self.observation_provider)
out = _generate_with_policy(
self.policy,
ctx,
observation=observation,
state=state,
label="plan/say gen",
suppress_loc_tokens=True,
)
if not out:
# Don't log every empty completion — happens repeatedly on
# MPS during warm-up and floods the panel. The user can
# re-trigger by typing again.
return None
if _looks_like_gibberish(out):
count = state.get("plan_gibberish_count", 0) + 1
state["plan_gibberish_count"] = count
push_log(
state,
f" [info] plan/say gen rejected (gibberish ×{count}): {out[:60]!r}",
)
return None
# Heuristic split: model is trained to emit one assistant turn
# carrying both plan text AND a `say` tool call. Look for a
# "<say>...</say>" or "say(...)" marker; fall back to whole
# text → plan, no speech.
plan_text, speech_text = _split_plan_and_say(out)
if plan_text and _looks_like_gibberish(plan_text):
plan_text = ""
if plan_text:
set_if_changed(state, "current_plan", plan_text, label="plan")
if speech_text:
push_log(state, f" speech: {speech_text}")
state.setdefault("tool_calls_pending", []).append(
{
"type": "function",
"function": {"name": "say", "arguments": {"text": speech_text}},
}
)
state.setdefault("events_this_tick", []).append("tool_call_pending")
# Mark interjection consumed.
state["recent_interjection"] = None
return None
@dataclass
class AskVQAFwd(InferenceStep):
"""On stdin question, answer a frame-grounded VQA.
Mirrors the ``ask_vqa_*`` recipe layout exactly: a single user
turn carrying just the VQA question, plus the camera image block
in training (we drop the image at inference because the dataset's
image preprocessing doesn't match SmolVLM's vision tower input).
user: <question>
↓ generate ↓
assistant: <vqa answer>
"""
policy: Any = None
observation_provider: Any = None
trigger: Trigger = field(default_factory=lambda: EventTrigger("user_vqa_query"))
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
if self.policy is None or not take_event(state, "user_vqa_query"):
return None
question = state.get("recent_vqa_query")
if not question:
return None
ctx = _msgs_for_vqa(question)
observation = _maybe_observation(self.observation_provider)
answer = _generate_with_policy(
self.policy,
ctx,
observation=observation,
state=state,
label="vqa gen",
)
# VQA answers are intentionally JSON-like during training, so
# ``_looks_like_gibberish`` would false-positive on them. Keep
# the answer as-is — the VQA panel line lets the user judge.
if answer:
push_log(state, f" vqa: {answer}")
state["recent_vqa_query"] = None
return None
# ---------------------------------------------------------------------------
# Tool dispatch
# ---------------------------------------------------------------------------
@dataclass
class DispatchToolCalls(InferenceStep):
"""Pop ``tool_calls_pending`` and execute them via :data:`TOOL_REGISTRY`."""
tools: dict[str, Any] = field(default_factory=dict)
trigger: Trigger = field(default_factory=lambda: EventTrigger("tool_call_pending"))
def run(self, state: dict[str, Any]) -> dict[str, Any] | None:
take_event(state, "tool_call_pending")
pending = state.get("tool_calls_pending") or []
for call in pending:
try:
fn = (call or {}).get("function") or {}
name = fn.get("name")
args = fn.get("arguments") or {}
tool = self.tools.get(name)
if tool is None:
push_log(state, f" [warn] tool {name!r} not registered — skipping call")
continue
tool.call(args)
except Exception as exc: # noqa: BLE001
push_log(state, f" [error] tool dispatch failed: {exc}")
state["tool_calls_pending"] = []
return None
# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------
def _looks_like_gibberish(text: str) -> bool:
"""Heuristically detect generation that's clearly off the rails.
Memorised models can collapse to dominant-mode outputs when the
prompt drifts even slightly from training distribution. Reject:
* empty / whitespace-only
* too few alphabetic characters (mostly punctuation)
* a single character repeated past the threshold
* starts with ``":"`` and contains no letters
* too few unique tokens — e.g. ``"the"``, ``"the the the"``,
``"Ass\\n::\\nthe"`` (the collapse seen on real-robot frames
where the model emits one or two memorised tokens repeatedly)
* chat-template fragment leakage (``Assistant:``, ``User:``,
``Ass\\n``)
Real subtasks look like ``"close the gripper to grasp the blue
cube"`` — multiple unique alphabetic tokens, no role-marker
fragments. Anything materially shorter than that is rejected.
"""
if not text or not text.strip():
return True
stripped = text.strip()
alpha = sum(1 for c in stripped if c.isalpha())
if alpha < max(3, len(stripped) // 8):
return True
if stripped.startswith('":') and stripped.count('"') > stripped.count(" "):
return True
# Single repeating char: e.g. ``""""""``.
if len(set(stripped)) <= 2 and len(stripped) > 4:
return True
# Chat-template fragment leakage — the model emits ``Ass``,
# ``Assistant:``, ``User:``, often with extra newlines/colons.
# Reject if the cleaned text is mostly role-marker shards.
cleaned = stripped.replace("\n", " ").replace(":", " ")
for marker in ("Assistant", "User", "Ass "):
if marker in cleaned and len(cleaned.split()) < 4:
return True
tokens = [t for t in cleaned.split() if any(c.isalpha() for c in t)]
unique_alpha = {t.lower() for t in tokens}
# Short degenerate output — model stuck on ``the`` or a couple of
# memorised single-token continuations.
if len(unique_alpha) < 3 and len(stripped) < 80:
return True
# Long repetition collapse — the LM head loops an n-gram for the
# whole generation budget ("the arm the arm … the the the the").
# Length-independent: many tokens but a tiny unique ratio. The
# earlier ``< 80`` check missed these because the looped string
# blows well past 80 chars.
return len(tokens) >= 8 and len(unique_alpha) <= max(3, len(tokens) // 10)
def _control_context_messages(
state: dict[str, Any],
*,
include_completed: bool = False,
extra_user: str | None = None,
) -> list[dict[str, Any]]:
"""Build a chat-template-ready prompt from current runtime state.
Mirrors what the recipe renders into ``${task}\nPlan:
${plan}\nMemory: ${memory}`` for the high-level branches.
"""
# Always emit ``Plan: `` / ``Memory: `` labels — even with empty
# values — to mirror the training-time recipe substitution.
task = state.get("task") or ""
plan = state.get("current_plan") or ""
memory = state.get("current_memory") or ""
parts = [task, f"Plan: {plan}", f"Memory: {memory}"]
if include_completed and state.get("current_subtask"):
parts.append(f"Completed subtask: {state['current_subtask']}")
head = "\n".join(parts)
msgs: list[dict[str, Any]] = [{"role": "user", "content": head}]
if extra_user:
msgs.append({"role": "user", "content": extra_user})
return msgs
# ---------------------------------------------------------------------------
# Per-recipe prompt builders. Each one mirrors a single sub-recipe's
# message layout in the recipe so the chat-templated
# prompt at inference matches what the model saw during training.
# Generic ``_control_context_messages`` is kept around as a fallback
# for ad-hoc callers but the four high-level steps now use these.
# ---------------------------------------------------------------------------
def _hirobot_user_head(state: dict[str, Any]) -> str:
"""Build the ``task\\nPlan: …\\nMemory: …`` user content string.
Mirrors what the recipe renders at training time, where
``language_render._substitute`` substitutes empty strings for
missing ``${plan}`` / ``${memory}`` bindings — i.e. the
``Plan: `` / ``Memory: `` prefix labels are *always* in the
user turn, even when their values aren't set yet. Skipping them
here (the previous behaviour) produced a different prompt shape
on early frames before plan / memory are populated and on
samples where the dataset has no plan / memory annotation.
"""
task = state.get("task") or ""
plan = state.get("current_plan") or ""
memory = state.get("current_memory") or ""
return f"{task}\nPlan: {plan}\nMemory: {memory}"
def _msgs_for_subtask(state: dict[str, Any]) -> list[dict[str, Any]]:
"""``high_level_subtask`` recipe layout — predict the subtask from the
task. The v-current recipe's user turn is just ``${task}`` (plan and
memory are not trained), so the inference prompt is the bare task —
no ``Plan: `` / ``Memory: `` lines.
"""
return [{"role": "user", "content": state.get("task") or ""}]
def _msgs_for_memory(state: dict[str, Any]) -> list[dict[str, Any]]:
"""Memory-update prompt — mirrors ``memory_update`` recipe layout.
Recipe layout (``subtask_mem.yaml``):
user: "${task}"
assistant: "Previous memory: ${prior_memory}" (if_present prior)
user: "Completed subtask: ${completed}" (if_present completed)
assistant: → predicts new memory
Fired by ``MemoryUpdateFwd`` on a ``subtask_change`` event:
``state['current_memory']`` is the memory the policy last emitted
(= the ``prior_memory`` binding at training), and
``state['prior_subtask']`` is the subtask that just got replaced
(= the ``completed_subtask`` binding at training).
"""
msgs: list[dict[str, Any]] = [
{"role": "user", "content": state.get("task") or ""},
]
prior_memory = state.get("current_memory")
if prior_memory:
msgs.append({"role": "assistant", "content": f"Previous memory: {prior_memory}"})
completed_subtask = state.get("prior_subtask")
if completed_subtask:
msgs.append({"role": "user", "content": f"Completed subtask: {completed_subtask}"})
return msgs
def _msgs_for_interjection(state: dict[str, Any]) -> list[dict[str, Any]]:
"""``user_interjection_response`` recipe layout."""
msgs: list[dict[str, Any]] = [{"role": "user", "content": state.get("task") or ""}]
if state.get("current_plan"):
msgs.append({"role": "assistant", "content": f"Previous plan:\n{state['current_plan']}"})
interjection = state.get("recent_interjection")
if interjection:
msgs.append({"role": "user", "content": interjection})
return msgs
def _msgs_for_plan(state: dict[str, Any]) -> list[dict[str, Any]]:
"""``plan_generation`` recipe layout — bare task → plan.
The assistant turn is the generation target, so we only render
the user turn at inference; the runtime appends the predicted
plan after sampling.
"""
return [{"role": "user", "content": state.get("task") or ""}]
def _msgs_for_vqa(question: str) -> list[dict[str, Any]]:
"""``ask_vqa_*`` recipe layout (text-only at inference)."""
return [{"role": "user", "content": question}]
def _maybe_observation(provider: Any) -> dict | None:
"""Pull one observation from ``provider`` if it's set, else ``None``.
Errors from the provider are logged at debug level and swallowed —
text generation still runs (in text-only mode) so a flaky frame
source doesn't kill the REPL.
"""
if provider is None:
return None
try:
return provider()
except Exception as exc: # noqa: BLE001
logger.debug("observation_provider raised %s — falling back to text-only", exc)
return None
def _generate_with_policy(
policy: Any,
messages: list[dict[str, Any]],
*,
observation: dict | None = None,
state: dict[str, Any] | None = None,
label: str = "select_message",
min_new_tokens: int = 0,
temperature: float = 0.0,
top_p: float = 1.0,
suppress_loc_tokens: bool = False,
) -> str:
"""Drive ``policy.select_message`` with a chat batch (and optional obs).
When ``observation`` carries ``observation.images.*`` and
``observation.state``, those are merged into the batch so
``select_message`` runs the same VLM prefix the policy was trained
on. Without an observation the runtime falls back to a text-only
prompt — the text head still runs, but generations may drift from
the training distribution.
Failures are surfaced both to the module logger (``warning``) and,
when ``state`` is given, to the runtime's user-visible log via
:func:`push_log`, so the REPL no longer "looks dead" when
something goes wrong inside generation.
"""
if not hasattr(policy, "select_message"):
if state is not None:
push_log(state, f" [warn] policy has no select_message — skipping {label}")
return ""
text_batch = _build_text_batch(policy, messages)
try:
from lerobot.utils.constants import ( # noqa: PLC0415
OBS_LANGUAGE_ATTENTION_MASK,
OBS_LANGUAGE_TOKENS,
)
batch: dict[str, Any] = {
OBS_LANGUAGE_TOKENS: text_batch["lang_tokens"],
OBS_LANGUAGE_ATTENTION_MASK: text_batch["lang_masks"],
}
if observation:
for k, v in observation.items():
if isinstance(k, str) and k.startswith("observation.") and k not in batch:
batch[k] = v
kwargs: dict[str, Any] = {
"tokenizer": text_batch["tokenizer"],
"min_new_tokens": min_new_tokens,
"temperature": temperature,
"top_p": top_p,
}
kwargs["suppress_loc_tokens"] = suppress_loc_tokens
return policy.select_message(batch, **kwargs)
except Exception as exc: # noqa: BLE001
logger.warning("%s failed: %s", label, exc, exc_info=logger.isEnabledFor(logging.DEBUG))
if state is not None:
push_log(state, f" [warn] {label} failed: {type(exc).__name__}: {exc}")
return ""
_SAY_RE = re.compile(r"<\s*say\s*>(.*?)<\s*/\s*say\s*>", re.IGNORECASE | re.DOTALL)
def _split_plan_and_say(text: str) -> tuple[str, str]:
"""Pull a ``<say>...</say>`` snippet out of ``text``; remainder is plan.
The training-time tool-call serializer wraps ``say(text="")`` in a
deterministic textual marker so prefix-LM-style training learns to
emit it. The runtime parses it back here. If no marker is present,
the entire text is treated as plan with no speech.
"""
if not text:
return "", ""
match = _SAY_RE.search(text)
if not match:
return text.strip(), ""
speech = match.group(1).strip().strip('"').strip("'")
plan = (text[: match.start()] + text[match.end() :]).strip()
return plan, speech
"""Compatibility exports for PI052 model helper imports."""
from .pi052_adapter import (
_build_text_batch,
_generate_with_policy,
_get_loc_tokenizer,
looks_like_gibberish as _looks_like_gibberish,
)
__all__ = [
"_build_text_batch",
"_generate_with_policy",
"_get_loc_tokenizer",
"_looks_like_gibberish",
]
@@ -1,134 +0,0 @@
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Trigger primitives for PI052's multi-rate inference runtime.
Mirrors the plan's Section "Runtime orchestration": each
``InferenceStep`` is gated by a :class:`Trigger` that decides per tick
whether the step fires. Two trigger flavours cover all the cadences
the canonical recipe needs:
* :class:`HzTrigger` for periodic beats (action chunks at ~3-5 Hz,
high-level subtask generation at ~1 Hz, action dispatch at ~50 Hz)
* :class:`EventTrigger` for one-shot reactions (subtask boundary →
memory update; user interjection → plan refresh; user VQA query →
vqa answer; pending tool call → dispatcher)
Triggers are stateless except for ``HzTrigger``'s last-fire timestamp.
The runtime stores the :class:`Tick` clock as ``state["_tick"]`` so
every step shares a single time source.
"""
from __future__ import annotations
import time
from dataclasses import dataclass, field
from typing import Any, Protocol
@dataclass
class Tick:
"""Single tick from :class:`TickClock`. Carries time references the
runtime steps consume to gate themselves."""
index: int
"""Monotonic counter — increments by one per tick."""
monotonic_seconds: float
"""``time.monotonic()`` at the start of this tick."""
@dataclass
class TickClock:
"""Drives the runtime loop at up to ``max_rate_hz``.
Sleeps just enough between :meth:`advance` calls to enforce the
rate. With ``max_rate_hz=50`` the loop wakes ~every 20ms; the
higher-level ``HzTrigger`` slices that timeline into sub-cadences.
"""
max_rate_hz: float = 50.0
_index: int = field(default=0, init=False)
_last_seconds: float | None = field(default=None, init=False)
def advance(self) -> Tick:
period = 1.0 / max(self.max_rate_hz, 0.1)
now = time.monotonic()
if self._last_seconds is not None:
sleep_for = (self._last_seconds + period) - now
if sleep_for > 0:
time.sleep(sleep_for)
now = time.monotonic()
self._last_seconds = now
self._index += 1
return Tick(index=self._index, monotonic_seconds=now)
class Trigger(Protocol):
"""Decide whether the next ``InferenceStep`` should fire."""
def should_fire(self, tick: Tick, state: dict[str, Any]) -> bool: ...
@dataclass
class HzTrigger:
"""Fire at most ``hz`` times per second.
A step that gates further (e.g. ``HighLevelSubtaskFwd`` skipping
when the action queue is non-empty) and wants the trigger to
retry next tick instead of waiting a full period can call
:meth:`rearm` from inside ``run``. Without this, a low-hz trigger
(e.g. ``hz=0.2`` = once per 5 s) almost never coincides with the
brief queue-empty window and the step never fires at all.
"""
hz: float
_last_seconds: float | None = field(default=None, init=False)
def should_fire(self, tick: Tick, state: dict[str, Any]) -> bool:
period = 1.0 / max(self.hz, 1e-6)
if self._last_seconds is None or (tick.monotonic_seconds - self._last_seconds) >= period:
self._last_seconds = tick.monotonic_seconds
return True
return False
def rearm(self) -> None:
"""Mark the trigger as not having fired, so the next tick re-evaluates.
Used by a step that decided to skip after ``should_fire`` already
committed the firing — keeps the cadence honest without losing
the slot.
"""
self._last_seconds = None
@dataclass
class EventTrigger:
"""Fire when ``event_name`` is in ``state["events_this_tick"]``.
The runtime fills ``events_this_tick`` once per tick from:
* stdin / network input (``user_interjection``, ``user_vqa_query``,
``stop``)
* internal state transitions (``subtask_change``,
``tool_call_pending``)
The list is consumed (cleared at the end of the tick) so events
fire at most once.
"""
event_name: str
def should_fire(self, tick: Tick, state: dict[str, Any]) -> bool:
events: list[str] = state.get("events_this_tick") or []
return self.event_name in events
+2 -7
View File
@@ -76,10 +76,7 @@ def make_state_panel(state: dict[str, Any]) -> Any:
table.add_column(justify="left")
for key, label in _STATE_KEYS:
value = state.get(key)
if value is None:
rendered = Text("(not set)", style="dim italic")
else:
rendered = Text(str(value), style="bold")
rendered = Text("(not set)", style="dim italic") if value is None else Text(str(value), style="bold")
table.add_row(label, rendered)
queue = state.get("action_queue")
queue_len = len(queue) if hasattr(queue, "__len__") else 0
@@ -92,9 +89,7 @@ def make_state_panel(state: dict[str, Any]) -> Any:
)
table.add_row("", footer)
run_mode = state.get("mode", "action")
mode_tag = (
"[green]action[/]" if run_mode == "action" else "[yellow]paused[/]"
)
mode_tag = "[green]action[/]" if run_mode == "action" else "[yellow]paused[/]"
return Panel(
table,
title=f"[bold]PI052 state[/] · mode: {mode_tag}",
+25 -29
View File
@@ -42,11 +42,10 @@ import subprocess
import sys
import time
import webbrowser
from contextlib import suppress
from pathlib import Path
from typing import Any
from .runtime_state import push_log
logger = logging.getLogger(__name__)
_IMAGE_PREFIX = "observation.images."
@@ -162,8 +161,7 @@ def parse_loc_answer(answer: str) -> dict | None:
boxes.append((x1, y1, x2, y2, label))
if boxes:
detections = [
{"label": lbl, "bbox_format": "xyxy", "bbox": [x1, y1, x2, y2]}
for (x1, y1, x2, y2, lbl) in boxes
{"label": lbl, "bbox_format": "xyxy", "bbox": [x1, y1, x2, y2]} for (x1, y1, x2, y2, lbl) in boxes
]
return {"kind": "bbox", "payload": {"detections": detections}, "normalized": True}
if len(points) == 1:
@@ -174,9 +172,7 @@ def parse_loc_answer(answer: str) -> dict | None:
"normalized": True,
}
if points: # several bare points → treat as detections-as-points
detections = [
{"label": lbl, "bbox_format": "xyxy", "bbox": [x, y, x, y]} for (x, y, lbl) in points
]
detections = [{"label": lbl, "bbox_format": "xyxy", "bbox": [x, y, x, y]} for (x, y, lbl) in points]
return {"kind": "bbox", "payload": {"detections": detections}, "normalized": True}
return None
@@ -311,11 +307,11 @@ def _open_file(path: Path) -> None:
"""Best-effort open ``path`` in the OS default viewer."""
try:
if sys.platform == "darwin":
subprocess.run(["open", str(path)], check=False)
subprocess.run(["open", str(path)], check=False) # nosec B607
elif sys.platform.startswith("linux"):
subprocess.run(["xdg-open", str(path)], check=False)
subprocess.run(["xdg-open", str(path)], check=False) # nosec B607
elif os.name == "nt":
os.startfile(str(path)) # type: ignore[attr-defined] # noqa: S606
os.startfile(str(path)) # type: ignore[attr-defined] # noqa: S606 # nosec B606
else: # pragma: no cover - exotic platform
webbrowser.open(path.resolve().as_uri())
except Exception as exc: # noqa: BLE001
@@ -339,10 +335,11 @@ def save_and_open_overlay(image: Any, out_dir: str | Path = "./vqa_overlays") ->
def handle_vqa_query(
*,
policy: Any,
policy_adapter: Any | None = None,
policy: Any | None = None,
observation_provider: Any,
question: str,
state: dict[str, Any],
state: Any,
input_fn: Any = input,
print_fn: Any = print,
) -> None:
@@ -351,22 +348,26 @@ def handle_vqa_query(
Called synchronously from the input layer while the runtime is in
``/question`` mode (the action loop is gated off, so the policy is
not in concurrent use). Progress is reported via both
:func:`push_log` (REPL panel scrollback) and ``print_fn`` (direct
stdout) — in autonomous question mode the panel redraw is suspended,
``state.log`` (REPL panel scrollback) and ``print_fn`` (direct stdout)
— in autonomous question mode the panel redraw is suspended,
so the direct print is what the operator actually sees.
"""
from .steps import _generate_with_policy, _msgs_for_vqa # noqa: PLC0415
if policy_adapter is None and policy is not None:
from .pi052_adapter import PI052PolicyAdapter # noqa: PLC0415
policy_adapter = PI052PolicyAdapter(policy)
def report(line: str) -> None:
"""Surface a line both to the panel scrollback and to stdout."""
push_log(state, line)
try:
if hasattr(state, "log"):
state.log(line)
else:
state.setdefault("log_lines", []).append(line)
with suppress(Exception):
print_fn(line)
except Exception: # noqa: BLE001
pass
if policy is None or not hasattr(policy, "select_message"):
report(" [warn] vqa: policy has no select_message — skipping")
if policy_adapter is None:
report(" [warn] vqa: no policy adapter — skipping")
return
observation: dict | None = None
@@ -383,19 +384,14 @@ def handle_vqa_query(
# consumes *all* ``config.image_features`` regardless of which
# camera the sub-recipe was tagged for. So the model always sees
# every camera; the operator never has to name one to ask.
answer = _generate_with_policy(
policy,
_msgs_for_vqa(question),
observation=observation,
state=state,
label="vqa gen",
)
result = policy_adapter.answer_vqa(question, None, observation, state)
answer = result.answer
if not answer:
report(" [info] vqa gen returned empty")
return
report(f" vqa: {answer}")
parsed = parse_vqa_answer(answer)
parsed = result.parsed if result.parsed is not None else parse_vqa_answer(answer)
if not answer_has_overlay(parsed):
if parsed is None:
report(" [info] vqa answer is not JSON — no overlay")
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,88 @@
from lerobot.policies.language_conditioned import (
LanguageConditionedRuntime,
RuntimeState,
ToolCall,
VQAResult,
)
class FakeAdapter:
def __init__(self):
self.updated = False
self.text_calls = []
def select_action(self, observation, state):
assert observation == {"observation.state": 1}
assert state.task == "clean"
return ["a0", "a1"]
def select_text(self, kind, observation, state, user_text=None):
self.text_calls.append((kind, user_text))
return "new plan <say>ok</say>"
def parse_tool_calls(self, text):
assert text == "new plan <say>ok</say>"
return [ToolCall("say", {"text": "ok"})]
def answer_vqa(self, question, camera, observation, state):
return VQAResult(answer=f"answer: {question}")
def update_language_state(self, observation, state):
self.updated = True
state.set_context("subtask", "pick cup", label="subtask")
class FakeTool:
def __init__(self):
self.calls = []
def call(self, args):
self.calls.append(args)
def test_runtime_tick_updates_language_enqueues_and_dispatches_action():
adapter = FakeAdapter()
executed = []
runtime = LanguageConditionedRuntime(
policy_adapter=adapter,
observation_provider=lambda: {"observation.state": 1},
action_executor=executed.append,
)
runtime.set_task("clean")
logs = runtime.step_once()
assert adapter.updated
assert runtime.state.language_context["subtask"] == "pick cup"
assert executed == ["a0"]
assert list(runtime.state.action_queue) == ["a1"]
assert " subtask: pick cup" in logs
def test_runtime_handles_user_interjection_and_dispatches_tools():
adapter = FakeAdapter()
tool = FakeTool()
runtime = LanguageConditionedRuntime(
policy_adapter=adapter,
observation_provider=lambda: {"observation.state": 1},
tools={"say": tool},
)
runtime.set_task("clean")
runtime.state.extra["recent_interjection"] = "please say ok"
runtime.state.emit("user_interjection")
logs = runtime.step_once()
assert ("interjection", "please say ok") in adapter.text_calls
assert runtime.state.language_context["plan"] == "new plan <say>ok</say>"
assert tool.calls == [{"text": "ok"}]
assert " speech: ok" in logs
def test_runtime_state_aliases_legacy_keys_to_language_context():
state = RuntimeState()
state["current_subtask"] = "open drawer"
state["current_memory"] = "drawer open"
assert state.get("current_subtask") == "open drawer"
assert state.language_context == {"subtask": "open drawer", "memory": "drawer open"}
@@ -0,0 +1,54 @@
from types import SimpleNamespace
from lerobot.policies.language_conditioned import RuntimeState
from lerobot.policies.pi052.inference.pi052_adapter import PI052PolicyAdapter, split_plan_and_say
def test_pi052_adapter_builds_recipe_prompts_from_runtime_state():
adapter = PI052PolicyAdapter(policy=object())
state = RuntimeState(
task="clean the kitchen",
language_context={"memory": "cup moved", "plan": "pick then place"},
extra={"prior_subtask": "pick the cup"},
)
assert adapter.messages_for("subtask", state) == [{"role": "user", "content": "clean the kitchen"}]
assert adapter.messages_for("memory", state) == [
{"role": "user", "content": "clean the kitchen"},
{"role": "assistant", "content": "Previous memory: cup moved"},
{"role": "user", "content": "Completed subtask: pick the cup"},
]
assert adapter.messages_for("interjection", state, user_text="wait") == [
{"role": "user", "content": "clean the kitchen"},
{"role": "assistant", "content": "Previous plan:\npick then place"},
{"role": "user", "content": "wait"},
]
assert adapter.messages_for("vqa", state, user_text="where is the cup?") == [
{"role": "user", "content": "where is the cup?"}
]
def test_pi052_adapter_parses_say_tool_calls_and_plan_text():
adapter = PI052PolicyAdapter(policy=object())
text = "Move to the sink. <say>heading to the sink</say>"
assert split_plan_and_say(text) == ("Move to the sink.", "heading to the sink")
assert adapter.parse_tool_calls(text)[0].name == "say"
assert adapter.parse_tool_calls(text)[0].arguments == {"text": "heading to the sink"}
assert adapter.plan_from_text(text) == "Move to the sink."
def test_pi052_runtime_cli_smoke_does_not_load_model(monkeypatch):
from lerobot.policies.pi052.inference import runtime_cli
fake_policy = SimpleNamespace(config=SimpleNamespace(device="cpu"))
monkeypatch.setattr(
runtime_cli,
"_load_policy_and_preprocessor",
lambda policy_path, dataset_repo_id: (fake_policy, None, None, None),
)
monkeypatch.setattr(runtime_cli, "_build_tools", lambda no_tts, tts_voice: {})
monkeypatch.setattr(runtime_cli, "_run_repl", lambda runtime, initial_task, max_ticks: 0)
assert runtime_cli.main(["--policy.path=fake", "--no_robot", "--task=clean", "--max_ticks=0"]) == 0