Trajectory Replay & the Per-Action RPC

Contents

0 · TL;DR

1 · The data: a frame, the queue, the library

A single "replay action" is a RobotState

Every frame in a trajectory is a full RobotState dataclass servo7/robot/robot_types.py — the same object that flows everywhere else in the system. The replay-relevant fields:

@dataclass
class RobotState:
    timestamp: float
    joint_positions:  list[float] | None    # the core payload — N joint angles
    joint_velocities: list[float] | None
    joint_efforts:    list[float] | None
    gripper_position: float | None
    end_effector_pose: list[float] | None   # [x,y,z, qx,qy,qz,qw]
    robot_type: RobotType | None             # retagged on the way out (see §3)
    units: str = "radians"
    trace_id: str = ...                      # profiling
    hops: list[list] = ...                   # per-hop timestamps
    extras: dict[str, Any] = ...
    # ... + to_dict() / from_dict() for the wire

Serialized to JSON, one frame is ~1–2 KB. It already round-trips through to_dict()/from_dict(), so the whole trajectory is trivially serializable.

The queue and the library

The TrajectoryManager servo7/control/trajectory_manager.py keeps two things:

The single drain primitive — note it pops exactly one frame and is lock-guarded:

def get_action(self) -> "RobotState | None":
    with self._record_lock:
        if not self._record_queue:
            if self._loop_sequence and self._sequence_template:
                self._record_queue.extend(self._sequence_template)   # refill on loop
                ...
            else:
                return None
        return self._record_queue.popleft()

2 · Ownership: who holds the trajectory

This is the whole reason the RPC exists. The trajectory lives several layers deep inside the follower node, and the orchestrator sits in a different object graph:

        ORCHESTRATOR NODE                         FOLLOWER ROBOT NODE
        ─────────────────                         ───────────────────
   OrchestratorNode                          RobotNode
     └─ command_router : CommandRouter          └─ robot : DigitalTwinPID   (a Robot)
            │                                          └─ trajectories : TrajectoryManager
            │   no direct reference                          ├─ _record_queue : deque[RobotState]
            │   ───────✗──────────►                          ├─ _trajectories : dict[str,list]
            │                                                └─ _replay_active : bool
            │
            └──────────  bus (PubSub)  ──────────────► _on_robot_command()  ──► handler ──► twin
                 cmd.follower / reply.orch

3 · The replay lifecycle

Three phases: arm (once), drain (per frame, the hot path), stop (once).

Phase 1 — Arm (one RPC)

OrchestratorNode.trigger_replay() orchestrator_node.py:1064 sends one TRIGGER_REPLAY request. On the node, _trigger_replay() loads the named trajectory/sequence into _record_queue, calls begin_replay(), and returns the loaded frame count. The orchestrator then flips its own control_mode = TRAJECTORIES and starts a mode blend.

Phase 2 — Drain (the hot path: one RPC per frame)

A dedicated sync thread, command_dispatch_worker() orchestrator_node.py:396, spins at a fixed 30 Hz. In TRAJECTORIES mode each tick does:

# orchestrator_node.py — command_dispatch_worker(), 30 Hz loop
elif self.control_mode == ControlMode.TRAJECTORIES:
    action = self._pop_replay_action()           # <-- blocking RPC, EVERY tick
    if action is not None:
        self.smooth_and_publish_action(action, source="controller")

# _pop_replay_action(), orchestrator_node.py:695
def _pop_replay_action(self) -> "RobotState | None":
    raw = self.command_router.request(self.follower_robot_id,
                                      RobotCommand.GET_REPLAY_ACTION)   # round-trip
    if raw is None:
        return None
    return RobotState.from_dict(raw)

On the node side the handler pops one frame and retags its robot_type so the follower's converter passes it through unchanged:

# robot_node.py:446 — _get_replay_action()
twin = self._twin()
if twin is None or not getattr(twin, "_replay_active", False):
    return None
action = twin.get_action()         # popleft() one frame
if action is None:
    return None
d = action.to_dict()
d["robot_type"] = self.robot.type.value
return d

   ORCH dispatch thread (30 Hz)                       FOLLOWER node
   ────────────────────────────                       ─────────────
   tick ──► _pop_replay_action()
              request(GET_REPLAY_ACTION) ──cmd.follower──► _on_robot_command
                                                              _get_replay_action()
                                                                twin.get_action() → frame
                                                                frame.to_dict()
                          reply.orch ◄────────────────────── publish {corr, value}
              RobotState.from_dict(raw)
            smooth_and_publish_action ──action.follower──► _on_command_received
   wait 33 ms                                                robot.set_state(...)
   tick ──► (repeat, once per frame, for the whole trajectory)

Phase 3 — Stop (one RPC)

stop_replay() → STOP_REPLAY → _stop_replay() calls end_replay() and optionally clear_recording(). Sibling arming commands ride the same channel: GOTO_TRAJECTORY (smooth move to start/end), SNAP_TWIN_ANCHOR, GET_TRAJECTORY_FRAMES (scrub).

4 · How request() works — corr + reply_to + Event

The bus only does fire-and-forget pub/sub. CommandRouter.request() command_router.py:125 fakes a synchronous RPC on top of it with two tricks: a correlation id stamped on the message and echoed on the reply, and a reply_to topic the node publishes the answer on. A per-request threading.Event blocks the caller until the answer lands.

def request(self, robot_id, action, params=None, timeout=2.0) -> Any:
    self._ensure_reply_subscription()                       # subscribe reply.orch once
    corr = next(self._corr)                                 # unique id: 1,2,3,...
    slot = {"event": threading.Event(), "value": None}      # this request's mailbox
    with self._pending_lock:
        self._pending[corr] = slot                          # register by corr
    self.bus.publish(command_topic(robot_id), dumps({
        "action": act, "params": params or {},
        "reply_to": self._reply_topic, "corr": corr }))     # ask
    slot["event"].wait(timeout)                             # <-- BLOCKS here (≤ 2s)
    with self._pending_lock:
        self._pending.pop(corr, None)
    return slot["value"]                                    # answer, or None on timeout

def _on_reply(self, raw):                                   # fires on reply.orch
    msg = loads(raw)
    slot = self._pending.get(msg.get("corr"))               # find the right mailbox
    if slot is not None:
        slot["value"] = msg.get("value")                    # drop the answer in
        slot["event"].set()                                 # ring the doorbell

5 · The in-process nuance (important before "fixing the threading")

   ONE THREAD, ONE CALL STACK (InProcessBus)

   request ─► bus.publish("cmd.follower") ─► _on_robot_command ─► get_action()
                                                                 ─► bus.publish("reply.orch")
                                                                       ─► _on_reply ─► event.set()  ★
   request (cont.) ─► event.wait(2.0)   ← event ALREADY set at ★, returns instantly
                   ─► return value

Two consequences worth holding onto:

• The cost in-process is not "threading" — it's the indirection: dumps → bus → loads → handler → dumps → bus → loads, per frame, for what is morally a local deque.popleft().
• It runs on the dispatch worker thread, not the asyncio loop. command_dispatch_worker is a dedicated sync thread, so the per-frame block doesn't freeze the event loop. (The arm/stop calls are made from async context, though — those are the ones that can stall the loop for up to the 2 s timeout.)

6 · The overhead, counted

7 · The remote special case (the constraint any fix must respect)

So there are really two replay engines today:

• Local twin: orchestrator pulls frame-by-frame over the bus (the chatty path).
• Remote twin: server pushes; orchestrator's pull returns None and is ignored.

The remote path is the proof that a push model already works in this codebase — which points at the cleanest fix.

8 · Three ways to fix it

9 · Recommendation