From e786034e45ec1e582cd7f80ccf4f96f657faf26a Mon Sep 17 00:00:00 2001 From: exijn Date: Tue, 10 Nov 2026 20:43:14 +0900 Subject: [PATCH 1/2] =?UTF-8?q?fix(physical):=20=EC=9E=90=EC=84=B8=C2=B7?= =?UTF-8?q?=ED=94=84=EB=A6=AC=EC=85=8B=20=EC=8B=A4=ED=8C=A8=20=EC=8B=9C=20?= =?UTF-8?q?=EC=9E=A0=EA=B9=80=20=EB=8C=80=EC=8B=A0=20=EC=9E=90=EC=84=B8=20?= =?UTF-8?q?=EB=B6=88=EB=AA=85=EC=9C=BC=EB=A1=9C=20=EB=91=90=EA=B3=A0=20?= =?UTF-8?q?=EC=9E=AC=EC=A1=B0=ED=9A=8C=EB=A1=9C=20=EB=B3=B5=EA=B5=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - SIT/STAND/PRESET 실패(SDK 거절·예외)와 STAND 취소 시 추적 자세를 UNKNOWN 으로 둔다. - UNKNOWN 동안 물리 명령 프로그램은 dispatch 전에 거부하고 NOT_READY(POSTURE_UNKNOWN)로 보고한다. - adapter 의 읽기 전용 observed_posture() 가 있으면 실패 직후·READY 계산·승인 때 재조회해 동기화한다. - 10/31·11/1 잠김(Sit 거절 → sitting=true → RiseSit 거절) 재현·복구 회귀 테스트 추가. Part of #98 Co-Authored-By: Claude Opus 5.5 --- docs/controlled-motion-open.md | 21 +- src/poppy_agent/execution/open_motion.py | 109 ++++++-- src/poppy_agent/hardware/factory.py | 9 +- src/poppy_agent/hardware/unitree.py | 13 +- src/poppy_agent/main.py | 21 +- tests/test_posture_lock_recovery.py | 303 +++++++++++++++++++++++ 6 files changed, 454 insertions(+), 22 deletions(-) create mode 100644 tests/test_posture_lock_recovery.py diff --git a/docs/controlled-motion-open.md b/docs/controlled-motion-open.md index b73a49a..6f0403f 100644 --- a/docs/controlled-motion-open.md +++ b/docs/controlled-motion-open.md @@ -35,10 +35,29 @@ - MOVE 명령당 ≤ 3.0 m, 총 이동 ≤ 4.0 m. TURN 명령당 ≤ 360°. - 총 계획 시간(이동 거리/속도 + 회전 각도/각속도 + 자세 settle + preset settle + WAIT) ≤ 120 s. - SIT 이후 STAND 전까지 MOVE·TURN·PRESET 거부. SIT 상태는 실행 사이에도 유지된다(SIT 명령을 - 보낸 순간부터 앉은 것으로 간주, STAND 성공 후 해제). + 보낸 순간부터 앉은 것으로 간주, STAND 성공 후 해제, 실패 시 아래 `UNKNOWN`). - PRESET은 allow-list 코드만 허용(`CommandSafetyPolicy`와 guard 두 곳에서 검사). - STOP 이후 명령은 실행하지 않는다. +## 자세 불명(`POSTURE_UNKNOWN`)과 복구 (#98) + +10/31·11/1 에 `Sit` 이 SDK 에서 거절됐는데도 Agent 는 SIT 전송 시점부터 앉음(`sitting=true`)으로 +간주했다. 그 뒤 HELLO 는 "after SIT" 로 거부되고, STAND 는 서 있는 로봇에 `RiseSit` 을 보내 다시 +거절되어 재시작 전까지 잠겼다. + +- POSTURE(SIT/STAND)·PRESET 이 실패(SDK 거절·예외)하거나 STAND 가 취소되면 추적 자세를 버리고 + `UNKNOWN` 으로 둔다. MOVE 실패는 자세를 바꾸지 않는다. +- `UNKNOWN` 동안 물리 명령이 있는 프로그램은 dispatch 전에 모두 거부된다(맹목적인 `RiseSit`·`StandUp` + 없음). WAIT 만 있는 프로그램은 허용된다. `physical.state` 는 `NOT_READY`, `reasons` 에 + `POSTURE_UNKNOWN`, `trial.posture`·`trial.postureUnknownReason` 에 상태와 원인이 보인다. +- 실제 자세 재조회: adapter 가 읽기 전용 `observed_posture()`(SIT/STAND/모름)를 제공하면 실패 직후, + READY 계산 때, 다음 실행 승인 때 자동으로 다시 읽고 그 값으로 동기화한다. 로봇이 답하지 않으면 + `UNKNOWN` 이 유지된다. adapter 가 있으면 시작 시 자세도 이 값으로 정한다. +- 현재 `develop` 의 `UnitreeGo2Adapter` 는 `rt/lowstate` 만 구독하므로 `observed_posture()` 가 없다. + `rt/sportmodestate`(`mode`) 기반 구현은 #97 구독 위에서 실기 확인 후 추가한다. +- 그 전까지 운영자 복구: 리모컨으로 로봇이 **서 있는 것을 눈으로 확인**한 뒤 Agent 를 재시작한다 + (재시작 시 서 있음으로 시작). 앉아 있으면 리모컨으로 먼저 세운다. + ## SDK 매핑 | Poppy | SDK 호출 | 근거 | diff --git a/src/poppy_agent/execution/open_motion.py b/src/poppy_agent/execution/open_motion.py index 290b22b..f1b3e5d 100644 --- a/src/poppy_agent/execution/open_motion.py +++ b/src/poppy_agent/execution/open_motion.py @@ -5,6 +5,11 @@ TURN or PRESET after SIT until STAND), the PRESET allow-list and the approval expiry. There is no process budget; a failed or unknown outcome fails only that Execution and is recorded, never retried. + +Posture is tracked across Executions. A failed or cancelled POSTURE or PRESET +leaves the physical posture unknown: every physical program is then rejected +until the posture is re-read from the Robot (``posture_probe``) or the Agent is +restarted after an operator has confirmed the Robot is standing. """ from __future__ import annotations @@ -40,6 +45,21 @@ class OpenMotionGuardState(StrEnum): AVAILABLE = "AVAILABLE" EXECUTING = "EXECUTING" EXPIRED = "EXPIRED" + POSTURE_UNKNOWN = "POSTURE_UNKNOWN" + + +class TrackedPosture(StrEnum): + """The guard's belief about the physical posture between commands.""" + + STANDING = "STANDING" + SITTING = "SITTING" + UNKNOWN = "UNKNOWN" + + +POSTURE_UNKNOWN_MESSAGE = ( + "Robot posture is unknown after a failed posture or preset command; " + "re-read the posture from the Robot, or confirm it is standing and restart the Agent" +) @dataclass(frozen=True, slots=True) @@ -60,15 +80,23 @@ def __init__( *, telemetry_fresh: Callable[[], bool], now: Callable[[], datetime], + posture_probe: Callable[[], Posture | None] | None = None, ) -> None: self._settings = settings self._telemetry_fresh = telemetry_fresh self._now = now + self._posture_probe = posture_probe self._lock = Lock() self._executing = False - # The Robot is checked standing before the trial; SIT is tracked across - # Executions because the physical posture persists between them. - self._sitting = False + # Without a probe the Robot is checked standing before the trial; with a + # probe the actual posture is read before the first Execution. The + # posture is tracked across Executions because it persists between them. + self._posture = TrackedPosture.STANDING + self._posture_unknown_reason: str | None = None + if posture_probe is not None: + self._posture = TrackedPosture.UNKNOWN + self._posture_unknown_reason = "posture not yet read from the Robot" + self._resync_posture_locked() self._executions_admitted = 0 self._last_failure: str | None = None self._last_plan: OpenMotionPlanSummary | None = None @@ -143,7 +171,11 @@ def admit(self, program: HighLevelCommandProgram) -> None: raise OpenMotionGuardError("the open trial approval window has expired") if self._executing: raise OpenMotionGuardError("an Execution is already in progress") - summary = self.plan(program, sitting=self._sitting) + if self._posture is TrackedPosture.UNKNOWN: + self._resync_posture_locked() + summary = self.plan(program, sitting=self._posture is TrackedPosture.SITTING) + if summary.physical_commands and self._posture is TrackedPosture.UNKNOWN: + raise OpenMotionGuardError(POSTURE_UNKNOWN_MESSAGE) if summary.physical_commands and self._telemetry_fresh() is not True: raise OpenMotionGuardError("Robot telemetry is not fresh") self._executing = True @@ -160,17 +192,59 @@ def lock(self, reason: str) -> None: self._last_failure = reason def note_sit_requested(self) -> None: - """Assume SIT from the moment it is sent, even if its result is unknown.""" + """Assume SIT from the moment it is sent; a failure later makes it unknown.""" with self._lock: - self._sitting = True + self._posture = TrackedPosture.SITTING def sitting(self) -> bool: with self._lock: - return self._sitting + return self._posture is TrackedPosture.SITTING + + def posture(self) -> TrackedPosture: + with self._lock: + return self._posture def note_stand_completed(self) -> None: with self._lock: - self._sitting = False + self._posture = TrackedPosture.STANDING + self._posture_unknown_reason = None + + def mark_posture_unknown(self, reason: str) -> TrackedPosture: + """Record a failed posture-changing command and re-read the actual posture. + + The SDK may reject or time out on a command the Robot partly performed, + so the internal belief is dropped instead of guessed. Returns the + posture after the immediate re-read (``UNKNOWN`` without a probe). + """ + with self._lock: + self._last_failure = reason + self._posture = TrackedPosture.UNKNOWN + self._posture_unknown_reason = reason + return self._resync_posture_locked() + + def resync_posture(self) -> TrackedPosture: + """Operator path: replace the tracked posture with the posture read now. + + Only an answer from the Robot can clear ``UNKNOWN``; without a probe, or + when the probe has no fresh answer, the tracked posture is unchanged. + """ + with self._lock: + return self._resync_posture_locked() + + def _resync_posture_locked(self) -> TrackedPosture: + if self._posture_probe is None: + return self._posture + try: + observed = self._posture_probe() + except Exception: + observed = None + if observed is Posture.SIT: + self._posture = TrackedPosture.SITTING + self._posture_unknown_reason = None + elif observed is Posture.STAND: + self._posture = TrackedPosture.STANDING + self._posture_unknown_reason = None + return self._posture def available(self) -> bool: return self.state() is OpenMotionGuardState.AVAILABLE @@ -179,11 +253,13 @@ def state(self) -> OpenMotionGuardState: if self.expired(): return OpenMotionGuardState.EXPIRED with self._lock: - return ( - OpenMotionGuardState.EXECUTING - if self._executing - else OpenMotionGuardState.AVAILABLE - ) + if self._executing: + return OpenMotionGuardState.EXECUTING + if self._posture is TrackedPosture.UNKNOWN: + self._resync_posture_locked() + if self._posture is TrackedPosture.UNKNOWN: + return OpenMotionGuardState.POSTURE_UNKNOWN + return OpenMotionGuardState.AVAILABLE def snapshot(self) -> dict[str, object]: state = self.state() @@ -191,7 +267,10 @@ def snapshot(self) -> dict[str, object]: plan = self._last_plan return { "state": state.value, - "sitting": self._sitting, + "sitting": self._posture is TrackedPosture.SITTING, + "posture": self._posture.value, + "postureUnknownReason": self._posture_unknown_reason, + "postureProbe": self._posture_probe is not None, "executionsAdmitted": self._executions_admitted, "lastFailure": self._last_failure, "lastPlan": None @@ -232,5 +311,7 @@ def open_motion_readiness_evidence( "OpenMotionGuardError", "OpenMotionGuardState", "OpenMotionPlanSummary", + "POSTURE_UNKNOWN_MESSAGE", + "TrackedPosture", "open_motion_readiness_evidence", ] diff --git a/src/poppy_agent/hardware/factory.py b/src/poppy_agent/hardware/factory.py index 734430f..d8cbbf5 100644 --- a/src/poppy_agent/hardware/factory.py +++ b/src/poppy_agent/hardware/factory.py @@ -7,6 +7,7 @@ from datetime import UTC, datetime from uuid import UUID +from poppy_agent.command import Posture from poppy_agent.config.open_motion import PRESET_SDK_METHODS, OpenMotionSettings from poppy_agent.config.physical_trial import PhysicalTrialSettings from poppy_agent.execution.cancellation import ExecutionCancellationToken @@ -169,12 +170,14 @@ def create_open_motion_executor( sport_client_factory: Callable[[], object] | None = None, timing: MotionStreamTiming | None = None, now: Callable[[], datetime] | None = None, + posture_probe: Callable[[], Posture | None] | None = None, ) -> OpenMotionExecution: """Create the ``controlled_motion_open`` executor after the readiness gate passes. Evidence is derived only from validated ``settings``; the gate runs before any SDK object is constructed. ``sport_client_factory``, ``timing`` and - ``now`` are test seams; production passes none of them. + ``now`` are test seams; production passes none of them. ``posture_probe`` + reads the actual posture from read-only Robot state (``None`` when unknown). """ evidence = ( open_motion_readiness_evidence(settings, command_client_kind=PhysicalCommandClientKind.REAL) @@ -187,7 +190,9 @@ def create_open_motion_executor( ) assert settings is not None clock = now or (lambda: datetime.now(UTC)) - guard = OpenMotionGuard(settings, telemetry_fresh=telemetry_fresh, now=clock) + guard = OpenMotionGuard( + settings, telemetry_fresh=telemetry_fresh, now=clock, posture_probe=posture_probe + ) stream_timing = timing or MotionStreamTiming() client = UnitreeSdkCommandClient( sdk_client_factory=sport_client_factory, diff --git a/src/poppy_agent/hardware/unitree.py b/src/poppy_agent/hardware/unitree.py index b67acc6..3badfda 100644 --- a/src/poppy_agent/hardware/unitree.py +++ b/src/poppy_agent/hardware/unitree.py @@ -339,10 +339,17 @@ def _dispatch_open_posture( self._require_client_ok(self._balance_stand(), "BalanceStand") actions.guard.note_stand_completed() except ExecutionCancelledError: + if posture is Posture.STAND: + # The stand may have started; the tracked SIT/STAND no longer holds. + actions.guard.mark_posture_unknown(f"{posture.value} cancelled") self._log_action_finished(posture.value, "CANCELLED") raise except Exception as exc: - actions.guard.lock(f"{posture.value} outcome unknown: {safe_exception_type(exc)}") + # A rejected Sit/RiseSit leaves the belief wrong either way (the 10/31 + # and 11/1 lock): drop it and re-read the actual posture. + actions.guard.mark_posture_unknown( + f"{posture.value} outcome unknown: {safe_exception_type(exc)}" + ) self._log_action_finished(posture.value, "FAILED", exc) raise self._log_action_finished(posture.value, "COMPLETED") @@ -368,7 +375,9 @@ def _dispatch_preset( self._log_action_finished(code, "CANCELLED") raise except Exception as exc: - actions.guard.lock(f"PRESET {code} outcome unknown: {safe_exception_type(exc)}") + actions.guard.mark_posture_unknown( + f"PRESET {code} outcome unknown: {safe_exception_type(exc)}" + ) self._log_action_finished(code, "FAILED", exc) raise self._log_action_finished(code, "COMPLETED") diff --git a/src/poppy_agent/main.py b/src/poppy_agent/main.py index fc6ee9d..3bf2590 100644 --- a/src/poppy_agent/main.py +++ b/src/poppy_agent/main.py @@ -12,8 +12,10 @@ from uuid import UUID from poppy_agent.agent import Agent, create_agent +from poppy_agent.command import Posture from poppy_agent.config import AgentConfig, ConfigurationError, OpenMotionSettings from poppy_agent.execution import MockExecutionExecutor, PhysicalExecutionBlockedError +from poppy_agent.execution.open_motion import OpenMotionGuardState from poppy_agent.hardware import create_controlled_trial_executor, create_open_motion_executor from poppy_agent.observability import ( PHYSICAL_EXECUTION_BLOCKED, @@ -139,8 +141,14 @@ def _create_physical_trial_executor( _publish_blocked(runtime, config, ("ROBOT_ADAPTER_UNSUPPORTED",)) return None if isinstance(config.physical_trial, OpenMotionSettings): + posture_probe = getattr(adapter, "observed_posture", None) return _create_open_motion_executor( - config, config.physical_trial, runtime, telemetry_fresh, set_command_readiness + config, + config.physical_trial, + runtime, + telemetry_fresh, + set_command_readiness, + posture_probe if callable(posture_probe) else None, ) try: trial = create_controlled_trial_executor( @@ -204,8 +212,13 @@ def _create_open_motion_executor( runtime: AgentServerRuntime, telemetry_fresh: Callable[[], bool], set_command_readiness: Callable[[Callable[[], bool]], None], + posture_probe: Callable[[], Posture | None] | None = None, ) -> MockExecutionExecutor | None: - """Return the ``controlled_motion_open`` executor, or ``None`` for heartbeat-only.""" + """Return the ``controlled_motion_open`` executor, or ``None`` for heartbeat-only. + + ``posture_probe`` comes from the adapter's read-only ``observed_posture`` when + it exists; without it a failed posture command needs an Agent restart. + """ try: trial = create_open_motion_executor( robot_id=UUID(config.robot_id), @@ -213,6 +226,7 @@ def _create_open_motion_executor( settings=settings, telemetry_fresh=telemetry_fresh, network_interface=config.network_interface, + posture_probe=posture_probe, ) except PhysicalExecutionBlockedError as exc: _publish_blocked(runtime, config, tuple(str(exc).split(": ", 1)[-1].split(", "))) @@ -241,10 +255,11 @@ def physical_status() -> dict[str, object]: "trial": guard.snapshot(), } ready = command_ready() and telemetry_fresh() is True + posture_unknown = guard.state() is OpenMotionGuardState.POSTURE_UNKNOWN return { "enabled": True, "state": "READY" if ready else "NOT_READY", - "reasons": [], + "reasons": ["POSTURE_UNKNOWN"] if posture_unknown else [], "configIssues": [], "mode": settings.mode, "linearSpeedMetersPerSecond": settings.linear_speed_meters_per_second, diff --git a/tests/test_posture_lock_recovery.py b/tests/test_posture_lock_recovery.py new file mode 100644 index 0000000..88ab834 --- /dev/null +++ b/tests/test_posture_lock_recovery.py @@ -0,0 +1,303 @@ +"""Posture lock after a rejected SIT/STAND/PRESET and its recovery (#98), fakes only.""" + +from __future__ import annotations + +from datetime import UTC, datetime, timedelta +from types import SimpleNamespace + +import pytest + +import poppy_agent.main as main_module +from poppy_agent.command import Posture +from poppy_agent.execution import ExecutionStatus +from poppy_agent.execution.open_motion import ( + OpenMotionGuard, + OpenMotionGuardError, + OpenMotionGuardState, + TrackedPosture, +) +from poppy_agent.hardware import OpenMotionExecution, create_open_motion_executor +from poppy_agent.hardware.unitree_sdk import MotionStreamTiming +from test_controlled_motion_open import ( + EXPIRES, + OPEN_ENV, + ROBOT_ID, + START, + FakeAdapter, + FakeClock, + FakeRuntime, + FakeSportClient, + build, + move, + open_settings, + posture, + preset, + run, + task, + turn, + unitree_config, + wait, +) + + +class RejectingSportClient(FakeSportClient): + """SDK double that returns a non-zero code for the named operations.""" + + def __init__(self, clock: FakeClock, *, reject: set[str]) -> None: + super().__init__(clock) + self.reject = reject + + def _record(self, name: str, *args: float) -> int: + self.calls.append((name, args)) + return 3104 if name in self.reject else 0 + + +class Probe: + """Read-only posture source double; ``None`` means no fresh answer.""" + + def __init__(self, value: Posture | None) -> None: + self.value = value + self.calls = 0 + + def __call__(self) -> Posture | None: + self.calls += 1 + return self.value + + +def build_with( + clock: FakeClock, sport: FakeSportClient, probe: Probe | None +) -> OpenMotionExecution: + return create_open_motion_executor( + robot_id=ROBOT_ID, + physical_execution_enabled=True, + settings=open_settings(), + telemetry_fresh=lambda: True, + sport_client_factory=lambda: sport, + timing=MotionStreamTiming(monotonic=clock.monotonic, sleep=clock.sleep, wait=clock.wait), + now=clock.wall_now, + posture_probe=probe, + ) + + +# --- reproduction of the 10/31 and 11/1 lock ---------------------------------- + + +def test_rejected_sit_no_longer_leaves_a_false_sitting_lock() -> None: + """Before #98: Sit rejected -> sitting=True -> HELLO blocked, STAND sent RiseSit.""" + clock = FakeClock() + sport = RejectingSportClient(clock, reject={"Sit"}) + trial = build(clock, sport) + + failed = run(trial, posture(Posture.SIT)) + assert failed.status is ExecutionStatus.FAILED + assert trial.guard.sitting() is False + assert trial.guard.posture() is TrackedPosture.UNKNOWN + assert trial.guard.snapshot()["posture"] == "UNKNOWN" + + sport.calls.clear() + for program in ((preset("HELLO"),), (posture(Posture.STAND),), (move(0.5),)): + blocked = run(trial, *program) + assert blocked.status is ExecutionStatus.FAILED + assert "posture is unknown" in (blocked.failure_reason or "") + # Nothing reached the SDK while the posture was unknown: no blind RiseSit. + assert sport.names() == [] + assert trial.guard.available() is False + assert trial.guard.state() is OpenMotionGuardState.POSTURE_UNKNOWN + + +@pytest.mark.parametrize( + ("reject", "program"), + [ + ({"Sit"}, (posture(Posture.SIT),)), + ({"RiseSit"}, (posture(Posture.SIT), posture(Posture.STAND))), + ({"StandUp"}, (posture(Posture.STAND),)), + ({"BalanceStand"}, (posture(Posture.STAND),)), + ({"Hello"}, (preset("HELLO"),)), + ], +) +def test_every_rejected_posture_or_preset_makes_posture_unknown( + reject: set[str], program: tuple +) -> None: + clock = FakeClock() + sport = RejectingSportClient(clock, reject=reject) + trial = build(clock, sport) + + assert run(trial, *program).status is ExecutionStatus.FAILED + assert trial.guard.posture() is TrackedPosture.UNKNOWN + assert trial.guard.snapshot()["lastFailure"] is not None + + +def test_failed_move_does_not_touch_posture() -> None: + clock = FakeClock() + sport = FakeSportClient(clock, move_error_at=3) + trial = build(clock, sport) + + assert run(trial, move(0.5)).status is ExecutionStatus.FAILED + assert trial.guard.posture() is TrackedPosture.STANDING + assert trial.guard.available() is True + + +def test_cancelled_stand_makes_posture_unknown() -> None: + clock = FakeClock(cancel_after_waits=2) + sport = FakeSportClient(clock) + trial = build(clock, sport) + + result = run(trial, posture(Posture.SIT), posture(Posture.STAND)) + assert result.status is ExecutionStatus.CANCELLED + assert sport.names() == ["Sit", "RiseSit"] + assert trial.guard.posture() is TrackedPosture.UNKNOWN + + +# --- recovery from the actual posture ----------------------------------------- + + +@pytest.mark.parametrize( + ("observed", "tracked", "next_ops"), + [ + (Posture.STAND, TrackedPosture.STANDING, ["Hello", "BalanceStand"]), + (Posture.SIT, TrackedPosture.SITTING, ["RiseSit"]), + ], +) +def test_rejected_sit_resyncs_from_the_observed_posture( + observed: Posture, tracked: TrackedPosture, next_ops: list[str] +) -> None: + clock = FakeClock() + sport = RejectingSportClient(clock, reject={"Sit"}) + probe = Probe(observed) + trial = build_with(clock, sport, probe) + + assert run(trial, posture(Posture.SIT)).status is ExecutionStatus.FAILED + assert trial.guard.posture() is tracked + assert trial.guard.available() is True + + sport.calls.clear() + sport.reject = set() + follow_up = preset("HELLO") if observed is Posture.STAND else posture(Posture.STAND) + assert run(trial, follow_up).status is ExecutionStatus.COMPLETED + assert sport.names() == next_ops + + +def test_unknown_posture_clears_only_when_the_robot_answers() -> None: + clock = FakeClock() + sport = RejectingSportClient(clock, reject={"Hello"}) + probe = Probe(Posture.STAND) + trial = build_with(clock, sport, probe) + probe.value = None + + assert run(trial, preset("HELLO")).status is ExecutionStatus.FAILED + assert trial.guard.state() is OpenMotionGuardState.POSTURE_UNKNOWN + assert run(trial, move(0.5)).status is ExecutionStatus.FAILED + + probe.value = Posture.STAND + assert trial.guard.state() is OpenMotionGuardState.AVAILABLE + assert trial.guard.snapshot()["postureUnknownReason"] is None + assert run(trial, move(0.5)).status is ExecutionStatus.COMPLETED + + +def test_probe_errors_count_as_no_answer() -> None: + def broken() -> Posture | None: + raise RuntimeError("stale sport state") + + guard = OpenMotionGuard( + open_settings(), telemetry_fresh=lambda: True, now=lambda: START, posture_probe=broken + ) + assert guard.posture() is TrackedPosture.UNKNOWN + with pytest.raises(OpenMotionGuardError, match="posture is unknown"): + guard.admit(task(turn(90)).command_program) + + +def test_probe_sets_the_initial_posture() -> None: + guard = OpenMotionGuard( + open_settings(), + telemetry_fresh=lambda: True, + now=lambda: START, + posture_probe=Probe(Posture.SIT), + ) + assert guard.sitting() is True + with pytest.raises(OpenMotionGuardError, match="after SIT"): + guard.admit(task(move(0.5)).command_program) + + +def test_without_probe_operator_resync_keeps_the_lock() -> None: + guard = OpenMotionGuard(open_settings(), telemetry_fresh=lambda: True, now=lambda: START) + guard.mark_posture_unknown("SIT outcome unknown: UnitreeCommandBackendError") + assert guard.resync_posture() is TrackedPosture.UNKNOWN + assert guard.state() is OpenMotionGuardState.POSTURE_UNKNOWN + + +def test_wait_only_program_is_still_admitted_with_unknown_posture() -> None: + guard = OpenMotionGuard(open_settings(), telemetry_fresh=lambda: True, now=lambda: START) + guard.mark_posture_unknown("test") + guard.admit(task(wait(1)).command_program) + + +# --- main wiring --------------------------------------------------------------- + + +class ProbingAdapter(FakeAdapter): + def __init__(self, probe: Probe) -> None: + super().__init__() + self.observed_posture = probe + + +def test_main_wires_observed_posture_and_reports_posture_unknown(monkeypatch) -> None: + clock = FakeClock() + captured: dict[str, object] = {} + + def factory(**kwargs: object) -> OpenMotionExecution: + captured.update(kwargs) + guard = OpenMotionGuard( + open_settings(), + telemetry_fresh=lambda: True, + now=clock.wall_now, + posture_probe=kwargs["posture_probe"], # type: ignore[arg-type] + ) + return OpenMotionExecution(object(), guard) # type: ignore[arg-type] + + monkeypatch.setattr(main_module, "create_open_motion_executor", factory) + expires = (datetime.now(UTC) + timedelta(hours=1)).isoformat() + config = unitree_config( + enabled=True, trial={**OPEN_ENV, "POPPY_PHYSICAL_TRIAL_EXPIRES_AT": expires} + ) + probe = Probe(None) + adapter, runtime = ProbingAdapter(probe), FakeRuntime() + + main_module._create_physical_trial_executor( + config, + SimpleNamespace(adapter=adapter), # type: ignore[arg-type] + runtime, # type: ignore[arg-type] + ) + + assert captured["posture_probe"] is probe + status = runtime.physical_status() + assert status["state"] == "NOT_READY" + assert status["reasons"] == ["POSTURE_UNKNOWN"] + assert adapter.readiness() is False + + probe.value = Posture.STAND + status = runtime.physical_status() + assert status["state"] == "READY" + assert status["reasons"] == [] + assert status["trial"]["posture"] == "STANDING" + assert clock.wall < EXPIRES + + +def test_main_without_observed_posture_passes_no_probe(monkeypatch) -> None: + captured: dict[str, object] = {} + + def factory(**kwargs: object) -> OpenMotionExecution: + captured.update(kwargs) + guard = OpenMotionGuard(open_settings(), telemetry_fresh=lambda: True, now=lambda: START) + return OpenMotionExecution(object(), guard) # type: ignore[arg-type] + + monkeypatch.setattr(main_module, "create_open_motion_executor", factory) + expires = (datetime.now(UTC) + timedelta(hours=1)).isoformat() + config = unitree_config( + enabled=True, trial={**OPEN_ENV, "POPPY_PHYSICAL_TRIAL_EXPIRES_AT": expires} + ) + main_module._create_physical_trial_executor( + config, + SimpleNamespace(adapter=FakeAdapter()), # type: ignore[arg-type] + FakeRuntime(), # type: ignore[arg-type] + ) + assert captured["posture_probe"] is None From d38bfe216c720d8b981ef50df06fafe33da48f4f Mon Sep 17 00:00:00 2001 From: exijn Date: Tue, 10 Nov 2026 20:55:22 +0900 Subject: [PATCH 2/2] =?UTF-8?q?feat(physical):=20=EC=9E=90=EC=84=B8=20?= =?UTF-8?q?=EB=B6=88=EB=AA=85=20=EA=B1=B0=EB=B6=80=EC=97=90=20POSTURE=5FUN?= =?UTF-8?q?KNOWN=20=EC=8B=A4=ED=8C=A8=20=EC=BD=94=EB=93=9C=20=EC=97=B0?= =?UTF-8?q?=EA=B2=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Part of #98 Co-Authored-By: Claude Opus 5.5 --- docs/failure-codes.md | 2 +- src/poppy_agent/execution/open_motion.py | 4 +++- tests/test_posture_lock_recovery.py | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/docs/failure-codes.md b/docs/failure-codes.md index 60821ec..516c60a 100644 --- a/docs/failure-codes.md +++ b/docs/failure-codes.md @@ -8,7 +8,7 @@ Agent 는 FAILED 실행마다 `ExecutionResult.failure_code` 와 구조화 로 | `SAFETY_REJECTED` | 안전 정책·guard 한도로 dispatch 전 거부 | `ExecutionSafetyValidator`, `OpenMotionGuard`/`TrialGuard` 한도 | | `APPROVAL_EXPIRED` | 실제 동작 승인 시간 만료 | `OpenMotionGuard.admit` | | `TELEMETRY_STALE` | 로봇 텔레메트리가 신선하지 않아 거부 | `OpenMotionGuard`/`TrialGuard` | -| `POSTURE_UNKNOWN` | 자세 불명으로 거부 | #110(#98) 머지 후 연결 | +| `POSTURE_UNKNOWN` | 자세·프리셋 실패 뒤 자세 불명으로 거부 | `OpenMotionGuard.admit` (#98) | | `SDK_REJECTED` | SDK 가 명령을 거절(0 이 아닌 결과) | `UnitreeSdkCommandClient._call_operation`, backend 결과 검사 | | `TIMEOUT` | SDK `Move` 호출이 제한 시간 이상 막힘 | `execute_motion_with_cancellation` | | `NOT_MOVED` | 폐루프에서 미이동 판정 | #97 폐루프 머지 후 연결 | diff --git a/src/poppy_agent/execution/open_motion.py b/src/poppy_agent/execution/open_motion.py index 27edfa0..070b60e 100644 --- a/src/poppy_agent/execution/open_motion.py +++ b/src/poppy_agent/execution/open_motion.py @@ -181,7 +181,9 @@ def admit(self, program: HighLevelCommandProgram) -> None: self._resync_posture_locked() summary = self.plan(program, sitting=self._posture is TrackedPosture.SITTING) if summary.physical_commands and self._posture is TrackedPosture.UNKNOWN: - raise OpenMotionGuardError(POSTURE_UNKNOWN_MESSAGE) + raise OpenMotionGuardError( + POSTURE_UNKNOWN_MESSAGE, failure_code=FailureCode.POSTURE_UNKNOWN + ) if summary.physical_commands and self._telemetry_fresh() is not True: raise OpenMotionGuardError( "Robot telemetry is not fresh", failure_code=FailureCode.TELEMETRY_STALE diff --git a/tests/test_posture_lock_recovery.py b/tests/test_posture_lock_recovery.py index 88ab834..677be8d 100644 --- a/tests/test_posture_lock_recovery.py +++ b/tests/test_posture_lock_recovery.py @@ -9,7 +9,7 @@ import poppy_agent.main as main_module from poppy_agent.command import Posture -from poppy_agent.execution import ExecutionStatus +from poppy_agent.execution import ExecutionStatus, FailureCode from poppy_agent.execution.open_motion import ( OpenMotionGuard, OpenMotionGuardError, @@ -99,6 +99,7 @@ def test_rejected_sit_no_longer_leaves_a_false_sitting_lock() -> None: blocked = run(trial, *program) assert blocked.status is ExecutionStatus.FAILED assert "posture is unknown" in (blocked.failure_reason or "") + assert blocked.failure_code is FailureCode.POSTURE_UNKNOWN # Nothing reached the SDK while the posture was unknown: no blind RiseSit. assert sport.names() == [] assert trial.guard.available() is False