[WIP] Controller refactor: Extract orchestration FSM out of BasePolicy

[tomasz-lewicki]

Summary

Carves the per-cycle run loop out of BasePolicy into a dedicated Controller that owns the SDK interface, input providers, rate limiter, and keyboard listener. Policies become pluggable objects conforming to PolicyProtocol (act, on_activate, on_deactivate, apply_velocity, apply_command); transitions like INIT, DAMP, and STIFF_HOLD are themselves first-class policies the Controller swaps in. DualModePolicy collapses to a thin swap object; its parallel run loop is gone.

New first-class DAMP state (StateCommand.DAMP, keyboard \, joystick B+X chord) holds the last observed q with the policy's KP/KD so the robot stays energized when the teleop handle is released.

What goes away

• DualModePolicy's parallel run loop and _dispatch_command lambda-patching
• BasePolicy._handle_{start,stop,init,damp}_* handlers (and WBT overrides)
• _shared_hardware_source guard pattern
• 5-way branching in policy_action()
• ControllerState enum write-through (replaced by controller.active.name)

Functionality Map

Function	Old abstraction	New abstraction
Owns SDK interface + input providers + rate limiter	Constructed inside BasePolicy.__init__ (_init_sdk_components, _init_communication_components, _init_input_handlers)	Controller, built once by build_default_hardware(config) in run_policy.py
Per-tick run loop (poll velocity → poll command → act → send)	BasePolicy.run()	Controller.step() driving a PolicyProtocol
Maps state → low-level command	BasePolicy.policy_action() (5-way branching on flags)	PolicyProtocol.act(ctx, state) -> Command
State machine	Three coupled flags: use_policy_action, get_ready_state, _stiff_hold_active	Controller.active: PolicyProtocol (the active policy is the state)
Lifecycle hooks on state transitions	BasePolicy._handle_{start,stop,init,damp}_* (and WBT overrides)	PolicyProtocol.on_activate(ctx) / on_deactivate(ctx)
Discrete command dispatch	BasePolicy._dispatch_command() (monkey-patched by DualModePolicy)	PolicyProtocol.apply_command(cmd) -> bool, with Controller._builtin_dispatch as fallback
Velocity command side-channel	_apply_velocity hook on subclasses	PolicyProtocol.apply_velocity(vc)
Hold last pose with KP/KD when handle released	Did not exist	DampingPolicy (policies/damping.py), entered via StateCommand.DAMP
Init-pose ramp before policy starts	Inline get_ready_state branch in policy_action()	InitRampPolicy (policies/init_ramp.py)
WBT stiff-hold pre-roll	_stiff_hold_active flag + branch in WBT's policy_action	StiffHoldPolicy (policies/stiff_hold.py)
Two-policy runtime swap (X-button)	DualModePolicy class with parallel run() loop + _shared_hardware_source guard + _dispatch_command lambda patching	A Controller with multiple entries in its policies dict; SWITCH_MODE cycles controller.active
Hardware sharing across policies	secondary._shared_hardware_source = primary guard pattern in BasePolicy.__init__	One Controller owns hardware; all policies receive it via ctx in act()
Policy class selection from config	Lived inside DualModePolicy.__init__	_select_policy_class(config) standalone helper in policies/dual_mode.py
sim2sim regression test	Did not exist	tests/sim2sim/harness.py + MujocoInterface adapter