home(calibrate=): forced end-stop referencing on the waldoctl ABC; StatusBuffer/loop_stats/homing-progress conformance

parol6/client/async_client.py

@@ -15,8 +15,9 @@
 import msgspec
 import numpy as np
 from waldoctl import RobotClient as _RobotClientABC, Shape, ShapeWorld, ToolStatus
+from msgspec.structs import asdict
 from waldoctl.shapes import shape_from_wire
-from waldoctl.status import ActionState, ActivityResult, ToolResult
+from waldoctl.status import ActionState, ActivityResult, LoopStatsResult, ToolResult
 from waldoctl.tools import ToolSpec
 
 from .. import config as cfg
@@ -657,13 +658,21 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg:
     # --------------- Motion / Control ---------------
 
     async def home(
-        self, wait: bool = False, timeout: float = 60.0, **wait_kwargs: Any
+        self,
+        wait: bool = False,
+        calibrate: bool = False,
+        timeout: float = 60.0,
+        **wait_kwargs: Any,
     ) -> int:
         """Home the robot to its home position.
 
-        Unhomed, this runs the full referencing sequence (each joint seeks
-        its limit switch, then moves to standby). Already homed, it returns
-        to standby with a normal planned, collision-checked joint move.
+        Uncalibrated (first home after power-on), this runs the full
+        referencing sequence: each joint seeks its limit switch, then the
+        robot moves to standby. Calibrated, it returns to standby with a
+        normal planned, collision-checked joint move — unless
+        ``calibrate=True``, which re-runs the referencing sequence. The
+        referencing sequence is firmware-driven and ignores the collision
+        world, so clear keep-out geometry from the joints' sweep first.
 
         Returns the command index (≥ 0) on success, -1 on failure.
 
@@ -674,9 +683,10 @@ async def home(
 
         Args:
             wait: If True, block until motion completes
+            calibrate: If True, always run the referencing sequence
             timeout: Maximum time to wait in seconds (only used when wait=True)
         """
-        index = await self._send(HomeCmd())
+        index = await self._send(HomeCmd(calibrate=calibrate))
         assert isinstance(index, int)
         if wait and index >= 0:
             ok = await self.wait_command(index, timeout=timeout)
@@ -891,7 +901,7 @@ async def status(self) -> StatusResultStruct | None:
         resp = await self._request(StatusCmd())
         return resp if isinstance(resp, StatusResultStruct) else None
 
-    async def loop_stats(self) -> LoopStatsResultStruct | None:
+    async def loop_stats(self) -> LoopStatsResult | None:
         """Fetch control-loop runtime metrics.
 
         Category: Query
@@ -900,7 +910,16 @@ async def loop_stats(self) -> LoopStatsResultStruct | None:
             stats = rbt.loop_stats()
         """
         resp = await self._request(LoopStatsCmd())
-        return resp if isinstance(resp, LoopStatsResultStruct) else None
+        if not isinstance(resp, LoopStatsResultStruct):
+            return None
+        # No fieldbus and no real-time scheduling on this backend.
+        return LoopStatsResult(
+            **asdict(resp),
+            can_frame_age_min_ticks=0,
+            can_frame_age_max_ticks=0,
+            rt_fifo=False,
+            rt_pinned=False,
+        )
 
     async def reset_loop_stats(self) -> int:
         """Reset control-loop min/max metrics and overrun count.

parol6/client/dry_run_client.py

@@ -250,7 +250,7 @@ def _snap_to_angles(self, angles_deg: list[float]) -> DryRunResult:
     def _dispatch(self, params: Any) -> DryRunResult | None:
         """Route a command struct through the trajectory planner."""
         if isinstance(params, HomeCmd):
-            if not self._planner.state.Homed_in[:6].all():
+            if params.calibrate or not self._planner.state.Homed_in[:6].all():
                 return self._snap_to_angles(HOME_ANGLES_DEG)
             # Already referenced → fall through: the planner fast-paths HOME
             # into a planned return move, so the preview renders the path.

parol6/client/sync_client.py

@@ -14,12 +14,11 @@
 from waldoctl.tools import ToolSpec
 
 from waldoctl import PingResult, ToolStatus
-from waldoctl.status import ActivityResult, ToolResult
+from waldoctl.status import ActivityResult, LoopStatsResult, ToolResult
 
 from waldoctl.types import Axis, Frame
 from ..protocol.wire import (
     EnablementResultStruct,
-    LoopStatsResultStruct,
     StatusBuffer,
     StatusResultStruct,
 )
@@ -175,20 +174,27 @@ def port(self) -> int:
 
     # ---------- motion / control ----------
 
-    def home(self, wait: bool = False, timeout: float = 60.0) -> int:
+    def home(
+        self, wait: bool = False, calibrate: bool = False, timeout: float = 60.0
+    ) -> int:
         """Home the robot to its home position.
 
-        Unhomed, this runs the full referencing sequence (each joint seeks
-        its limit switch, then moves to standby). Already homed, it returns
-        to standby with a normal planned, collision-checked joint move.
+        Uncalibrated (first home after power-on), this runs the full
+        referencing sequence: each joint seeks its limit switch, then the
+        robot moves to standby. Calibrated, it returns to standby with a
+        normal planned, collision-checked joint move — unless
+        ``calibrate=True``, which re-runs the referencing sequence. The
+        referencing sequence is firmware-driven and ignores the collision
+        world, so clear keep-out geometry from the joints' sweep first.
 
         Returns the command index (≥ 0) on success, -1 on failure.
 
         Args:
             wait: If True, block until motion completes.
+            calibrate: If True, always run the referencing sequence.
             timeout: Maximum time to wait in seconds (only used when wait=True).
         """
-        return _run(self._inner.home(wait=wait, timeout=timeout))
+        return _run(self._inner.home(wait=wait, timeout=timeout, calibrate=calibrate))
 
     def teleport(
         self,
@@ -301,11 +307,11 @@ def status(self) -> StatusResultStruct | None:
         """
         return _run(self._inner.status())
 
-    def loop_stats(self) -> LoopStatsResultStruct | None:
+    def loop_stats(self) -> LoopStatsResult | None:
         """Control loop runtime statistics.
 
         Returns:
-            LoopStatsResultStruct with loop timing metrics, or None on timeout.
+            LoopStatsResult with loop timing metrics, or None on timeout.
         """
         return _run(self._inner.loop_stats())
 

parol6/commands/basic_commands.py

@@ -77,8 +77,10 @@ class HomeState(Enum):
 class HomeCommand(MotionCommand[HomeCmd]):
     """
     A non-blocking command that tells the robot to perform its internal homing sequence.
-    Reached only while the robot is unhomed — the planner routes HOME from an
-    already-referenced robot to a planned return move instead.
+    Reached while the robot is unhomed, or on HOME(calibrate=True) from a
+    referenced robot — the planner routes plain HOME from an already-referenced
+    robot to a planned return move instead. The firmware clears the homed bits
+    when the sequence starts, which WAITING_FOR_UNHOMED relies on.
     """
 
     PARAMS_TYPE = HomeCmd
@@ -97,6 +99,7 @@ def __init__(self, p: HomeCmd):
 
     def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
         """Manages the homing command and monitors for completion using a state machine."""
+        state.homing_step = self.state.value
         if self.state == HomeState.START:
             logger.debug(
                 "  -> Sending home signal (100)... Countdown: %d",

parol6/commands/query_commands.py

@@ -176,6 +176,8 @@ def compute(self, state: "ControllerState") -> bytes:
                 p95_period_s=state.p95_period_s,
                 p99_period_s=state.p99_period_s,
                 mean_hz=mean_hz,
+                p50_period_s=state.p50_period_s,
+                p90_period_s=state.p90_period_s,
             )
         )
 

parol6/protocol/wire.py

@@ -16,6 +16,7 @@
 
 import logging
 from dataclasses import dataclass, field
+from collections.abc import Sequence
 from enum import IntEnum, auto
 from typing import Annotated, TypeAlias, Union, cast
 
@@ -500,9 +501,15 @@ def __post_init__(self) -> None:
 class HomeCmd(
     msgspec.Struct, tag=int(CmdType.HOME), array_like=True, frozen=True, gc=False
 ):
-    """HOME: [CmdType.HOME]"""
+    """HOME: [CmdType.HOME, calibrate]
 
-    pass
+    calibrate=True always runs the firmware's end-stop referencing sequence
+    to re-derive joint zero. Otherwise an already-referenced robot gets a
+    planned, collision-checked return move instead; the referencing sequence
+    itself is firmware-driven and ignores the collision world.
+    """
+
+    calibrate: bool = False
 
 
 class ResetCmd(
@@ -1009,6 +1016,8 @@ class LoopStatsResultStruct(
     p95_period_s: float
     p99_period_s: float
     mean_hz: float
+    p50_period_s: float = 0.0
+    p90_period_s: float = 0.0
 
 
 class ToolResultStruct(
@@ -1322,6 +1331,9 @@ def pack_response(result: Response) -> bytes:
     return _encoder.encode(ResponseMsg(result))
 
 
+_NO_JOINTS_HOMED: tuple[int, ...] = (0, 0, 0, 0, 0, 0)
+
+
 def pack_status(
     pose: np.ndarray,
     angles: np.ndarray,
@@ -1347,6 +1359,9 @@ def pack_status(
     scene_epoch: int = 0,
     accepted_index: int = -1,
     homed: bool = True,
+    enabled: bool = True,
+    homing_step: int = 0,
+    joints_homed: Sequence[int] = _NO_JOINTS_HOMED,
 ) -> bytes:
     """Pack a status broadcast message.
 
@@ -1391,6 +1406,9 @@ def pack_status(
             scene_epoch,
             accepted_index,
             homed,
+            enabled,
+            homing_step,
+            joints_homed,
         ),
         option=ormsgpack.OPT_SERIALIZE_NUMPY,
     )
@@ -1438,6 +1456,25 @@ class StatusBuffer:
     # All joints homed. True from producers that predate the field (permissive:
     # old servers gate nothing, so claiming unhomed would be a false alarm).
     homed: bool = True
+    # Whether the controller accepts motion (False while disabled, e.g. the
+    # e-stop latch). True from producers that predate the field.
+    enabled: bool = True
+    # Remaining waldoctl StatusBuffer Protocol members. parol6 has no torque
+    # sensing, fieldbus, or warning source, so these hold their empty values.
+    torques: np.ndarray = field(default_factory=lambda: np.zeros(6, dtype=np.float64))
+    torques_ext: np.ndarray = field(
+        default_factory=lambda: np.zeros(6, dtype=np.float64)
+    )
+    warnings: list[tuple] = field(default_factory=list)
+    link_health: dict = field(default_factory=dict)
+    # Firmware referencing progress in waldoctl's shape: active, sequence_step,
+    # and one (HomingJointState, HomingPhase) pair per joint; empty while idle.
+    homing: dict = field(default_factory=dict)
+    # Last decoded (step, per-joint bits) so the view is rebuilt only on change.
+    _homing_step: int = field(default=0, init=False, repr=False, compare=False)
+    _homing_bits: list[int] = field(
+        default_factory=lambda: [0] * 6, init=False, repr=False, compare=False
+    )
     # Built once in __post_init__, aliasing the two enable arrays the decoder
     # mutates in place.
     cart_en: dict[str, np.ndarray] = field(init=False, repr=False, compare=False)
@@ -1448,6 +1485,15 @@ def __post_init__(self) -> None:
             "TRF": self.cart_en_trf,
         }
 
+    @property
+    def freedrive(self) -> bool:
+        """PAROL6 steppers cannot be back-driven."""
+        return False
+
+    @property
+    def mode(self) -> ActionState:
+        return self.action_state
+
     def copy(self) -> "StatusBuffer":
         """Return a deep copy with all arrays copied."""
         ts = self.tool_status
@@ -1484,9 +1530,48 @@ def copy(self) -> "StatusBuffer":
             scene_epoch=self.scene_epoch,
             accepted_index=self.accepted_index,
             homed=self.homed,
+            enabled=self.enabled,
+            torques=self.torques.copy(),
+            torques_ext=self.torques_ext.copy(),
+            warnings=list(self.warnings),
+            link_health=dict(self.link_health),
+            homing=dict(self.homing),
         )
 
 
+class HomingJointState(IntEnum):
+    """Per-joint firmware referencing state (StatusBuffer.homing["joints"])."""
+
+    SEEKING = 0
+    HOMED = 1
+
+
+class HomingPhase(IntEnum):
+    """PAROL6 firmware exposes no sub-phase; kept for the (state, phase) shape."""
+
+    NONE = 0
+
+
+_HOMING_JOINT_VIEW = (
+    (HomingJointState.SEEKING, HomingPhase.NONE),
+    (HomingJointState.HOMED, HomingPhase.NONE),
+)
+
+
+def _apply_homing_progress(buf: StatusBuffer, step: int, bits: list[int]) -> None:
+    """Rebuild the homing view only when the step or per-joint bits change."""
+    if step == buf._homing_step and bits == buf._homing_bits:
+        return
+    buf._homing_step = step
+    buf._homing_bits[:] = bits
+    if step == 0:
+        buf.homing.clear()
+        return
+    buf.homing["active"] = True
+    buf.homing["sequence_step"] = step
+    buf.homing["joints"] = [_HOMING_JOINT_VIEW[b] for b in bits]
+
+
 def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
     """Zero-allocation decode of STATUS message into preallocated buffer.
 
@@ -1496,7 +1581,7 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
                      error, queued_segments, queued_duration, action_params,
                      tool_status_tuple, tcp_speed, simulator_active,
                      collision_active, collision_pairs, scene_epoch,
-                     accepted_index, homed]
+                     accepted_index, homed, enabled, homing_step, joints_homed]
 
     Args:
         data: Raw msgpack bytes
@@ -1568,6 +1653,9 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
             buf.scene_epoch = int(msg[22])
         buf.accepted_index = int(msg[23]) if len(msg) > 23 else -1
         buf.homed = bool(msg[24]) if len(msg) > 24 else True
+        buf.enabled = bool(msg[25]) if len(msg) > 25 else True
+        if len(msg) > 27:
+            _apply_homing_progress(buf, int(msg[26]), msg[27])
 
         return True
     except Exception as e:
@@ -1799,6 +1887,8 @@ def unpack_rx_frame_into(
     "MoveSCmd",
     "MovePCmd",
     "HomeCmd",
+    "HomingJointState",
+    "HomingPhase",
     "CheckpointCmd",
     # Command structs — streaming (servo/jog)
     "ServoJCmd",

parol6/server/controller.py

@@ -358,6 +358,8 @@ def _handle_estop(self, state: ControllerState) -> None:
                 self._executor.clear_queue("E-Stop activated")
                 state.Command_out = CommandCode.DISABLE
                 state.Speed_out.fill(0)
+                state.enabled = False
+                state.disabled_reason = "E-STOP pressed"
                 state.error = make_error(ErrorCode.SYS_ESTOP_ACTIVE)
         elif state.InOut_in[4] == 1:  # E-stop released
             if self.estop_active:
@@ -464,6 +466,8 @@ def _sync_timer_metrics(self, state: ControllerState) -> None:
             state.max_period_s = m.max_period_s
             state.p95_period_s = m.p95_period_s
             state.p99_period_s = m.p99_period_s
+            state.p90_period_s = m.p90_period_s
+            state.p50_period_s = m.p50_period_s
 
     def _log_periodic_status(self, state: ControllerState) -> None:
         """Log performance metrics every 3 seconds."""