Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions src/inspect_robots/controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@

import numpy as np

from inspect_robots.errors import PolicyError
from inspect_robots.policy import Policy
from inspect_robots.spaces import Box
from inspect_robots.types import Action, Observation
Expand Down Expand Up @@ -58,6 +59,8 @@ def next_action(
buffer: deque[Action] = store.setdefault(_BUFFER_KEY, deque())
if not buffer:
chunk = policy.act(observation)
if not chunk.actions:
raise PolicyError("policy emitted an empty ActionChunk (0 actions)")
take = self.replan_interval or len(chunk)
taken = list(chunk.actions)[:take]
buffer.extend(taken)
Expand Down Expand Up @@ -163,6 +166,8 @@ def next_action(
) -> Action:
"""Query once for step ``t`` and blend every retained prediction for that step."""
chunk = policy.act(observation)
if not chunk.actions:
raise PolicyError("policy emitted an empty ActionChunk (0 actions)")
store.setdefault(_INFER_KEY, []).append((chunk.inference_latency_s, len(chunk)))

buffer: list[tuple[int, list[Any], dict[str, Any]]] = store.setdefault(_ENSEMBLE_KEY, [])
Expand All @@ -179,6 +184,9 @@ def next_action(
buffer.sort(key=lambda e: e[0])

predictions = [acts[t - q] for (q, acts, _meta) in buffer]
if not predictions: # pragma: no cover
raise PolicyError("no valid action predictions available for current step")

weights = np.exp(-self.m * np.arange(len(predictions)))
weights = weights / weights.sum()
blended = np.average(np.stack(predictions), axis=0, weights=weights)
Expand Down
50 changes: 50 additions & 0 deletions tests/test_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -99,3 +99,53 @@ def test_stacked_smoothing_controllers_keep_separate_state() -> None:
expected.append(outer_prev)

assert np.allclose(actual, expected)


class _EmptyChunkPolicy:
"""Emits an empty ActionChunk with zero actions."""

def __init__(self) -> None:
self.info = PolicyInfo(
name="empty",
action_space=Box(shape=(2,), semantics=ActionSemantics(control_mode="joint_delta")),
)
self.config = PolicyConfig()

def reset(self, scene: Scene) -> None:
pass

def act(self, observation: Observation) -> ActionChunk:
chunk = ActionChunk(actions=[Action(data=np.zeros(2))])
object.__setattr__(chunk, "actions", [])
return chunk


def test_default_controller_raises_policy_error_on_empty_chunk() -> None:
import pytest

from inspect_robots.errors import PolicyError

policy = _EmptyChunkPolicy()
embodiment = CubePickEmbodiment()
obs = _obs(embodiment)
store: dict[str, object] = {}
ctrl = DefaultController()

with pytest.raises(PolicyError, match="empty ActionChunk"):
ctrl.next_action(policy, obs, 0, store)


def test_ensembling_controller_raises_policy_error_on_empty_chunk() -> None:
import pytest

from inspect_robots.controller import EnsemblingController
from inspect_robots.errors import PolicyError

policy = _EmptyChunkPolicy()
embodiment = CubePickEmbodiment()
obs = _obs(embodiment)
store: dict[str, object] = {}
ctrl = EnsemblingController(policy.info.action_space)

with pytest.raises(PolicyError, match="empty ActionChunk"):
ctrl.next_action(policy, obs, 0, store)
Loading