Skip to content
21 changes: 20 additions & 1 deletion docs/controlled-motion-open.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 호출 | 근거 |
Expand Down
109 changes: 95 additions & 14 deletions src/poppy_agent/execution/open_motion.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -179,19 +253,24 @@ 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()
with self._lock:
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
Expand Down Expand Up @@ -232,5 +311,7 @@ def open_motion_readiness_evidence(
"OpenMotionGuardError",
"OpenMotionGuardState",
"OpenMotionPlanSummary",
"POSTURE_UNKNOWN_MESSAGE",
"TrackedPosture",
"open_motion_readiness_evidence",
]
9 changes: 7 additions & 2 deletions src/poppy_agent/hardware/factory.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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,
Expand Down
13 changes: 11 additions & 2 deletions src/poppy_agent/hardware/unitree.py
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand All @@ -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")
Expand Down
21 changes: 18 additions & 3 deletions src/poppy_agent/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -204,15 +212,21 @@ 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),
physical_execution_enabled=True,
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(", ")))
Expand Down Expand Up @@ -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,
Expand Down
Loading
Loading