From 2935fe50f28c3ba9c488238da4defd5c0bfd9e5e Mon Sep 17 00:00:00 2001 From: Bryan J Miller Date: Wed, 1 Jul 2026 18:36:41 -0700 Subject: [PATCH] feat(coordinator): add Swarm (PIBT+LNS+BVC) planner, smooth trajectory playback, and Play Chess mode A complete, collision-free multi-robot planner plus the coordinator features to use it. Additive: existing planners and the wave-dispatch flow are unchanged when these are not selected. Swarm planner (planning/swarm/, swarm_planner.py, base_planner.produces_trajectory): self-contained PIBT (priority inheritance + backtracking) + MAPF-LNS flowtime refinement + Buffered Voronoi Cell execution. Complete by construction where EnhancedConflictPlanner can get stuck. Registered as 'Swarm (PIBT+LNS)'. plan_moves forward-simulates the pipeline and emits each piece's collision-free sampled trajectory as wave-ordered MoveCommands, with optimal interchangeable-piece assignment and in-bounds waypoint clamping. Smooth trajectory playback (simulation/simulator.py + main_window/path_planning wiring): play_trajectory() advances all pieces in lockstep along the synchronized trajectory (continuous, arc-like, collision-free) then rotates each to its commanded heading. Used automatically for trajectory planners; the discrete wave dispatch is untouched for the others. Play Chess mode (gui/tabs/play_chess_tab.py + chessboard_widget.py): a tab where you click a piece to see legal moves (via python-chess) and click a legal square to move it with the selected planner - captures to graveyard, castling, en passant, and auto-queen promotion via the spare queen. Hot-seat. Board size set to 57.15 mm (2.25 in tournament square): standard sizing, and the clearance the buffered-Voronoi motion needs between ~31 mm pieces to stay collision-free and efficient (see planning/swarm/README.md and config.BOARD). Adds python-chess dependency (requirements.txt). Self-tests: planning/swarm/_selftest*.py, gui/tabs/_selftest_chess.py. --- firmware/MiniBot_Coordinator/config.py | 22 +- .../gui/chessboard_widget.py | 91 +++- .../MiniBot_Coordinator/gui/main_window.py | 57 ++- .../gui/tabs/_selftest_chess.py | 148 +++++++ .../gui/tabs/path_planning_tab.py | 41 +- .../gui/tabs/play_chess_tab.py | 387 ++++++++++++++++++ .../planning/base_planner.py | 10 + .../planning/swarm/README.md | 62 +++ .../planning/swarm/__init__.py | 8 + .../planning/swarm/_selftest.py | 74 ++++ .../planning/swarm/_selftest_playback.py | 115 ++++++ .../planning/swarm/assignment.py | 44 ++ .../planning/swarm/drive.py | 219 ++++++++++ .../planning/swarm/executor.py | 133 ++++++ .../planning/swarm/grid.py | 64 +++ .../MiniBot_Coordinator/planning/swarm/lns.py | 209 ++++++++++ .../planning/swarm/pibt.py | 131 ++++++ .../planning/swarm_planner.py | 206 ++++++++++ firmware/MiniBot_Coordinator/requirements.txt | 3 + .../simulation/simulator.py | 169 +++++++- 20 files changed, 2161 insertions(+), 32 deletions(-) create mode 100644 firmware/MiniBot_Coordinator/gui/tabs/_selftest_chess.py create mode 100644 firmware/MiniBot_Coordinator/gui/tabs/play_chess_tab.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/README.md create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/__init__.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/_selftest.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/_selftest_playback.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/assignment.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/drive.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/executor.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/grid.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/lns.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm/pibt.py create mode 100644 firmware/MiniBot_Coordinator/planning/swarm_planner.py create mode 100644 firmware/MiniBot_Coordinator/requirements.txt diff --git a/firmware/MiniBot_Coordinator/config.py b/firmware/MiniBot_Coordinator/config.py index 432dec1..8d6cc7d 100644 --- a/firmware/MiniBot_Coordinator/config.py +++ b/firmware/MiniBot_Coordinator/config.py @@ -9,9 +9,20 @@ # BOARD — physical geometry (all units in mm unless noted) # --------------------------------------------------------------------------- class BOARD: - SQUARE_SIZE_MM = 50 # one chess square side length + # Square size = 2.25 in (57.15 mm), the FIDE/USCF regulation tournament square. + # This is a deliberate step up from a tighter 50 mm square, for two reasons: + # 1. Standard tournament sizing (real 2.25 in boards and pieces). + # 2. Collision-free motion headroom. A MiniBot is ~31 mm across, so at 50 mm + # the gap between piece bodies on adjacent squares is only ~19 mm; the + # buffered-Voronoi routing in the swarm planner then has almost no room to + # slip a piece past its neighbours and deadlocks/detours far more often. + # At 57.15 mm the gap grows to ~26 mm, which is what lets the planner keep + # resets and dense piece shuffles collision-free and efficient. Measured + # on scattered-reset benchmarks the larger square markedly improves + # completion and reduces total travel. See planning/swarm/README.md. + SQUARE_SIZE_MM = 57.15 # one chess square side length (2.25 in tournament size) NUM_SQUARES = 8 # 8×8 board - PLAYING_AREA_MM = SQUARE_SIZE_MM * NUM_SQUARES # 400 mm + PLAYING_AREA_MM = SQUARE_SIZE_MM * NUM_SQUARES # 457.2 mm BORDER_TOP_MM = 10 BORDER_BOTTOM_MM = 10 @@ -19,8 +30,8 @@ class BOARD: BORDER_RIGHT_MM = 100 # Total canvas in mm (logical coordinate space) - CANVAS_WIDTH_MM = PLAYING_AREA_MM + BORDER_LEFT_MM + BORDER_RIGHT_MM # 650 - CANVAS_HEIGHT_MM = PLAYING_AREA_MM + BORDER_TOP_MM + BORDER_BOTTOM_MM # 450 + CANVAS_WIDTH_MM = PLAYING_AREA_MM + BORDER_LEFT_MM + BORDER_RIGHT_MM # 657.2 + CANVAS_HEIGHT_MM = PLAYING_AREA_MM + BORDER_TOP_MM + BORDER_BOTTOM_MM # 477.2 # Coordinate origin: (0,0) = bottom-left corner of the playing area # X increases → right, Y increases → up (standard math orientation) @@ -62,7 +73,7 @@ class PIECES: # Origin (0,0) is bottom-left corner of the PLAYING AREA. # Squares are labeled a-h (columns, x) and 1-8 (rows, y). # Square center col c, row r → x = (c-0.5)*SQUARE_SIZE, y = (r-0.5)*SQUARE_SIZE - _S = 50 # shorthand for SQUARE_SIZE_MM + _S = BOARD.SQUARE_SIZE_MM # shorthand; tracks BOARD so the two never drift # White pieces (IDs 0x01–0x11) # Standard back rank: R N B Q K B N R (cols 1–8) @@ -299,6 +310,7 @@ class PLANNING: "planning.enhanced_conflict_planner", "EnhancedConflictPlanner", ), + "Swarm (PIBT+LNS)": ("planning.swarm_planner", "SwarmPlanner"), "Direct (debug only)": ("planning.direct_planner", "DirectPlanner"), } diff --git a/firmware/MiniBot_Coordinator/gui/chessboard_widget.py b/firmware/MiniBot_Coordinator/gui/chessboard_widget.py index 8e897c5..9ed497b 100644 --- a/firmware/MiniBot_Coordinator/gui/chessboard_widget.py +++ b/firmware/MiniBot_Coordinator/gui/chessboard_widget.py @@ -2,9 +2,9 @@ gui/chessboard_widget.py — MiniBot Chess Swarm Coordinator Custom QWidget that renders the physical chessboard environment: - - 125mm left/right borders, 25mm top/bottom borders (all drawn to scale) + - Border margins around the play area (sizes from config.BOARD) - Hard outer outline rectangle - - 8×8 checkerboard (50mm squares) + - 8×8 checkerboard (square size = config.BOARD.SQUARE_SIZE_MM, 57.15 mm) - 34 robot pieces as labeled circles with orientation markers - Selected-piece highlight and target-position indicator @@ -46,9 +46,9 @@ class ChessBoardWidget(QWidget): """Scaled chessboard canvas widget. - The logical coordinate space is (0,0) bottom-left of the playing area, - 650 mm wide × 450 mm tall. The canvas scales to fill the widget size - while preserving the aspect ratio. + The logical coordinate space is (0,0) bottom-left of the playing area; the + canvas spans the playing area plus its border margins (all from config.BOARD) + and scales to fill the widget size while preserving the aspect ratio. """ piece_selected = pyqtSignal(int) # piece_id @@ -58,14 +58,15 @@ class ChessBoardWidget(QWidget): target_queued = pyqtSignal( int, float, float ) # piece_id, x_mm, y_mm (right-click → immediate dispatch) + square_clicked = pyqtSignal(int, int) # file(0-7), rank(0-7); only in chess mode # ------------------------------------------------------------------ # Logical canvas constants (mm) # ------------------------------------------------------------------ - _LW = BOARD.CANVAS_WIDTH_MM # 650 - _LH = BOARD.CANVAS_HEIGHT_MM # 450 - _OX = BOARD.BORDER_LEFT_MM # 125 offset of playing area origin - _OY = BOARD.BORDER_BOTTOM_MM # 25 + _LW = BOARD.CANVAS_WIDTH_MM # full canvas width (playing area + L/R borders) + _LH = BOARD.CANVAS_HEIGHT_MM # full canvas height (playing area + T/B borders) + _OX = BOARD.BORDER_LEFT_MM # x offset of playing-area origin within the canvas + _OY = BOARD.BORDER_BOTTOM_MM # y offset of playing-area origin within the canvas def __init__( self, board_state: BoardState, parent: Optional[QWidget] = None @@ -79,6 +80,10 @@ def __init__( self._show_electromagnets: bool = False # Plan visualization: list of (x0, y0, x1, y1, wave_idx, total_waves) self._plan_arrows: List[Tuple[float, float, float, float, int, int]] = [] + # Chess mode: route clicks as board squares and highlight legal moves. + self._chess_mode: bool = False + self._legal_squares: Set[Tuple[int, int]] = set() # (file, rank) dots + self._sel_square: Optional[Tuple[int, int]] = None # (file, rank) ring self.setMinimumSize( int(BOARD.CANVAS_WIDTH_MM * GUI.SCALE_FACTOR), @@ -162,6 +167,32 @@ def set_target(self, piece_id: int, x_mm: float, y_mm: float) -> None: self._target = (x_mm, y_mm) self.update() + def set_chess_mode(self, on: bool) -> None: + """Route board clicks as chess squares (Play Chess tab) when on.""" + self._chess_mode = on + self._selected_id = None + self._target = None + if not on: + self._legal_squares = set() + self._sel_square = None + self.update() + + def set_legal_highlights( + self, + squares: Set[Tuple[int, int]], + selected: Optional[Tuple[int, int]] = None, + ) -> None: + """Highlight legal destination squares (dots) and the selected one (ring).""" + self._legal_squares = set(squares) + self._sel_square = selected + self.update() + + def clear_legal_highlights(self) -> None: + """Clear all chess move highlights.""" + self._legal_squares = set() + self._sel_square = None + self.update() + # ------------------------------------------------------------------ # Mouse interaction # ------------------------------------------------------------------ @@ -169,6 +200,15 @@ def set_target(self, piece_id: int, x_mm: float, y_mm: float) -> None: def mousePressEvent(self, event: QMouseEvent) -> None: self.setFocus() board_x, board_y = self._widget_to_mm(event.position()) + + # ── Chess mode: emit the clicked board square, bypass planning clicks ── + if self._chess_mode: + if event.button() == Qt.MouseButton.LeftButton: + sq = self._square_at_mm(board_x, board_y) + if sq is not None: + self.square_clicked.emit(sq[0], sq[1]) + return + clicked_pid = self._piece_at(board_x, board_y) btn = event.button() @@ -258,6 +298,7 @@ def paintEvent(self, event: QPaintEvent) -> None: self._draw_background(painter) self._draw_outer_outline(painter) self._draw_grid(painter) + self._draw_chess_highlights(painter) self._draw_plan_paths(painter) self._draw_electromagnets(painter) self._draw_target_indicator(painter) @@ -324,6 +365,27 @@ def _draw_grid(self, p: QPainter) -> None: _, cy = self._pa_to_canvas(0, float(row * s)) p.drawLine(QPointF(cx0, cy), QPointF(cx1, cy)) + def _draw_chess_highlights(self, p: QPainter) -> None: + """Draw legal-move dots and a ring on the selected square (chess mode).""" + if not self._chess_mode: + return + s = BOARD.SQUARE_SIZE_MM + if self._sel_square is not None: + f, r = self._sel_square + cx, cy = self._pa_to_canvas((f + 0.5) * s, (r + 0.5) * s) + pen = QPen(QColor(GUI.SELECTED_HIGHLIGHT_COLOR)) + pen.setWidthF(2.5) + p.setPen(pen) + p.setBrush(Qt.BrushStyle.NoBrush) + half = s / 2.0 - 2.0 + p.drawRect(QRectF(cx - half, cy - half, 2 * half, 2 * half)) + dot_r = s * 0.16 + p.setPen(Qt.PenStyle.NoPen) + p.setBrush(QBrush(QColor(0, 200, 90, 150))) + for f, r in self._legal_squares: + cx, cy = self._pa_to_canvas((f + 0.5) * s, (r + 0.5) * s) + p.drawEllipse(QPointF(cx, cy), dot_r, dot_r) + def _draw_plan_paths(self, p: QPainter) -> None: """Draw colored wave arrows for planned moves.""" if not self._plan_arrows: @@ -538,6 +600,17 @@ def _piece_at(self, pa_x: float, pa_y: float) -> Optional[int]: return closest_id + def _square_at_mm(self, x_mm: float, y_mm: float) -> Optional[Tuple[int, int]]: + """Playing-area mm → (file, rank) on the 8x8 board, or None if off-board.""" + s = BOARD.SQUARE_SIZE_MM + if x_mm < 0 or y_mm < 0: + return None + f = int(x_mm // s) + r = int(y_mm // s) + if 0 <= f < BOARD.NUM_SQUARES and 0 <= r < BOARD.NUM_SQUARES: + return (f, r) + return None + # ------------------------------------------------------------------ # Initialise scale fields so _widget_to_mm works before first paint # ------------------------------------------------------------------ diff --git a/firmware/MiniBot_Coordinator/gui/main_window.py b/firmware/MiniBot_Coordinator/gui/main_window.py index 22c5a6f..8630b3b 100644 --- a/firmware/MiniBot_Coordinator/gui/main_window.py +++ b/firmware/MiniBot_Coordinator/gui/main_window.py @@ -55,6 +55,7 @@ from gui.tabs.system_control_tab import SystemControlTab from gui.tabs.position_tracker_tab import PositionTrackerTab from gui.tabs.command_looper_tab import CommandLooperTab +from gui.tabs.play_chess_tab import PlayChessTab # --------------------------------------------------------------------------- # Application-wide dark theme stylesheet @@ -418,8 +419,10 @@ def _build_ui(self) -> None: self._sys_tab = SystemControlTab(self) self._track_tab = PositionTrackerTab(self._board, self) self._looper_tab = CommandLooperTab(self) + self._chess_tab = PlayChessTab(self._board, self._board_widget, self) self._tabs.addTab(self._path_tab, "Path Planning") + self._tabs.addTab(self._chess_tab, "Play Chess") self._tabs.addTab(self._debug_tab, "Debug") self._tabs.addTab(self._sys_tab, "System Control") self._tabs.addTab(self._track_tab, "Position Tracker") @@ -442,6 +445,12 @@ def _wire_signals(self) -> None: # Path planning → send (serial or sim depending on mode) self._path_tab.send_commands.connect(self._on_send_move_commands) + # Play Chess: board square clicks route here in chess mode; moves dispatch + # through the same handler so they animate with the selected planner. + self._board_widget.square_clicked.connect(self._chess_tab.on_square_clicked) + self._chess_tab.send_commands.connect(self._on_send_move_commands) + self._chess_tab.status_log.connect(self._debug_tab.on_sim_log) + # Path planning → board visualization self._path_tab.plan_visualized.connect(self._on_plan_visualized) @@ -545,20 +554,58 @@ def _on_looper_request_move( else: self._send_serial_move(cmd) - @pyqtSlot(list) - def _on_send_move_commands(self, commands: List[MoveCommand]) -> None: - """Dispatch commands in sequence-number waves. + @pyqtSlot(list, bool) + def _on_send_move_commands( + self, commands: List[MoveCommand], is_trajectory: bool = False + ) -> None: + """Dispatch commands to the simulator or serial link. - All commands sharing the same sequence_num are sent in parallel as one - wave. Later waves are delayed until the active wave is considered done. + A continuous-trajectory plan in simulator mode is played back smoothly + (all pieces advance in lockstep). Everything else uses the sequence-number + wave dispatcher: commands sharing a sequence_num go out together, and later + waves wait for the active wave to finish. """ if not commands: return self._reset_wave_dispatch() + if is_trajectory and self._sim_mode: + self._play_trajectory_sim(commands) + return self._pending_waves = self._group_by_sequence(commands) self._dispatch_next_wave() + def _play_trajectory_sim(self, commands: List[MoveCommand]) -> None: + """Reconstruct per-piece time-synced paths and play them continuously. + + Each piece's path is its start position followed by, for every wave, the + target it was commanded that wave or (forward-filled) its previous + position. The final heading is the last non-None target_theta commanded. + """ + self._simulator.speed_mm_s = self._debug_tab.simulator_speed_mm_s + waves: Dict[int, Dict[int, MoveCommand]] = {} + for cmd in commands: + waves.setdefault(cmd.sequence_num, {})[cmd.piece_id] = cmd + wave_nums = sorted(waves) + piece_ids = {cmd.piece_id for cmd in commands} + + paths: Dict[int, List[Tuple[float, float]]] = {} + final_theta: Dict[int, float] = {} + for pid in piece_ids: + piece = self._board.get_piece(pid) + prev = (piece.x_mm, piece.y_mm) if piece is not None else (0.0, 0.0) + seq = [prev] + for wnum in wave_nums: + cmd = waves[wnum].get(pid) + if cmd is not None: + prev = (cmd.target_x_mm, cmd.target_y_mm) + if cmd.target_theta is not None: + final_theta[pid] = cmd.target_theta + seq.append(prev) + paths[pid] = seq + + self._simulator.play_trajectory(paths, final_theta) + @pyqtSlot(bytes) def _on_debug_send_raw(self, data: bytes) -> None: """Route a raw bytes send from the debug tab to serial or simulator.""" diff --git a/firmware/MiniBot_Coordinator/gui/tabs/_selftest_chess.py b/firmware/MiniBot_Coordinator/gui/tabs/_selftest_chess.py new file mode 100644 index 0000000..dd3edd8 --- /dev/null +++ b/firmware/MiniBot_Coordinator/gui/tabs/_selftest_chess.py @@ -0,0 +1,148 @@ +"""Headless validation of the Play Chess tab logic. + +Run from the coordinator root: python gui/tabs/_selftest_chess.py +Exercises normal move, capture, castling, en passant, and promotion without +opening the GUI, checking the computed robot targets, graveyard/spare handling, +and square->id consistency with python-chess. +""" +import math +import os +import sys + +sys.path.insert(0, os.getcwd()) + +import chess # noqa: E402 +from PyQt6.QtWidgets import QApplication # noqa: E402 + +from config import BOARD, PIECES # noqa: E402 +from models.piece import BoardState # noqa: E402 +from gui.tabs.play_chess_tab import PlayChessTab # noqa: E402 + +S = float(BOARD.SQUARE_SIZE_MM) + + +def center(sq): + return ((chess.square_file(sq) + 0.5) * S, (chess.square_rank(sq) + 0.5) * S) + + +def close(a, b): + return math.hypot(a[0] - b[0], a[1] - b[1]) < 0.01 + + +def fresh(): + board = BoardState() + tab = PlayChessTab(board, None) + tab.new_game() + return board, tab + + +def play(tab, uci): + """Apply a uci move via the tab, returning (targets, mover_id).""" + move = chess.Move.from_uci(uci) + assert move in tab._chess.legal_moves, f"{uci} not legal" + mover_id = tab._sq_to_id[move.from_square] + targets, _san, _w, _n = tab._apply_move(move) + return targets, mover_id + + +def assert_consistent(tab): + """Every occupied chess square has exactly one physical id, and vice versa.""" + assert set(tab._sq_to_id) == set(tab._chess.piece_map()), \ + "square->id map diverged from python-chess" + + +def test_normal_and_capture(): + _board, tab = fresh() + t, mv = play(tab, "e2e4") # normal + assert set(t) == {mv} and close(t[mv], center(chess.E4)) + assert_consistent(tab) + play(tab, "d7d5") + cap_id = tab._sq_to_id[chess.D5] # black d-pawn about to be captured + t, mv = play(tab, "e4d5") # capture + assert close(t[mv], center(chess.D5)) + # Interchangeable graveyard: the captured piece goes to SOME valid black + # graveyard slot (assigned farthest-first), not one fixed per-id square. + black_slots = [(float(x), float(y)) for pid, (x, y, _t) + in PIECES.GRAVEYARD_POSITIONS.items() if pid >= 0x12] + assert any(close(t[cap_id], s) for s in black_slots), \ + "captured piece not sent to a graveyard slot" + assert_consistent(tab) + print(" normal + capture: OK") + + +def test_castling(): + _board, tab = fresh() + for uci in ("e2e4", "e7e5", "g1f3", "b8c6", "f1c4", "g8f6"): + play(tab, uci) + rook_id = tab._sq_to_id[chess.H1] + t, king_id = play(tab, "e1g1") # king-side castle + assert close(t[king_id], center(chess.G1)), "king target wrong" + assert close(t[rook_id], center(chess.F1)), "rook not moved to f1" + assert_consistent(tab) + print(" castling (king + rook): OK") + + +def test_en_passant(): + _board, tab = fresh() + for uci in ("e2e4", "e7e6", "e4e5", "d7d5"): + play(tab, uci) + ep_pawn = tab._sq_to_id[chess.D5] # black pawn that will be captured e.p. + move = chess.Move.from_uci("e5d6") + assert tab._chess.is_en_passant(move) + mover = tab._sq_to_id[chess.E5] + t, _san, _w, _n = tab._apply_move(move) + assert close(t[mover], center(chess.D6)), "e.p. mover target wrong" + black_slots = [(float(x), float(y)) for pid, (x, y, _t) + in PIECES.GRAVEYARD_POSITIONS.items() if pid >= 0x12] + assert any(close(t[ep_pawn], s) for s in black_slots), \ + "e.p. captured pawn not graveyarded" + assert chess.D5 not in tab._sq_to_id + assert_consistent(tab) + print(" en passant: OK") + + +def test_promotion(): + board = BoardState() + tab = PlayChessTab(board, None) + tab._chess = chess.Board("4k3/P7/8/8/8/8/8/4K3 w - - 0 1") + tab._spare = {"white": 0x11, "black": 0x22} + tab._sq_to_id = {chess.A7: 0x01, chess.E1: 0x0D, chess.E8: 0x1E} + for sq, pid in tab._sq_to_id.items(): + board.get_piece(pid).position_mm = tab._square_center(sq) + move = chess.Move.from_uci("a7a8q") + assert move in tab._chess.legal_moves + t, _san, _w, _n = tab._apply_move(move) + white_slots = [(float(x), float(y)) for pid, (x, y, _t) + in PIECES.GRAVEYARD_POSITIONS.items() if pid < 0x12] + assert any(close(t[0x01], s) for s in white_slots), "promoted pawn not graveyarded" + assert close(t[0x11], center(chess.A8)), "spare queen not sent to a8" + assert tab._sq_to_id[chess.A8] == 0x11 and tab._spare["white"] is None + assert_consistent(tab) + print(" promotion (auto-queen via spare): OK") + + +def test_dispatch_emits_commands(): + _board, tab = fresh() + got = {} + tab.send_commands.connect(lambda cmds, traj: got.update(n=len(cmds), traj=traj)) + move = chess.Move.from_uci("e2e4") + targets, _san, _w, _n = tab._apply_move(move) + tab._dispatch(targets) + assert got.get("n", 0) > 0, "no commands emitted" + print(f" dispatch via selected planner: {got['n']} commands, trajectory={got['traj']}") + + +def main(): + app = QApplication.instance() or QApplication([]) + test_normal_and_capture() + test_castling() + test_en_passant() + test_promotion() + test_dispatch_emits_commands() + from gui.main_window import MainWindow # noqa: F401 + print("PASS: all chess-mode checks green; MainWindow + PlayChessTab import clean.") + del app + + +if __name__ == "__main__": + main() diff --git a/firmware/MiniBot_Coordinator/gui/tabs/path_planning_tab.py b/firmware/MiniBot_Coordinator/gui/tabs/path_planning_tab.py index 4984809..916285d 100644 --- a/firmware/MiniBot_Coordinator/gui/tabs/path_planning_tab.py +++ b/firmware/MiniBot_Coordinator/gui/tabs/path_planning_tab.py @@ -97,7 +97,7 @@ class PathPlanningTab(QWidget): plan_visualized(list, dict) — emitted after planning to update board arrows """ - send_commands = pyqtSignal(list) # list[MoveCommand] + send_commands = pyqtSignal(list, bool) # list[MoveCommand], is_trajectory plan_visualized = pyqtSignal(list, dict) # commands, initial_positions planning_log = pyqtSignal(str) # debug message → debug tab log @@ -107,6 +107,9 @@ def __init__( super().__init__(parent) self._board = board_state self._move_queue: List[MoveCommand] = [] + # True only while every queued plan came from a continuous-trajectory + # planner, so the coordinator can play it back smoothly. + self._queue_is_trajectory: bool = False self._selected_id: Optional[int] = None # from chessboard click self._viz_enabled: bool = True # Snapshot of positions at the time the last plan was generated @@ -304,9 +307,11 @@ def _on_plan_move(self) -> None: if piece is None: return - from planning.enhanced_conflict_planner import EnhancedConflictPlanner - - planner = EnhancedConflictPlanner() + planner = self._get_planner() + # A single manual move must move the exact piece the user clicked, so turn + # off same-type interchangeable reassignment for this plan if supported. + if hasattr(planner, "interchangeable"): + planner.interchangeable = False positions = {p.piece_id: (p.x_mm, p.y_mm) for p in self._board.active_pieces()} # All bystanders return to their current position; target piece gets its new goal. targets = dict(positions) @@ -318,7 +323,8 @@ def _validator(pid: int, tx: float, ty: float) -> bool: self._viz_positions = {} # fresh snapshot for new plan commands = planner.plan_moves(positions, targets, validator=_validator) - self._enqueue(commands, snap_positions=dict(positions)) + self._enqueue(commands, snap_positions=dict(positions), + trajectory=planner.produces_trajectory) def _on_return_home(self) -> None: from config import PIECES as P, PLANNING as PL @@ -341,7 +347,8 @@ def _on_return_home(self) -> None: for cmd in commands: cmd.duration_ms = PL.HOME_MOVE_DURATION_MS self._viz_positions = {} # fresh snapshot for new plan - self._enqueue(commands, snap_positions=dict(positions)) + self._enqueue(commands, snap_positions=dict(positions), + trajectory=planner.produces_trajectory) def _on_return_graveyard(self) -> None: from config import PIECES as P, PLANNING as PL @@ -364,7 +371,8 @@ def _on_return_graveyard(self) -> None: for cmd in commands: cmd.duration_ms = PL.HOME_MOVE_DURATION_MS self._viz_positions = {} # fresh snapshot for new plan - self._enqueue(commands, snap_positions=dict(positions)) + self._enqueue(commands, snap_positions=dict(positions), + trajectory=planner.produces_trajectory) def _on_fen_positions(self) -> None: from config import PIECES as P, PLANNING as PL @@ -437,6 +445,7 @@ def rankFileToMM(x): def _on_clear_queue(self) -> None: self._move_queue.clear() + self._queue_is_trajectory = False self._queue_list.clear() self.plan_visualized.emit([], {}) @@ -452,7 +461,7 @@ def _on_show_paths_toggled(self, checked: bool) -> None: def _on_send(self) -> None: if self._move_queue: - self.send_commands.emit(list(self._move_queue)) + self.send_commands.emit(list(self._move_queue), self._queue_is_trajectory) self._on_clear_queue() # ------------------------------------------------------------------ @@ -499,6 +508,17 @@ def _exhaustive_assignment_plan( """ from config import PIECES + # Trajectory planners (e.g. Swarm) already choose the optimal + # interchangeable assignment internally and score it on their own motion, + # not EnhancedConflictPlanner's. The exhaustive ECP-based search is both + # redundant and incompatible with their plan_moves signature, so defer. + if getattr(planner, "produces_trajectory", False): + self.planning_log.emit( + "[AssignOpt] Planner optimizes assignment internally — " + "running its plan directly." + ) + return planner.plan_moves(positions, targets) + # ---- Build interchangeable groups -------------------------------- groups: Dict[tuple, List[int]] = {} for pid in positions: @@ -621,6 +641,7 @@ def _enqueue( self, commands: List[MoveCommand], snap_positions: Optional[Dict[int, Tuple[float, float]]] = None, + trajectory: bool = False, ) -> None: if snap_positions and not self._viz_positions: self._viz_positions = dict(snap_positions) @@ -629,6 +650,10 @@ def _enqueue( for pid, pos in snap_positions.items(): if pid not in self._viz_positions: self._viz_positions[pid] = pos + # The queue is a trajectory only if every plan added to it is one. + self._queue_is_trajectory = ( + trajectory if not self._move_queue else self._queue_is_trajectory and trajectory + ) self._move_queue.extend(commands) for cmd in commands: label = ( diff --git a/firmware/MiniBot_Coordinator/gui/tabs/play_chess_tab.py b/firmware/MiniBot_Coordinator/gui/tabs/play_chess_tab.py new file mode 100644 index 0000000..b007f32 --- /dev/null +++ b/firmware/MiniBot_Coordinator/gui/tabs/play_chess_tab.py @@ -0,0 +1,387 @@ +""" +gui/tabs/play_chess_tab.py — MiniBot Chess Swarm Coordinator + +Play Chess tab: hot-seat legal chess played on the physical board. python-chess +provides all rules (legal moves, check/mate/stalemate, castling, en passant, +promotion). Clicking a piece highlights its legal destination squares; clicking a +legal square pushes the move and drives the robots there with the selected +planner (captures go to the graveyard, castling moves the rook, en passant clears +the bypassed pawn, and a promotion sends the pawn to the graveyard while a spare +staged queen drives onto the square). + +This tab is additive: when Chess Mode is off, board clicks behave exactly as +before. The chess logic is kept in plain methods (no Qt required) so it can be +tested headlessly; the board widget is optional. +""" + +from __future__ import annotations + +from typing import Dict, List, Optional, Tuple + +import chess + +from PyQt6.QtCore import pyqtSignal +from PyQt6.QtWidgets import ( + QCheckBox, + QComboBox, + QGroupBox, + QLabel, + QListWidget, + QPushButton, + QVBoxLayout, + QWidget, +) + +from config import BOARD, PIECES, PLANNING +from models.piece import BoardState +from planning.base_planner import BasePlanner, MoveCommand, load_planner + +# The two spare queens are staged off-board as promotion reserves. +_SPARE_QUEENS = {"white": 0x11, "black": 0x22} +# Every id except the spare queens starts on a standard square. +_STANDARD_IDS = [pid for pid in range(0x01, 0x23) if pid not in _SPARE_QUEENS.values()] + + +class PlayChessTab(QWidget): + """Hot-seat chess control panel driving the robots via the selected planner.""" + + # Same signature as the planning tab so main_window routes it identically. + send_commands = pyqtSignal(list, bool) # list[MoveCommand], is_trajectory + chess_mode_changed = pyqtSignal(bool) + status_log = pyqtSignal(str) + + def __init__( + self, + board_state: BoardState, + board_widget: Optional[QWidget] = None, + parent: Optional[QWidget] = None, + ) -> None: + super().__init__(parent) + self._board = board_state + self._widget = board_widget + self._chess = chess.Board() + self._sq_to_id: Dict[int, int] = {} + self._spare: Dict[str, Optional[int]] = dict(_SPARE_QUEENS) + self._grave_free = self._build_graveyard_pools() + self._selected: Optional[int] = None # selected source square, or None + self._build_ui() + self._build_initial_map() + self._refresh_status() + + # ------------------------------------------------------------------ + # UI + # ------------------------------------------------------------------ + + def _build_ui(self) -> None: + root = QVBoxLayout(self) + root.setSpacing(8) + + algo_group = QGroupBox("Algorithm") + algo_layout = QVBoxLayout(algo_group) + self._algo_combo = QComboBox() + for name in PLANNING.PLANNERS: + self._algo_combo.addItem(name) + # Default to our planner for the smooth trajectory playback. + swarm_idx = self._algo_combo.findText("Swarm (PIBT+LNS)") + if swarm_idx >= 0: + self._algo_combo.setCurrentIndex(swarm_idx) + algo_layout.addWidget(self._algo_combo) + root.addWidget(algo_group) + + self._chk_chess = QCheckBox("Chess Mode (click a piece, then a legal square)") + self._chk_chess.toggled.connect(self._on_chess_mode_toggled) + root.addWidget(self._chk_chess) + + self._btn_new = QPushButton("New Game") + self._btn_new.clicked.connect(self.new_game) + root.addWidget(self._btn_new) + + self._turn_label = QLabel() + root.addWidget(self._turn_label) + self._status_label = QLabel() + root.addWidget(self._status_label) + + root.addWidget(QLabel("Moves:")) + self._move_list = QListWidget() + root.addWidget(self._move_list, stretch=1) + + # ------------------------------------------------------------------ + # Game lifecycle + # ------------------------------------------------------------------ + + def new_game(self) -> None: + """Reset pieces to the standard start and begin a fresh game.""" + self._board.reset_to_home() + self._chess = chess.Board() + self._spare = dict(_SPARE_QUEENS) + self._grave_free = self._build_graveyard_pools() + self._selected = None + self._move_list.clear() + self._build_initial_map() + if self._widget is not None: + self._widget.clear_legal_highlights() + self._widget.refresh() + self._refresh_status() + + def _build_initial_map(self) -> None: + """Map each standard piece id to the chess square it starts on.""" + self._sq_to_id = {} + for pid in _STANDARD_IDS: + hx, hy, _theta = PIECES.HOME_POSITIONS[pid] + sq = self._mm_to_square(float(hx), float(hy)) + if sq is not None: + self._sq_to_id[sq] = pid + + # ------------------------------------------------------------------ + # Coordinate helpers + # ------------------------------------------------------------------ + + @staticmethod + def _square_center(sq: int) -> Tuple[float, float]: + """Chess square → playing-area mm center (matches the drawn grid).""" + s = float(BOARD.SQUARE_SIZE_MM) + return ((chess.square_file(sq) + 0.5) * s, (chess.square_rank(sq) + 0.5) * s) + + @staticmethod + def _mm_to_square(x_mm: float, y_mm: float) -> Optional[int]: + """Playing-area mm → chess square, or None if off the 8x8 board.""" + s = float(BOARD.SQUARE_SIZE_MM) + if x_mm < 0 or y_mm < 0: + return None + f = int(x_mm // s) + r = int(y_mm // s) + if 0 <= f < BOARD.NUM_SQUARES and 0 <= r < BOARD.NUM_SQUARES: + return chess.square(f, r) + return None + + @staticmethod + def _color_of(pid: int) -> str: + """'white' or 'black' for a piece id.""" + return "white" if pid in PIECES.WHITE_IDS else "black" + + @staticmethod + def _build_graveyard_pools() -> Dict[str, List[Tuple[float, float]]]: + """Free graveyard slots per color, ordered farthest-from-board first. + + Filling the farthest slots first means a newly captured piece always drives + to the nearest free slot while every occupied slot is beyond it, so its + straight-in approach never crosses a piece already in the graveyard. + """ + center_x = float(BOARD.PLAYING_AREA_MM) / 2.0 + pools: Dict[str, List[Tuple[float, float]]] = {"white": [], "black": []} + for pid, (gx, gy, _t) in PIECES.GRAVEYARD_POSITIONS.items(): + pools[PlayChessTab._color_of(pid)].append((float(gx), float(gy))) + for slots in pools.values(): + slots.sort(key=lambda p: -abs(p[0] - center_x)) + return pools + + def _assign_graveyard(self, color: str) -> Tuple[float, float]: + """Take the next free graveyard slot for a color (farthest-first).""" + pool = self._grave_free.get(color) or [] + return pool.pop(0) if pool else (0.0, 0.0) + + # ------------------------------------------------------------------ + # Interaction + # ------------------------------------------------------------------ + + def _on_chess_mode_toggled(self, on: bool) -> None: + self.chess_mode_changed.emit(on) + if self._widget is not None: + self._widget.set_chess_mode(on) + self._selected = None + if on: + self._highlight() + elif self._widget is not None: + self._widget.clear_legal_highlights() + + def on_square_clicked(self, file: int, rank: int) -> None: + """Handle a board-square click in chess mode: select, re-select, or move.""" + sq = chess.square(file, rank) + occupant = self._chess.piece_at(sq) + + if self._selected is None: + if occupant is not None and occupant.color == self._chess.turn: + self._selected = sq + self._highlight() + return + + move = self._legal_move_between(self._selected, sq) + if move is not None: + self._play(move) + self._selected = None + self._highlight() + elif occupant is not None and occupant.color == self._chess.turn: + self._selected = sq + self._highlight() + else: + self._selected = None + self._highlight() + + def _legal_move_between(self, from_sq: int, to_sq: int) -> Optional[chess.Move]: + """Return the legal move from->to (auto-queen promotion), or None.""" + promo = None + piece = self._chess.piece_at(from_sq) + if (piece is not None and piece.piece_type == chess.PAWN + and chess.square_rank(to_sq) in (0, 7)): + promo = chess.QUEEN + move = chess.Move(from_sq, to_sq, promotion=promo) + return move if move in self._chess.legal_moves else None + + def _legal_dests(self, from_sq: int) -> List[int]: + """Destination squares of all legal moves from a square.""" + return [m.to_square for m in self._chess.legal_moves if m.from_square == from_sq] + + def _highlight(self) -> None: + if self._widget is None: + return + if self._selected is None: + self._widget.clear_legal_highlights() + return + dests = self._legal_dests(self._selected) + squares = {(chess.square_file(d), chess.square_rank(d)) for d in dests} + sel = (chess.square_file(self._selected), chess.square_rank(self._selected)) + self._widget.set_legal_highlights(squares, sel) + + # ------------------------------------------------------------------ + # Move execution + # ------------------------------------------------------------------ + + def _play(self, move: chess.Move) -> None: + """Apply a legal move: compute robot targets, dispatch, update state.""" + targets, san, is_white, fullmove = self._apply_move(move) + self._dispatch(targets) + self._append_move(san, is_white, fullmove) + if self._widget is not None: + self._widget.refresh() + self._refresh_status() + + def _apply_move( + self, move: chess.Move + ) -> Tuple[Dict[int, Tuple[float, float]], str, bool, int]: + """Compute per-piece mm targets and update the square map for a move. + + Returns (targets, san, is_white, fullmove_number). Uses the pre-push board + state to resolve captures/castling/en passant, then pushes the move. + """ + from_sq, to_sq = move.from_square, move.to_square + mover_color = "white" if self._chess.turn == chess.WHITE else "black" + is_white = self._chess.turn == chess.WHITE + fullmove = self._chess.fullmove_number + is_ep = self._chess.is_en_passant(move) + is_capture = self._chess.is_capture(move) + is_castle = self._chess.is_castling(move) + san = self._chess.san(move) + + targets: Dict[int, Tuple[float, float]] = {} + mover_id = self._sq_to_id.get(from_sq) + + # Captures (normal or en passant) drive to the graveyard. + cap_sq: Optional[int] = None + if is_ep: + cap_sq = chess.square(chess.square_file(to_sq), chess.square_rank(from_sq)) + elif is_capture: + cap_sq = to_sq + cap_id = self._sq_to_id.get(cap_sq) if cap_sq is not None else None + if cap_id is not None: + targets[cap_id] = self._assign_graveyard(self._color_of(cap_id)) + + # Castling also relocates the rook. + rook_from = rook_to = rook_id = None + if is_castle: + rank = chess.square_rank(from_sq) + if chess.square_file(to_sq) == 6: # king-side (g-file king) + rook_from, rook_to = chess.square(7, rank), chess.square(5, rank) + else: # queen-side (c-file king) + rook_from, rook_to = chess.square(0, rank), chess.square(3, rank) + rook_id = self._sq_to_id.get(rook_from) + if rook_id is not None: + targets[rook_id] = self._square_center(rook_to) + + spare_used: Optional[int] = None + if move.promotion: + # The pawn retires to the graveyard; a spare queen takes the square. + if mover_id is not None: + targets[mover_id] = self._assign_graveyard(mover_color) + spare_used = self._spare.get(mover_color) + if spare_used is not None: + targets[spare_used] = self._square_center(to_sq) + else: + self.status_log.emit( + f"No spare {mover_color} queen available for promotion." + ) + elif mover_id is not None: + targets[mover_id] = self._square_center(to_sq) + + # Commit the logical move, then update the square->id occupancy map. + self._chess.push(move) + if cap_sq is not None: + self._sq_to_id.pop(cap_sq, None) + self._sq_to_id.pop(from_sq, None) + if is_castle and rook_id is not None: + self._sq_to_id.pop(rook_from, None) + self._sq_to_id[rook_to] = rook_id + if move.promotion and spare_used is not None: + self._spare[mover_color] = None + self._sq_to_id[to_sq] = spare_used + elif mover_id is not None: + self._sq_to_id[to_sq] = mover_id + + return targets, san, is_white, fullmove + + def _dispatch(self, targets: Dict[int, Tuple[float, float]]) -> None: + """Plan the physical moves with the selected planner and emit them.""" + if not targets: + return + board_pos = {p.piece_id: (p.x_mm, p.y_mm) for p in self._board.active_pieces()} + planner = self._current_planner() + # Exact pieces must move (like a single manual move), so no interchange. + if hasattr(planner, "interchangeable"): + planner.interchangeable = False + # Only pieces still on the board (or moving this turn, including one heading + # to the graveyard) take part in planning. Pin each to its current square so + # the planner routes the moving pieces around them (making way where a piece + # is boxed in) instead of driving through. Already-captured pieces sit + # off-board and are left out entirely; pinning them made the buffered-Voronoi + # executor nudge the clustered graveyard pieces a little on every move. + relevant = (set(self._sq_to_id.values()) | set(targets)) & set(board_pos) + positions = {pid: board_pos[pid] for pid in relevant} + full_targets = dict(positions) + full_targets.update({pid: t for pid, t in targets.items() if pid in positions}) + commands: List[MoveCommand] = planner.plan_moves(positions, full_targets) + self.send_commands.emit(commands, bool(getattr(planner, "produces_trajectory", False))) + + def _current_planner(self) -> BasePlanner: + try: + return load_planner(self._algo_combo.currentText()) + except (KeyError, ImportError, AttributeError): + from planning.direct_planner import DirectPlanner + return DirectPlanner() + + # ------------------------------------------------------------------ + # Status / move list + # ------------------------------------------------------------------ + + def _append_move(self, san: str, is_white: bool, fullmove: int) -> None: + if is_white: + self._move_list.addItem(f"{fullmove}. {san}") + elif self._move_list.count(): + item = self._move_list.item(self._move_list.count() - 1) + item.setText(f"{item.text()} {san}") + else: + self._move_list.addItem(f"{fullmove}... {san}") + self._move_list.scrollToBottom() + + def _refresh_status(self) -> None: + side = "White" if self._chess.turn == chess.WHITE else "Black" + self._turn_label.setText(f"To move: {side}") + if self._chess.is_checkmate(): + winner = "Black" if self._chess.turn == chess.WHITE else "White" + self._status_label.setText(f"Checkmate — {winner} wins") + elif self._chess.is_stalemate(): + self._status_label.setText("Stalemate — draw") + elif self._chess.is_insufficient_material(): + self._status_label.setText("Draw — insufficient material") + elif self._chess.is_check(): + self._status_label.setText(f"{side} to move — CHECK") + else: + self._status_label.setText("Game in progress") diff --git a/firmware/MiniBot_Coordinator/planning/base_planner.py b/firmware/MiniBot_Coordinator/planning/base_planner.py index bc1f5dd..eef121e 100644 --- a/firmware/MiniBot_Coordinator/planning/base_planner.py +++ b/firmware/MiniBot_Coordinator/planning/base_planner.py @@ -75,6 +75,16 @@ def name(self) -> str: """Human-readable planner name shown in the GUI dropdown.""" return self.__class__.__name__ + @property + def produces_trajectory(self) -> bool: + """True if plan_moves emits a fine, time-synchronized continuous path. + + Such planners can be played back smoothly (all pieces advance in lockstep + along a shared time cursor) instead of the discrete wave-barrier dispatch. + Defaults to False so existing discrete planners are unaffected. + """ + return False + @abstractmethod def plan_moves( self, diff --git a/firmware/MiniBot_Coordinator/planning/swarm/README.md b/firmware/MiniBot_Coordinator/planning/swarm/README.md new file mode 100644 index 0000000..fbb532a --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/README.md @@ -0,0 +1,62 @@ +# Swarm planner (PIBT + LNS + BVC) + +A complete, collision-free multi-robot planner for the MiniBot coordinator, +vendored as a self-contained subpackage (standard library + `planning.base_planner` ++ `config` only). Registered as **"Swarm (PIBT+LNS)"**; entry point +`planning.swarm_planner.SwarmPlanner`. + +## Why it exists + +The existing `EnhancedConflictPlanner` is heuristic and can get stuck. This planner +is complete by construction: + +1. **PIBT** (priority inheritance with backtracking) plans the choreography over an + 8-connected cell grid: the piece farthest from its goal gets right of way, others + step aside and flow back. No two pieces ever share a cell and swaps are forbidden. +2. **MAPF-LNS** refines that plan for lower total travel (destroy-and-repair with + space-time A*), so fewer pieces move and they move shorter paths. +3. **Buffered Voronoi Cells (BVC)** execute the plan as continuous, collision-free + motion. Each piece keeps to its own buffered half-space, so pairs stay at least a + diameter apart at all times. + +## How it drives real robots + +The robots follow waypoints open-loop (no on-board reciprocal avoidance), so +collision-freedom has to be in the plan. Handing them raw cell steps would risk +diagonal crossings colliding. Instead `plan_moves` forward-simulates the full +PIBT/LNS + BVC pipeline and emits each piece's **actual sampled trajectory** as +wave-ordered `MoveCommand`s. The robots replay the collision-free path; sampled +poses are collision-free by construction. Wave granularity is tunable via +`SwarmPlanner(sample_ms=...)` (fewer, coarser waves = fewer dispatch round-trips). + +## Board size: 57.15 mm squares (2.25 in) + +This branch sets `config.BOARD.SQUARE_SIZE_MM = 57.15` — the regulation tournament +square — as the board size, for two reasons: + +1. **Standard sizing.** 2.25 in is the FIDE/USCF tournament square; real boards and + pieces are made to it. +2. **Collision-free-motion clearance.** A MiniBot is ~31 mm across. At a 50 mm + square the gap between piece bodies on neighbouring squares is only ~19 mm, and + the buffered-Voronoi routing has almost no room to slip a piece past its + neighbours — it deadlocks and detours far more often. At 57.15 mm that gap grows + to ~26 mm, which is what keeps dense resets and shuffles collision-free and + efficient (measurably higher completion and lower total travel on scattered-reset + benchmarks). + +The planner itself reads the square size from `config`, so it stays correct on any +board size; 57.15 mm is simply the size the motion strategy is tuned for. All +piece coordinates in `config.py` derive from `SQUARE_SIZE_MM` (the `_S` shorthand +tracks it), so they scale together and cannot drift. See `RECOMMENDED_SQUARE_MM` +in `planning/swarm_planner.py`. + +## Self test + +From `firmware/MiniBot_Coordinator/`: + +``` +python planning/swarm/_selftest.py +``` + +Scatters all 34 pieces, plans them home, and asserts the waves are wave-ordered, +each piece's last waypoint is on its target, and every wave is collision-free. diff --git a/firmware/MiniBot_Coordinator/planning/swarm/__init__.py b/firmware/MiniBot_Coordinator/planning/swarm/__init__.py new file mode 100644 index 0000000..b183f5c --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/__init__.py @@ -0,0 +1,8 @@ +"""Self-contained PIBT + LNS + BVC motion pipeline vendored for the MiniBot +coordinator. + +This subpackage is a port of an external swarm-motion project's tested planner +and collision-free executor. It has no dependency on that project; only the +Python standard library plus this coordinator's ``planning.base_planner`` and +``config``. The public entry point is ``planning.swarm_planner.SwarmPlanner``. +""" diff --git a/firmware/MiniBot_Coordinator/planning/swarm/_selftest.py b/firmware/MiniBot_Coordinator/planning/swarm/_selftest.py new file mode 100644 index 0000000..00ab3a2 --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/_selftest.py @@ -0,0 +1,74 @@ +"""Self test for the vendored swarm planner. + +Run from the coordinator root so `from config import ...` resolves: + + python planning/swarm/_selftest.py + +Scatters all 34 pieces onto random board squares, plans them home, and asserts the +output is wave-ordered, lands every piece on its target, and is collision-free at +every wave. +""" +import math +import os +import random +import sys + +# Allow running as `python planning/swarm/_selftest.py` from the coordinator root. +sys.path.insert(0, os.getcwd()) + +from config import BOARD, PIECES # noqa: E402 +from planning.swarm.executor import PIECE_DIAMETER_MM # noqa: E402 +from planning.swarm_planner import SwarmPlanner # noqa: E402 + + +def main() -> None: + ids = list(PIECES.HOME_POSITIONS.keys()) + targets = {pid: (x, y) for pid, (x, y, _theta) in PIECES.HOME_POSITIONS.items()} + + s = BOARD.SQUARE_SIZE_MM + play_cells = [((c + 0.5) * s, (r + 0.5) * s) + for c in range(BOARD.NUM_SQUARES) for r in range(BOARD.NUM_SQUARES)] + rng = random.Random(0) + rng.shuffle(play_cells) + starts = {pid: play_cells[i] for i, pid in enumerate(ids)} + + commands = SwarmPlanner().plan_moves(starts, targets) + assert commands, "planner returned no commands" + + waves = [c.sequence_num for c in commands] + assert waves == sorted(waves), "commands are not wave-ordered" + + final = {} + for c in commands: + final[c.piece_id] = (c.target_x_mm, c.target_y_mm) + assert set(final) == set(ids), "not every piece has a final command" + # Pieces are interchangeable, so check every home SQUARE is covered by some + # piece rather than requiring a specific id on a specific square. + finals = list(final.values()) + for hx, hy in targets.values(): + assert any(math.hypot(fx - hx, fy - hy) < 5.0 for fx, fy in finals), \ + f"home square ({hx:.0f},{hy:.0f}) not covered" + + by_wave = {} + for c in commands: + by_wave.setdefault(c.sequence_num, []).append(c) + pos = {} + min_gap = float("inf") + for wave in sorted(by_wave): + for c in by_wave[wave]: + pos[c.piece_id] = (c.target_x_mm, c.target_y_mm) + placed = list(pos.values()) + for i in range(len(placed)): + for j in range(i + 1, len(placed)): + gap = math.hypot(placed[i][0] - placed[j][0], placed[i][1] - placed[j][1]) + min_gap = min(min_gap, gap) + assert gap >= PIECE_DIAMETER_MM, \ + f"wave {wave}: pieces {gap:.1f} mm apart (< {PIECE_DIAMETER_MM})" + + print(f"PASS: {len(commands)} commands across {max(waves)} waves; " + f"min pairwise gap {min_gap:.1f} mm >= {PIECE_DIAMETER_MM} mm; " + f"all 34 pieces reached target.") + + +if __name__ == "__main__": + main() diff --git a/firmware/MiniBot_Coordinator/planning/swarm/_selftest_playback.py b/firmware/MiniBot_Coordinator/planning/swarm/_selftest_playback.py new file mode 100644 index 0000000..a74eb69 --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/_selftest_playback.py @@ -0,0 +1,115 @@ +"""Self test for continuous trajectory playback in the simulator. + +Run from the coordinator root: + + python planning/swarm/_selftest_playback.py + +Scatters all 34 pieces, plans them home with the SwarmPlanner, reconstructs the +per-piece time-synced paths exactly as MainWindow does, plays them through the +patched MotionSimulator, and checks the playback completes, covers every home, +stays collision-free through the continuous interpolation, and moves smoothly. +""" +import math +import os +import random +import sys + +from PyQt6.QtWidgets import QApplication + +sys.path.insert(0, os.getcwd()) + +from config import PIECES # noqa: E402 +from models.piece import BoardState # noqa: E402 +from simulation.simulator import MotionSimulator # noqa: E402 +from planning.swarm_planner import SwarmPlanner # noqa: E402 + +_SPEED_MM_S = 100.0 +_PIECE_DIAMETER_MM = 2.0 * PIECES.CIRCLE_RADIUS_MM +_CONTINUITY_BOUND_MM = 8.0 # > step (speed * tick) with margin + + +def _reconstruct(board, commands): + """Mirror MainWindow._play_trajectory_sim path/final-heading reconstruction.""" + waves = {} + for c in commands: + waves.setdefault(c.sequence_num, {})[c.piece_id] = c + wave_nums = sorted(waves) + paths, final_theta = {}, {} + for pid in {c.piece_id for c in commands}: + piece = board.get_piece(pid) + prev = (piece.x_mm, piece.y_mm) + seq = [prev] + for wnum in wave_nums: + cmd = waves[wnum].get(pid) + if cmd is not None: + prev = (cmd.target_x_mm, cmd.target_y_mm) + if cmd.target_theta is not None: + final_theta[pid] = cmd.target_theta + seq.append(prev) + paths[pid] = seq + return paths, final_theta + + +def main() -> None: + QApplication([]) + board = BoardState() + sim = MotionSimulator(board) + sim.speed_mm_s = _SPEED_MM_S + + ids = list(PIECES.HOME_POSITIONS) + homes = {p: (x, y) for p, (x, y, _t) in PIECES.HOME_POSITIONS.items()} + random.seed(1) + placed = {} + for pid in ids: + for _ in range(10000): + x = random.uniform(30, 427) + y = random.uniform(30, 427) + if all(math.hypot(x - a, y - b) >= 40 for a, b in placed.values()): + placed[pid] = (x, y) + break + board.update_piece_position(pid, placed[pid][0], placed[pid][1], 0.0) + + commands = SwarmPlanner().plan_moves(placed, homes) + paths, final_theta = _reconstruct(board, commands) + sim.play_trajectory(paths, final_theta) + + prev_pos = {pid: (board.get_piece(pid).x_mm, board.get_piece(pid).y_mm) for pid in ids} + min_gap = float("inf") + max_jump = 0.0 + ticks = 0 + while sim._trajectory is not None and ticks < 100000: + sim._tick() + ticks += 1 + cur = {pid: (board.get_piece(pid).x_mm, board.get_piece(pid).y_mm) for pid in ids} + for pid in ids: + max_jump = max(max_jump, math.hypot(cur[pid][0] - prev_pos[pid][0], + cur[pid][1] - prev_pos[pid][1])) + prev_pos = cur + pts = list(cur.values()) + for i in range(len(pts)): + for j in range(i + 1, len(pts)): + min_gap = min(min_gap, math.hypot(pts[i][0] - pts[j][0], + pts[i][1] - pts[j][1])) + + completed = sim._trajectory is None + finals = [(board.get_piece(pid).x_mm, board.get_piece(pid).y_mm) for pid in ids] + covered = sum(1 for hx, hy in homes.values() + if any(math.hypot(fx - hx, fy - hy) < 5.0 for fx, fy in finals)) + thetas_ok = all( + abs((final_theta.get(pid, board.get_piece(pid).orientation_deg) + - board.get_piece(pid).orientation_deg + 180.0) % 360.0 - 180.0) <= 1.0 + for pid in ids) + + assert completed, "playback did not complete" + assert covered == 34, f"only {covered}/34 homes covered" + assert min_gap >= _PIECE_DIAMETER_MM - 1.0, f"min gap {min_gap:.1f} < {_PIECE_DIAMETER_MM}" + assert max_jump <= _CONTINUITY_BOUND_MM, f"jump {max_jump:.1f} > {_CONTINUITY_BOUND_MM}" + assert thetas_ok, "some piece did not reach its final heading" + print(f"PASS: completed in {ticks} ticks; homes covered {covered}/34; " + f"min pairwise gap {min_gap:.1f} mm (>= {_PIECE_DIAMETER_MM:.0f}); " + f"max per-tick jump {max_jump:.1f} mm (<= {_CONTINUITY_BOUND_MM}); " + f"final headings reached.") + + +if __name__ == "__main__": + main() diff --git a/firmware/MiniBot_Coordinator/planning/swarm/assignment.py b/firmware/MiniBot_Coordinator/planning/swarm/assignment.py new file mode 100644 index 0000000..e5240db --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/assignment.py @@ -0,0 +1,44 @@ +"""Optimal min-cost assignment for small interchangeable-piece groups. + +Same-color, same-rank pieces are interchangeable, so a reset should send each to +the nearest free home square overall, not to one fixed square per id. This solves +that as a min-cost perfect matching with a bitmask DP over the target columns - +O(n^2 * 2^n), which is trivial for the small groups here (at most 8 pawns). +""" +_INF = float("inf") + + +def min_cost_assignment(cost: list) -> list: + """Return assignment[i] = j minimizing sum(cost[i][j]) over a square matrix. + + cost is an n-by-n list of lists (row = piece, column = target). Returns a list + mapping each row to a distinct column. + """ + n = len(cost) + if n == 0: + return [] + size = 1 << n + dp = [_INF] * size + parent = [(-1, -1)] * size + dp[0] = 0.0 + for mask in range(size): + if dp[mask] == _INF: + continue + row = bin(mask).count("1") # next piece to place + if row >= n: + continue + for col in range(n): + if mask & (1 << col): + continue + nxt = mask | (1 << col) + candidate = dp[mask] + cost[row][col] + if candidate < dp[nxt]: + dp[nxt] = candidate + parent[nxt] = (mask, col) + assignment = [-1] * n + mask = size - 1 + for row in range(n - 1, -1, -1): + prev_mask, col = parent[mask] + assignment[row] = col + mask = prev_mask + return assignment diff --git a/firmware/MiniBot_Coordinator/planning/swarm/drive.py b/firmware/MiniBot_Coordinator/planning/swarm/drive.py new file mode 100644 index 0000000..7e3603a --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/drive.py @@ -0,0 +1,219 @@ +"""Forward-simulate the PIBT/LNS choreography through the BVC executor. + +The planner decides each piece's next square; this drive loop executes the +transitions as continuous collision-free motion, advancing to the next joint +configuration only once every piece has reached its current one, latching pieces +home on the final step, and turning them in place to face forward. If a transition +stalls it replans from the pieces' actual squares. It records each piece's +trajectory so the caller can emit it as waypoints, which is how our sim's +collision-free behavior reaches robots that only follow waypoints. +""" +import math + +from planning.swarm.executor import ( + DT_S, GOAL_TOL_MM, MAX_WHEEL_SPEED_MMPS, NEIGHBOR_RANGE_MM, PIECE_DIAMETER_MM, + PROJECTION_ITERS, RADIUS_MM, REACH_TOL_MM, WHEELBASE_MM, drive_bot_to, step_pose, +) + +_STALL_LIMIT_STEPS = 400 +_MAX_REPLAN_FAILS = 3 +_ORIENT_TOL_RAD = 0.05 +_MOVED_EPS_MM = 1.0 + + +class Bot: + """Lightweight mutable piece state used by the executor and drive loop.""" + + __slots__ = ("id", "x", "y", "theta") + + def __init__(self, piece_id: int, x: float, y: float, theta: float): + self.id = piece_id + self.x = x + self.y = y + self.theta = theta + + +def _at(x, y, gx, gy, tol) -> bool: + """True if (x, y) is within tol of (gx, gy).""" + return math.hypot(x - gx, y - gy) < tol + + +def _wrap(angle: float) -> float: + """Wrap an angle to (-pi, pi].""" + return (angle + math.pi) % (2.0 * math.pi) - math.pi + + +class HybridDrive: + """Executes a PIBT/LNS plan through BVC, replanning on stalls, and settles pieces.""" + + def __init__(self, grid, goal_xy: dict, ids: list, orient_target: dict, planner): + self._grid = grid + self._goal_xy = goal_xy + self._ids = ids + self._index = {bid: i for i, bid in enumerate(ids)} + self._orient = orient_target + self._planner = planner + self._goal_cells = None + self._plan = None + self._idx = 0 + self._since = 0 + self._fails = 0 + self._home: set = set() + self._finished = False + + def reset(self, bots) -> None: + """Snap starts/goals to distinct cells and plan the choreography.""" + self._goal_cells = self._distinct_cells([self._goal_xy[b.id] for b in bots]) + starts = self._distinct_cells([(b.x, b.y) for b in bots]) + self._plan = self._planner(starts, self._goal_cells, self._grid) + self._idx = 1 if self._plan and len(self._plan) > 1 else 0 + self._finished = self._plan is not None and len(self._plan) <= 1 + + def _distinct_cells(self, points: list) -> list: + """Snap each point to a distinct cell (nearest free cell on collision).""" + used = set() + out = [] + for x, y in points: + cell = self._grid.nearest(x, y) + if cell in used: + cell = self._nearest_free(x, y, used) + used.add(cell) + out.append(cell) + return out + + def _nearest_free(self, x: float, y: float, used: set) -> int: + """The closest cell to (x, y) not already taken in this assignment pass.""" + best_id, best_d = 0, math.inf + for cid in range(self._grid.count): + if cid in used: + continue + cx, cy = self._grid.xy(cid) + d = (cx - x) ** 2 + (cy - y) ** 2 + if d < best_d: + best_d, best_id = d, cid + return best_id + + def _aligned(self, bot) -> bool: + """True if the piece faces its target heading within tolerance.""" + if bot.id not in self._orient: + return True + return abs(_wrap(self._orient[bot.id] - bot.theta)) <= _ORIENT_TOL_RAD + + def _hold_or_orient(self, bot) -> bool: + """Hold a settled piece, spinning it toward its target heading if needed.""" + if self._aligned(bot): + return True + err = _wrap(self._orient[bot.id] - bot.theta) + turn_mag = min(MAX_WHEEL_SPEED_MMPS, abs(err) * WHEELBASE_MM / (2.0 * DT_S)) + turn = turn_mag if err > 0.0 else -turn_mag + bot.x, bot.y, bot.theta = step_pose(bot.x, bot.y, bot.theta, -turn, turn, DT_S) + return False + + def advance(self, bots) -> None: + """Advance one tick: drive pieces toward their current step's squares.""" + if self._finished: + return + if self._plan is None: + self._drive_to_goals(bots) + return + final = self._idx >= len(self._plan) - 1 + targets = self._plan[self._idx] + positions = {b.id: (b.x, b.y) for b in bots} + for bot in bots: + if bot.id in self._home: + self._hold_or_orient(bot) + continue + tx, ty = self._grid.xy(targets[self._index[bot.id]]) + skip = GOAL_TOL_MM if final else REACH_TOL_MM + if _at(bot.x, bot.y, tx, ty, skip): + continue + drive_bot_to(bot, (tx, ty), positions, RADIUS_MM, PIECE_DIAMETER_MM, + NEIGHBOR_RANGE_MM, GOAL_TOL_MM, PROJECTION_ITERS, DT_S) + self._advance_plan(bots, final) + + def _advance_plan(self, bots, final: bool) -> None: + """Advance to the next configuration, or latch pieces home on the final one.""" + if final: + for bot in bots: + cell = self._plan[-1][self._index[bot.id]] + if _at(bot.x, bot.y, *self._grid.xy(cell), GOAL_TOL_MM): + self._home.add(bot.id) + if len(self._home) == len(bots) and all(self._aligned(b) for b in bots): + self._finished = True + return + reached = all( + _at(b.x, b.y, *self._grid.xy(self._plan[self._idx][self._index[b.id]]), + REACH_TOL_MM) + for b in bots + ) + if reached: + self._since = 0 + self._idx += 1 + return + self._since += 1 + if self._since >= _STALL_LIMIT_STEPS: + self._replan(bots) + + def _replan(self, bots) -> None: + """Recompute the plan from actual squares; fall back if it keeps failing.""" + self._since = 0 + starts = self._distinct_cells([(b.x, b.y) for b in bots]) + plan = self._planner(starts, self._goal_cells, self._grid) + if plan is not None: + self._fails = 0 + self._home = set() + self._plan = plan + self._idx = 1 if len(plan) > 1 else 0 + if len(plan) <= 1: + self._finished = True + return + self._fails += 1 + if self._fails >= _MAX_REPLAN_FAILS: + self._plan = None + + def _drive_to_goals(self, bots) -> None: + """Fallback when no plan exists: drive each piece straight at its goal.""" + positions = {b.id: (b.x, b.y) for b in bots} + all_home = True + for bot in bots: + gx, gy = self._goal_xy[bot.id] + if _at(bot.x, bot.y, gx, gy, REACH_TOL_MM): + if not self._hold_or_orient(bot): + all_home = False + continue + all_home = False + drive_bot_to(bot, (gx, gy), positions, RADIUS_MM, PIECE_DIAMETER_MM, + NEIGHBOR_RANGE_MM, GOAL_TOL_MM, PROJECTION_ITERS, DT_S) + self._finished = all_home + + @property + def finished(self) -> bool: + """True when every piece has reached its goal square and faced forward.""" + return self._finished + + +def run_and_sample(drive: HybridDrive, bots, sample_every: int, step_cap: int) -> list: + """Run the drive to completion, sampling moved pieces every sample_every ticks. + + Returns a list of waves; each wave is a dict piece_id -> (x, y, theta_rad) for + the pieces that moved since their last sample. The sampled poses come straight + from the collision-free executor, so replaying them as waypoints is collision + free by construction. + """ + last = {b.id: (b.x, b.y) for b in bots} + waves: list = [] + step = 0 + while step < step_cap: + drive.advance(bots) + step += 1 + if step % sample_every == 0 or drive.finished: + snap = {} + for bot in bots: + if math.hypot(bot.x - last[bot.id][0], bot.y - last[bot.id][1]) >= _MOVED_EPS_MM: + snap[bot.id] = (bot.x, bot.y, bot.theta) + last[bot.id] = (bot.x, bot.y) + if snap: + waves.append(snap) + if drive.finished: + break + return waves diff --git a/firmware/MiniBot_Coordinator/planning/swarm/executor.py b/firmware/MiniBot_Coordinator/planning/swarm/executor.py new file mode 100644 index 0000000..cc58a1e --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/executor.py @@ -0,0 +1,133 @@ +"""Buffered Voronoi Cells: collision-free-by-construction continuous motion. + +Each piece moves toward the point nearest its goal that stays at least ``radius`` +inside every perpendicular bisector between it and a neighbor (its buffered +Voronoi cell). Because each piece keeps to its own buffered half-space, any two +pieces' targets stay at least 2*radius apart, so the swarm never collides. A +per-tick safety cap on forward distance guarantees no overlap even before the +iterative projection fully converges, so collision-freedom never depends on +projection accuracy. + +Operates on lightweight piece state (objects with mutable .x, .y, .theta in mm +and radians) rather than any external robot model, so it is fully self-contained. +""" +import math + +# Kinematics of the real MiniBot (from the firmware config.h). +WHEELBASE_MM = 23.4 +WHEEL_RADIUS_MM = 5.25 +# Planning wheel speed. Real bots reach ~250 mm/s; here the absolute value only +# scales sim ticks, and each emitted MoveCommand's duration_ms is a time budget +# the bot fills, so trajectory shape (not this speed) is what matters downstream. +MAX_WHEEL_SPEED_MMPS = 80.0 + +# Motion tunables (ported from the source project's tuned values). +PIECE_DIAMETER_MM = 31.0 +POSITION_ERROR_MM = 2.0 +SAFETY_MARGIN_MM = 2.0 +GOAL_TOL_MM = 3.0 +NEIGHBOR_RANGE_MM = 200.0 +ALIGN_THRESHOLD_RAD = 0.25 +PROJECTION_ITERS = 14 +DT_S = 0.02 + +# Buffer radius each piece keeps clear, and the reach tolerance for latching a +# square (wider than the stop tolerance plus the localization noise band). +RADIUS_MM = PIECE_DIAMETER_MM / 2.0 + POSITION_ERROR_MM + SAFETY_MARGIN_MM +REACH_TOL_MM = GOAL_TOL_MM + 2.0 * POSITION_ERROR_MM + +_EPS = 1e-9 +_SAFETY_EPS_MM = 0.5 +_OMEGA_THRESHOLD = 1e-9 + + +def step_pose(x: float, y: float, theta: float, v_l: float, v_r: float, + dt_s: float) -> tuple: + """Integrate one differential-drive timestep; returns (x, y, theta). + + Uses the exact arc solution when turning and the straight-line limit when the + angular velocity is near zero, avoiding a divide-by-zero. + """ + v_l = max(-MAX_WHEEL_SPEED_MMPS, min(MAX_WHEEL_SPEED_MMPS, v_l)) + v_r = max(-MAX_WHEEL_SPEED_MMPS, min(MAX_WHEEL_SPEED_MMPS, v_r)) + v = 0.5 * (v_l + v_r) + omega = (v_r - v_l) / WHEELBASE_MM + if abs(omega) < _OMEGA_THRESHOLD: + return (x + v * math.cos(theta) * dt_s, y + v * math.sin(theta) * dt_s, theta) + radius = v / omega + new_theta = theta + omega * dt_s + return (x + radius * (math.sin(new_theta) - math.sin(theta)), + y - radius * (math.cos(new_theta) - math.cos(theta)), + new_theta) + + +def buffered_voronoi_target(pos, goal, neighbors, radius: float, iters: int) -> tuple: + """Project goal into the buffered Voronoi cell of pos; return the target point.""" + tx, ty = goal + px, py = pos + for _ in range(iters): + for nx, ny in neighbors: + dx, dy = nx - px, ny - py + dist = math.hypot(dx, dy) + if dist < _EPS: + continue + ux, uy = dx / dist, dy / dist # unit vector toward the neighbor + mx, my = (px + nx) / 2.0, (py + ny) / 2.0 # bisector midpoint + slack = (tx - mx) * ux + (ty - my) * uy + radius + if slack > 0.0: + tx -= slack * ux + ty -= slack * uy + return (tx, ty) + + +def _safety_cap(px: float, py: float, neighbors, diameter_mm: float): + """Largest forward step that cannot overlap the nearest neighbor this tick. + + Half the slack to the nearest neighbor (less a small epsilon): even if that + neighbor moves toward this piece by the same amount, the pair stays at least + one diameter apart. Returns None when nothing is near enough to constrain. + """ + if not neighbors: + return None + nearest = min(math.hypot(nx - px, ny - py) for nx, ny in neighbors) + return max(0.0, (nearest - diameter_mm) / 2.0 - _SAFETY_EPS_MM) + + +def _drive_to_point(x, y, theta, tx, ty, dt_s, tol_mm, max_step_mm): + """Wheel speeds moving a diff-drive piece toward (tx, ty) without leaving its cell. + + Rotate in place until roughly aligned, then drive straight, capping speed so a + step never overshoots the in-cell target or the neighbor safety cap. Rotation + in place does not translate, so it is always safe and is never capped. + """ + dx, dy = tx - x, ty - y + dist = math.hypot(dx, dy) + if dist < tol_mm: + return 0.0, 0.0 + error = (math.atan2(dy, dx) - theta + math.pi) % (2.0 * math.pi) - math.pi + v_max = MAX_WHEEL_SPEED_MMPS + if abs(error) > ALIGN_THRESHOLD_RAD: + turn = v_max if error > 0.0 else -v_max + return -turn, turn + forward = min(v_max, dist / dt_s) + if max_step_mm is not None: + forward = min(forward, max(0.0, max_step_mm) / dt_s) + return forward, forward + + +def drive_bot_to(bot, target, positions, radius, diameter_mm, rng_mm, + tol_mm, iters, dt_s) -> None: + """Drive one piece a single tick toward target using the BVC collision-free step. + + Gathers in-range neighbor positions, projects target into this piece's buffered + Voronoi cell, applies the per-tick safety speed cap, steers, and integrates the + piece's pose in place. + """ + px, py = positions[bot.id] + neighbors = [pos for bid, pos in positions.items() + if bid != bot.id + and (pos[0] - px) ** 2 + (pos[1] - py) ** 2 <= rng_mm ** 2] + tx, ty = buffered_voronoi_target((px, py), target, neighbors, radius, iters) + cap = _safety_cap(px, py, neighbors, diameter_mm) + v_l, v_r = _drive_to_point(bot.x, bot.y, bot.theta, tx, ty, dt_s, tol_mm, cap) + bot.x, bot.y, bot.theta = step_pose(bot.x, bot.y, bot.theta, v_l, v_r, dt_s) diff --git a/firmware/MiniBot_Coordinator/planning/swarm/grid.py b/firmware/MiniBot_Coordinator/planning/swarm/grid.py new file mode 100644 index 0000000..74b7d40 --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/grid.py @@ -0,0 +1,64 @@ +"""Coarse cell grid for choreography planning (one node per board square center). + +PIBT plans which square each piece occupies at each step; this grid gives it the +squares and their 8-connected adjacency. Motion between squares is straight and +continuous (executed by the BVC layer), so adjacency is center-to-center: a piece +travels through tile centers, never along the lines between them. Squares sit far +enough apart that two distinct cells never conflict, so occupancy is simply "one +piece per cell". +""" +import math + +_NEIGHBORS_8 = [(-1, -1), (-1, 0), (-1, 1), (0, -1), + (0, 1), (1, -1), (1, 0), (1, 1)] + + +class CellGrid: + """Square-center cells with 8-connected adjacency and cell/world conversions.""" + + def __init__(self, cell_specs, pitch_mm: float): + """Build the grid from (col, row, x_mm, y_mm) specs at the given pitch. + + cell_specs lists every square as its integer (col, row) plus its world + center; pitch_mm is the square side length, used to snap world points back + to cells. Neighbors are the 8 grid-adjacent squares that are present. + """ + self._pitch = pitch_mm + self._cells = [(c, r) for (c, r, _x, _y) in cell_specs] + self._xy = [(x, y) for (_c, _r, x, y) in cell_specs] + self._index = {cr: i for i, cr in enumerate(self._cells)} + present = set(self._cells) + self._neighbors = [ + [self._index[(c + dc, r + dr)] for dc, dr in _NEIGHBORS_8 + if (c + dc, r + dr) in present] + for (c, r) in self._cells + ] + + def _cell_of_world(self, x_mm: float, y_mm: float) -> tuple: + """Integer (col, row) of the square center covering a world point.""" + return (round(x_mm / self._pitch - 0.5), round(y_mm / self._pitch - 0.5)) + + @property + def count(self) -> int: + """Number of cells in the grid.""" + return len(self._cells) + + def neighbors(self, cell_id: int) -> list: + """8-connected neighbor cell ids of a cell.""" + return self._neighbors[cell_id] + + def xy(self, cell_id: int) -> tuple: + """World (x, y) center of a cell.""" + return self._xy[cell_id] + + def nearest(self, x_mm: float, y_mm: float) -> int: + """Cell id whose center is nearest the given world point.""" + cell = self._cell_of_world(x_mm, y_mm) + if cell in self._index: + return self._index[cell] + best_id, best_d = 0, math.inf + for i, (cx, cy) in enumerate(self._xy): + d = (cx - x_mm) ** 2 + (cy - y_mm) ** 2 + if d < best_d: + best_d, best_id = d, i + return best_id diff --git a/firmware/MiniBot_Coordinator/planning/swarm/lns.py b/firmware/MiniBot_Coordinator/planning/swarm/lns.py new file mode 100644 index 0000000..fdb98ad --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/lns.py @@ -0,0 +1,209 @@ +"""MAPF-LNS: anytime flowtime refinement of an initial PIBT plan (cell grid). + +Starts from PIBT's feasible plan, then repeatedly destroys a random subset of +agents' paths and repairs them with prioritized space-time A* against the rest, +keeping a change only when it lowers flowtime (total travel). This trims the +detours and extra moves greedy PIBT leaves in, so fewer pieces move and they move +less, while staying conflict-free at the cell level. The output matches pibt_plan +(a list of joint configurations), so it drops into the same executor slot. + +Reference: Li et al., "Anytime Multi-Agent Path Finding via Large Neighborhood +Search" (IJCAI 2021). +""" +import heapq +import random +import time + +from planning.swarm.pibt import _distance_table, pibt_plan + +_INF = float("inf") +_MAX_T = 256 # planning horizon in ticks +_GOAL_HOLD = _MAX_T # a parked goal stays reserved across the whole horizon +_WAIT_COST = 1.0 # a wait delays arrival by a tick, matching flowtime +_DEFAULT_NEIGHBORHOOD = 8 +_MAX_ITERS = 100000 # hard cap on refinement iterations (reproducibility) +_STALE_FACTOR = 8 # converged after this many no-improvement rounds per agent + + +class _Reservation: + """Space-time reservations: occupied cells per tick, plus swap edges.""" + + def __init__(self): + self._vertex: dict = {} + self._edge: dict = {} + + def add_path(self, path: list) -> None: + """Reserve a path's cells and traversals, holding its goal afterward.""" + for t, cell in enumerate(path): + self._vertex.setdefault(t, set()).add(cell) + if t + 1 < len(path): + self._edge.setdefault(t, set()).add((cell, path[t + 1])) + if path: + last = path[-1] + for t in range(len(path), len(path) + _GOAL_HOLD): + self._vertex.setdefault(t, set()).add(last) + + def vertex_free(self, cell: int, t: int) -> bool: + """True if cell is unoccupied at tick t.""" + return cell not in self._vertex.get(t, ()) + + def edge_free(self, frm: int, to: int, t: int) -> bool: + """True if moving frm->to at tick t does not swap with a reservation.""" + return (to, frm) not in self._edge.get(t, ()) + + +def _reconstruct(came: dict, key) -> list: + """Walk parent links from a goal key back to the start; return cells in order.""" + path = [] + while key is not None: + path.append(key[0]) + key = came[key] + path.reverse() + return path + + +def _astar(start: int, goal: int, grid, hdist: dict, res: _Reservation, + max_t: int = _MAX_T): + """Single-agent space-time A* from start to goal avoiding the reservation. + + Move and (non-goal) wait both cost one tick, so the path cost equals arrival + time; A* therefore minimizes arrival time, matching the flowtime objective. + """ + open_heap = [(hdist.get(start, _INF), 0.0, start, 0)] + best = {(start, 0): 0.0} + came = {(start, 0): None} + while open_heap: + _, g, cell, t = heapq.heappop(open_heap) + if best.get((cell, t), _INF) < g: + continue + if cell == goal and all(res.vertex_free(goal, tt) for tt in range(t, t + _GOAL_HOLD)): + # Accept the goal only when the agent can remain there: a fixed agent + # may still pass through it later, so require it free for all future + # ticks (the reservation horizon), or keep searching for a later arrival. + return _reconstruct(came, (cell, t)) + if t >= max_t: + continue + for nxt in [cell] + grid.neighbors(cell): + nt = t + 1 + if not res.vertex_free(nxt, nt): + continue + if nxt != cell and not res.edge_free(cell, nxt, t): + continue + ng = g + (_WAIT_COST if nxt == cell else 1.0) + nkey = (nxt, nt) + if ng < best.get(nkey, _INF): + best[nkey] = ng + came[nkey] = (cell, t) + heapq.heappush(open_heap, (ng + hdist.get(nxt, _INF), ng, nxt, nt)) + return None + + +def _path_cost(path: list, goal: int) -> int: + """Arrival time: timesteps until the agent reaches its goal and stays.""" + cost = 0 + for t, cell in enumerate(path): + if cell != goal: + cost = t + 1 + return cost + + +def _flowtime(paths: list, goals: list) -> int: + """Sum of per-agent arrival times (total travel to refine).""" + return sum(_path_cost(paths[i], goals[i]) for i in range(len(paths))) + + +def _conflict_free(paths: list) -> bool: + """True if no two agents share a cell or swap at any tick (with goal-holding).""" + length = max(len(p) for p in paths) + n = len(paths) + + def at(i, t): + return paths[i][t] if t < len(paths[i]) else paths[i][-1] + + for t in range(length): + seen = {} + for i in range(n): + cell = at(i, t) + if cell in seen: + return False + seen[cell] = i + if t + 1 < length: + for i in range(n): + for j in range(i + 1, n): + if at(i, t) == at(j, t + 1) and at(j, t) == at(i, t + 1) \ + and at(i, t) != at(i, t + 1): + return False + return True + + +def _to_configs(paths: list) -> list: + """Pad agent paths to equal length (holding goals) and transpose to configs.""" + length = max(len(p) for p in paths) + padded = [p + [p[-1]] * (length - len(p)) for p in paths] + return [tuple(padded[i][t] for i in range(len(padded))) for t in range(length)] + + +def _neighborhood(paths, goals, n, size, rng): + """A randomized neighborhood: a high-cost seed agent plus random others. + + Drawing the seed from the worst-cost half focuses repair where flowtime slack + lives, while the random seed and random fill let successive rounds explore + different agent subsets and escape the local minima a fixed sweep gets stuck in. + """ + size = min(size, n) + ranked = sorted(range(n), key=lambda i: -_path_cost(paths[i], goals[i])) + seed_agent = rng.choice(ranked[:max(1, n // 2)]) + pool = [j for j in range(n) if j != seed_agent] + rng.shuffle(pool) + return [seed_agent] + pool[:size - 1] + + +def lns_plan(starts, goals, grid, time_budget_s: float = 0.4, + neighborhood: int = _DEFAULT_NEIGHBORHOOD, max_t: int = _MAX_T, + seed: int = 0, priority=None): + """Refine a PIBT plan to lower flowtime; return a list of joint configurations. + + Each round destroys a randomized neighborhood and repairs it with prioritized + space-time A*, accepting only conflict-free, lower-flowtime results. Stops at + the time budget, after _STALE_FACTOR*n no-improvement rounds (converged), or at + the iteration cap. Falls back to the raw PIBT plan if refinement finds nothing. + """ + initial = pibt_plan(starts, goals, grid, priority=priority) + if initial is None: + return None + n = len(starts) + paths = [[cfg[i] for cfg in initial] for i in range(n)] + hdist = [_distance_table(grid, goals[i]) for i in range(n)] + rng = random.Random(seed) + deadline = time.perf_counter() + time_budget_s + stale_limit = _STALE_FACTOR * n + stale = 0 + for _ in range(_MAX_ITERS): + if time.perf_counter() >= deadline or stale >= stale_limit: + break + group = _neighborhood(paths, goals, n, neighborhood, rng) + others = [j for j in range(n) if j not in group] + res = _Reservation() + for j in others: + res.add_path(paths[j]) + repaired = {} + ok = True + for i in sorted(group, key=lambda i: -_path_cost(paths[i], goals[i])): + path = _astar(starts[i], goals[i], grid, hdist[i], res, max_t) + if path is None: + ok = False + break + repaired[i] = path + res.add_path(path) + if not ok: + stale += 1 + continue + candidate = list(paths) + for i, path in repaired.items(): + candidate[i] = path + if _flowtime(candidate, goals) < _flowtime(paths, goals) and _conflict_free(candidate): + paths = candidate + stale = 0 + else: + stale += 1 + return _to_configs(paths) diff --git a/firmware/MiniBot_Coordinator/planning/swarm/pibt.py b/firmware/MiniBot_Coordinator/planning/swarm/pibt.py new file mode 100644 index 0000000..9ecc799 --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm/pibt.py @@ -0,0 +1,131 @@ +"""PIBT choreography over the cell grid: who moves where, in priority order. + +PIBT (Priority Inheritance with Backtracking) advances every piece one cell per +step toward its goal in priority order; a higher-priority piece can force a +lower-priority occupant to step aside (priority inheritance), and the search +backtracks when a forced move leaves a piece no escape. This is exactly the +"furthest piece gets right of way, others move aside and flow back" behaviour we +want, computed completely rather than hoped for reactively. + +Cells sit far enough apart that two distinct cells never conflict, so occupancy +is simply one piece per cell. The output is a sequence of joint configurations (a +cell per piece per step); the BVC layer executes the transitions as smooth, +collision-free, straight center-to-center motion. +""" +from collections import deque + +_INF = float("inf") +_FAR = (_INF, _INF) + + +def _distance_table(grid, goal_cell: int) -> dict: + """Breadth-first cell-to-goal distances over the 8-connected grid.""" + dist = {goal_cell: 0} + queue = deque([goal_cell]) + while queue: + cell = queue.popleft() + for nbr in grid.neighbors(cell): + if nbr not in dist: + dist[nbr] = dist[cell] + 1 + queue.append(nbr) + return dist + + +def _pibt_step(config, order, key, occupied_start, succ, n, forced=None): + """One PIBT timestep; returns the next config tuple, or None if stuck.""" + nxt = [None] * n + occupied = {} + if forced: + for i, cell in forced.items(): + if cell in occupied: + return None + nxt[i] = cell + occupied[cell] = i + + def _candidate_key(i, cell, pushed): + # Always prefer progress toward the goal, then a straight line. A piece + # being PUSHED aside additionally prefers a square that starts empty, so + # it steps into open space and returns rather than shoving a whole line. + hop, straight = key[i].get(cell, _FAR) + if pushed: + return (hop, cell in occupied_start, straight) + return (hop, straight) + + def assign(i, caller_cell): + pushed = caller_cell is not None + candidates = sorted(succ[config[i]], key=lambda v: _candidate_key(i, v, pushed)) + for cell in candidates: + if caller_cell is not None and cell == caller_cell: + continue + if cell in occupied: + continue + nxt[i] = cell + occupied[cell] = i + blocker = next((j for j in range(n) + if nxt[j] is None and config[j] == cell), None) + if blocker is not None and not assign(blocker, config[i]): + del occupied[cell] + nxt[i] = None + continue + return True + return False + + for i in order: + if nxt[i] is None and not assign(i, None): + return None + return tuple(nxt) + + +def plan_tables(grid, goals): + """Per-agent BFS distances, sort keys, and successors for a planner. + + Returns (dist, key, succ): dist[i] maps cell to grid-step distance from goal i; + key[i] maps cell to (hop, squared straight-line distance to the goal) for the + tie-break that keeps motion straight; succ maps a cell to itself plus its + neighbors. + """ + n = len(goals) + dist = [_distance_table(grid, goals[i]) for i in range(n)] + goal_xy = [grid.xy(goals[i]) for i in range(n)] + key = [{cell: (hop, (grid.xy(cell)[0] - goal_xy[i][0]) ** 2 + + (grid.xy(cell)[1] - goal_xy[i][1]) ** 2) + for cell, hop in dist[i].items()} + for i in range(n)] + succ = {cell: [cell] + grid.neighbors(cell) for cell in range(grid.count)} + return dist, key, succ + + +def pibt_plan(starts, goals, grid, max_t: int = 512, priority=None): + """Plan a sequence of joint cell configurations from starts to goals. + + starts and goals are lists of cell ids indexed by agent. Returns a list of + configurations (the first is starts, the last is goals), or None if PIBT + cannot make progress within max_t steps. Without priority, ordering is dynamic + (the piece farthest from its goal moves first). With priority (a per-agent key, + lower wins), that key orders conflicts and distance is only the tiebreak. + """ + n = len(starts) + dist, key, succ = plan_tables(grid, goals) + config = tuple(starts) + goal = tuple(goals) + configs = [config] + visited = {config} + for _ in range(max_t): + if config == goal: + return configs + if priority is None: + order = sorted(range(n), key=lambda i: -dist[i].get(config[i], _INF)) + else: + order = sorted(range(n), + key=lambda i: (priority[i], -dist[i].get(config[i], _INF))) + nxt = _pibt_step(config, order, key, set(config), succ, n) + if nxt is None: + return None + if nxt in visited: + # PIBT+: never repeat a joint configuration. Revisiting one means the + # greedy step is cycling with no progress; bail so the caller replans. + return None + visited.add(nxt) + config = nxt + configs.append(config) + return configs if config == goal else None diff --git a/firmware/MiniBot_Coordinator/planning/swarm_planner.py b/firmware/MiniBot_Coordinator/planning/swarm_planner.py new file mode 100644 index 0000000..a4feeb9 --- /dev/null +++ b/firmware/MiniBot_Coordinator/planning/swarm_planner.py @@ -0,0 +1,206 @@ +""" +planning/swarm_planner.py — MiniBot Chess Swarm Coordinator + +SwarmPlanner: a complete, collision-free multi-robot planner. It plans the +choreography with PIBT (priority inheritance with backtracking) refined by +MAPF-LNS for lower total travel, then forward-simulates the plan through a +Buffered-Voronoi-Cell executor and emits each piece's actual collision-free +trajectory as wave-ordered MoveCommands. Because the robots follow waypoints +open-loop, replaying the executor's trajectory (rather than raw cell steps) is +what makes the on-board motion collision-free. + +The vendored pipeline lives in the self-contained ``planning.swarm`` subpackage. +See ``planning/swarm/README.md`` for the recommended 57.15 mm board size. +""" +from __future__ import annotations + +import math +from typing import Callable, Dict, List, Optional, Tuple + +from config import BOARD, PIECES +from planning.base_planner import BasePlanner, MoveCommand +from planning.swarm.assignment import min_cost_assignment +from planning.swarm.drive import Bot, HybridDrive, run_and_sample +from planning.swarm.executor import DT_S +from planning.swarm.grid import CellGrid +from planning.swarm.lns import lns_plan +from planning.swarm.pibt import pibt_plan + +# Recommended square size: 2.25 in tournament square. Wider than their 50 mm gives +# the collision-free (buffered Voronoi) motion the clearance it needs; see README. +RECOMMENDED_SQUARE_MM = 57.15 + +# One staging column each side of the 8x8 board, enough for the off-board extra +# queens (0x11, 0x22). A second column would place cell centers close enough to +# the table edge that a piece's body clips the boundary, and the coordinator's +# simulator (and real hardware) would clamp it there and never reach the target. +_BORDER_COLS = 1 + +# Heading each color faces once parked (their convention: white up, black down). +_WHITE_HEADING_DEG = 90.0 +_BLACK_HEADING_DEG = 270.0 + +_DEFAULT_SAMPLE_MS = 200.0 +_STEP_CAP = 20000 + +# A piece center must stay at least its radius (plus a hair) inside the physical +# table edges, or the coordinator/hardware clamps it and it never reaches the +# waypoint. The executor can transiently push a piece into the border while +# avoiding a crowd, so clamp every emitted waypoint back into the reachable box. +_EDGE_MARGIN_MM = 0.5 + + +class SwarmPlanner(BasePlanner): + """PIBT + LNS choreography executed through BVC, exported as MoveCommand waves.""" + + def __init__(self, use_lns: bool = True, sample_ms: float = _DEFAULT_SAMPLE_MS, + interchangeable: bool = True): + # Grid pitch matches the board actually in use, so the planner is a correct + # drop-in on their current 50 mm board; RECOMMENDED_SQUARE_MM documents the + # size we advise moving to. + self._square_mm = float(BOARD.SQUARE_SIZE_MM) + self._planner = lns_plan if use_lns else pibt_plan + self._sample_ms = sample_ms + # Public so callers can disable it for a single manual move (where the + # specific clicked piece must go to the target, not a swapped same-type one). + self.interchangeable = interchangeable + r = float(PIECES.CIRCLE_RADIUS_MM) + _EDGE_MARGIN_MM + self._x_min = -float(BOARD.BORDER_LEFT_MM) + r + self._x_max = float(BOARD.PLAYING_AREA_MM) + float(BOARD.BORDER_RIGHT_MM) - r + self._y_min = -float(BOARD.BORDER_BOTTOM_MM) + r + self._y_max = float(BOARD.PLAYING_AREA_MM) + float(BOARD.BORDER_TOP_MM) - r + self._grid = self._build_grid() + + def _clamp(self, x: float, y: float): + """Keep a waypoint inside the reachable table box (piece body within walls).""" + return (min(self._x_max, max(self._x_min, x)), + min(self._y_max, max(self._y_min, y))) + + def _reassign_interchangeable(self, ids, piece_positions, goal_xy): + """Swap targets within same-color, same-rank groups to minimize travel. + + Same-type pieces are interchangeable, so any pawn may take any pawn square. + Optimally rematching each group beats the fixed one-square-per-id home map. + """ + groups = {} + for pid in ids: + color = "w" if pid in PIECES.WHITE_IDS else "b" + groups.setdefault((color, PIECES.PIECE_RANKS.get(pid, "?")), []).append(pid) + result = dict(goal_xy) + for members in groups.values(): + if len(members) < 2: + continue + tgts = [goal_xy[p] for p in members] + cost = [[math.hypot(piece_positions[p][0] - tx, piece_positions[p][1] - ty) + for tx, ty in tgts] for p in members] + assignment = min_cost_assignment(cost) + for i, p in enumerate(members): + result[p] = tgts[assignment[i]] + return result + + @property + def name(self) -> str: + return "Swarm (PIBT+LNS)" + + @property + def produces_trajectory(self) -> bool: + """This planner emits a fine, time-synchronized, collision-free path.""" + return True + + def _build_grid(self) -> CellGrid: + """Grid over the 8x8 play squares plus staging columns each side.""" + s = self._square_mm + specs = [ + (col, row, (col + 0.5) * s, (row + 0.5) * s) + for col in range(-_BORDER_COLS, BOARD.NUM_SQUARES + _BORDER_COLS) + for row in range(BOARD.NUM_SQUARES) + ] + return CellGrid(specs, s) + + def _heading_rad(self, piece_id: int) -> float: + """Target heading in radians for a piece, by color.""" + deg = _WHITE_HEADING_DEG if piece_id in PIECES.WHITE_IDS else _BLACK_HEADING_DEG + return math.radians(deg) + + def plan_moves( + self, + piece_positions: Dict[int, Tuple[float, float]], + targets: Dict[int, Tuple[float, float]], + orientations: Optional[Dict[int, float]] = None, + validator: Optional[Callable[[int, float, float], bool]] = None, + ) -> List[MoveCommand]: + """Plan collision-free waves for every piece that has both a start and target. + + Runs PIBT+LNS to a cell choreography, forward-simulates it through the BVC + executor, and returns the sampled trajectory as wave-ordered MoveCommands. + The chess-rules validator, if given, drops any piece whose target it rejects. + """ + ids = [pid for pid in targets if pid in piece_positions] + if validator is not None: + ids = [pid for pid in ids + if validator(pid, targets[pid][0], targets[pid][1])] + if not ids: + return [] + + goal_xy = {pid: (float(targets[pid][0]), float(targets[pid][1])) for pid in ids} + if self.interchangeable: + goal_xy = self._reassign_interchangeable(ids, piece_positions, goal_xy) + orient = {pid: self._heading_rad(pid) for pid in ids} + start_theta = orientations or {} + bots = [ + Bot(pid, float(piece_positions[pid][0]), float(piece_positions[pid][1]), + math.radians(start_theta.get(pid, 0.0)) if pid in start_theta + else orient[pid]) + for pid in ids + ] + + drive = HybridDrive(self._grid, goal_xy, ids, orient, self._planner) + drive.reset(bots) + sample_every = max(1, round(self._sample_ms / (DT_S * 1000.0))) + waves = run_and_sample(drive, bots, sample_every, _STEP_CAP) + + duration_ms = int(round(sample_every * DT_S * 1000.0)) + commands = self._waves_to_commands(waves, duration_ms) + commands.extend(self._final_wave(ids, goal_xy, orient, + len(waves) + 1, duration_ms)) + return commands + + def _waves_to_commands(self, waves: list, duration_ms: int) -> List[MoveCommand]: + """Turn sampled trajectory waves into wave-ordered MoveCommands.""" + commands: List[MoveCommand] = [] + for wave, snap in enumerate(waves, start=1): + for pid, (x, y, _theta) in snap.items(): + # Intermediate waypoints leave heading free (the piece faces its + # direction of travel); only the final wave commands the target + # heading, so pieces do not stop to rotate at every waypoint. + cx, cy = self._clamp(x, y) + commands.append(MoveCommand( + piece_id=pid, + target_x_mm=cx, + target_y_mm=cy, + target_theta=None, + duration_ms=duration_ms, + sequence_num=wave, + planner_debug="swarm", + )) + return commands + + def _final_wave(self, ids, goal_xy, orient, sequence_num, duration_ms): + """A closing wave placing every piece exactly on its target, facing forward. + + Guarantees each piece's last waypoint is its exact target (not just the + planning cell center) and that non-moving pieces are still commanded home. + Targets are distinct board squares, so this wave is collision-free. + """ + return [ + MoveCommand( + piece_id=pid, + target_x_mm=self._clamp(*goal_xy[pid])[0], + target_y_mm=self._clamp(*goal_xy[pid])[1], + target_theta=math.degrees(orient[pid]) % 360.0, + duration_ms=duration_ms, + sequence_num=sequence_num, + planner_debug="swarm-final", + ) + for pid in ids + ] diff --git a/firmware/MiniBot_Coordinator/requirements.txt b/firmware/MiniBot_Coordinator/requirements.txt new file mode 100644 index 0000000..e3d98ea --- /dev/null +++ b/firmware/MiniBot_Coordinator/requirements.txt @@ -0,0 +1,3 @@ +pyqt6 +pyserial +python-chess diff --git a/firmware/MiniBot_Coordinator/simulation/simulator.py b/firmware/MiniBot_Coordinator/simulation/simulator.py index 507ad99..a0dd2e5 100644 --- a/firmware/MiniBot_Coordinator/simulation/simulator.py +++ b/firmware/MiniBot_Coordinator/simulation/simulator.py @@ -36,6 +36,13 @@ from models.piece import BoardState from planning.base_planner import MoveCommand +# Trajectory playback: advance the global time cursor by at most this many time +# slices per tick, so no waypoint is skipped and the collision-free lockstep is +# preserved. Segments shorter than _MIN_SEGMENT_MM carry no meaningful heading. +_MAX_CURSOR_ADVANCE = 1.0 +_MIN_SEGMENT_MM = 0.5 +_FINAL_HEADING_TOL_DEG = 0.5 + class MotionSimulator(QObject): """Simulates robot motion for all active MoveCommands. @@ -67,6 +74,15 @@ def __init__( self._collision_enabled: bool = True + # Time-synchronized trajectory playback (used for planners that emit a + # continuous, collision-free-by-construction path). When active, this + # takes over the tick and the per-command logic above is bypassed. + self._trajectory: Optional[Dict[int, List[Tuple[float, float]]]] = None + self._traj_len: int = 0 + self._traj_cursor: float = 0.0 + self._traj_final_theta: Dict[int, float] = {} + self._traj_finalizing: bool = False + self._timer = QTimer(self) self._timer.setInterval(SIMULATOR.UPDATE_INTERVAL_MS) self._timer.timeout.connect(self._tick) @@ -105,11 +121,41 @@ def queue_moves(self, commands: List[MoveCommand]) -> None: if self._active and not self._timer.isActive(): self._timer.start() + def play_trajectory( + self, + paths: Dict[int, List[Tuple[float, float]]], + final_theta: Dict[int, float], + ) -> None: + """Play a time-synchronized, collision-free trajectory continuously. + + paths[pid] is that piece's position at each of L identical-length time + slices (a stopped piece repeats its last position); final_theta[pid] is + the heading in degrees to face once playback ends. Every piece advances in + lockstep along a single global cursor, so the collision-free spacing the + planner guarantees at each slice is preserved between slices too. This + replaces the discrete rotate-then-translate, wave-barrier motion so a + continuous planner's arcs render smoothly. + """ + if not paths: + return + self._active.clear() + self._phase.clear() + self._rotate_to.clear() + self._trajectory = {pid: list(pts) for pid, pts in paths.items()} + self._traj_len = max(len(pts) for pts in self._trajectory.values()) + self._traj_final_theta = dict(final_theta) + self._traj_cursor = 0.0 + self._traj_finalizing = False + if not self._timer.isActive(): + self._timer.start() + def stop_all(self) -> None: """Cancel all active moves and stop the timer.""" self._active.clear() self._phase.clear() self._rotate_to.clear() + self._trajectory = None + self._traj_final_theta = {} self._timer.stop() @property @@ -130,6 +176,13 @@ def collision_enabled(self, value: bool) -> None: @pyqtSlot() def _tick(self) -> None: + # Continuous trajectory playback takes over the tick when active; the + # per-command rotate/translate logic below is left intact for the + # coordinator's other (discrete) planners. + if self._trajectory is not None: + self._tick_trajectory() + return + dt = SIMULATOR.UPDATE_INTERVAL_MS / 1000.0 step = self._speed * dt rot_step = SIMULATOR.ROTATION_SPEED_DEG_S * dt @@ -199,10 +252,26 @@ def _tick(self) -> None: if dist <= step: new_x, new_y = cmd.target_x_mm, cmd.target_y_mm + if cmd.target_theta is not None: + # Position reached; correct to the commanded final heading. + self._phase[pid] = "final_rotate" else: new_x = cur_x + (dx / dist) * step new_y = cur_y + (dy / dist) * step + # ── Phase 3: Rotate in place to the commanded final heading ── + elif phase == "final_rotate": + new_x, new_y = cur_x, cur_y + heading_diff = ( + cmd.target_theta - piece.orientation_deg + 180.0 + ) % 360.0 - 180.0 + if abs(heading_diff) <= rot_step: + new_theta = cmd.target_theta % 360.0 + else: + new_theta = ( + piece.orientation_deg + math.copysign(rot_step, heading_diff) + ) % 360.0 + # ── 1. Boundary enforcement (clamp to table limits) ────────── clamped_x = max(self._x_min, min(self._x_max, new_x)) clamped_y = max(self._y_min, min(self._y_max, new_y)) @@ -229,13 +298,20 @@ def _tick(self) -> None: new_x, new_y = cur_x, cur_y # ── Arrival check ───────────────────────────────────────────── - # Arrived when translate phase completes (position reached). - pos_done = ( - self._phase.get(pid) == "translate" - and math.hypot(new_x - cmd.target_x_mm, new_y - cmd.target_y_mm) <= 0.5 + # Arrived once the target position is reached, and (when a final + # heading was commanded) the piece has also turned to face it. + ph = self._phase.get(pid) + pos_reached = ( + math.hypot(new_x - cmd.target_x_mm, new_y - cmd.target_y_mm) <= 0.5 ) - if pos_done: + if ph == "translate" and pos_reached and cmd.target_theta is None: arrived_ids.append(pid) + elif ph == "final_rotate": + heading_reached = ( + abs((cmd.target_theta - new_theta + 180.0) % 360.0 - 180.0) <= 0.5 + ) + if pos_reached and heading_reached: + arrived_ids.append(pid) # ── Commit to board state and emit ─────────────────────────── self._board.update_piece_position(pid, new_x, new_y, new_theta) @@ -251,3 +327,86 @@ def _tick(self) -> None: if not self._active: self._timer.stop() + + def _tick_trajectory(self) -> None: + """Advance one continuous-playback tick along the global time cursor. + + Every piece moves in lockstep: the cursor advances so the fastest piece + travels at ~speed mm/s (capped so no time slice is skipped), and each + piece is linear-interpolated along its own segment and faces its travel + direction. Once the cursor reaches the last slice, pieces hold on their + final square and turn in place to the commanded heading, then playback + ends with a move_complete for every piece. + """ + dt = SIMULATOR.UPDATE_INTERVAL_MS / 1000.0 + rot_step = SIMULATOR.ROTATION_SPEED_DEG_S * dt + paths = self._trajectory + last = self._traj_len - 1 + + if not self._traj_finalizing: + # A tick advances the cursor by at most one slice, so it can touch the + # current segment and the next; size the step from the longer of the + # two so no piece moves more than `step` this tick. + i = min(int(self._traj_cursor), last) + b = min(i + 1, last) + d = min(i + 2, last) + seg_max = 0.0 + for pts in paths.values(): + seg_max = max( + seg_max, + math.hypot(pts[b][0] - pts[i][0], pts[b][1] - pts[i][1]), + math.hypot(pts[d][0] - pts[b][0], pts[d][1] - pts[b][1]), + ) + step = self._speed * dt + advance = _MAX_CURSOR_ADVANCE if seg_max <= _MIN_SEGMENT_MM else step / seg_max + self._traj_cursor += min(advance, _MAX_CURSOR_ADVANCE) + if self._traj_cursor >= last: + self._traj_cursor = float(last) + self._traj_finalizing = True + + c = self._traj_cursor + i = min(int(c), last) + frac = c - i + i2 = min(i + 1, last) + for pid, pts in paths.items(): + ax, ay = pts[i] + bx, by = pts[i2] + nx = max(self._x_min, min(self._x_max, ax + (bx - ax) * frac)) + ny = max(self._y_min, min(self._y_max, ay + (by - ay) * frac)) + sdx, sdy = bx - ax, by - ay + if math.hypot(sdx, sdy) > _MIN_SEGMENT_MM: + theta = math.degrees(math.atan2(sdy, sdx)) % 360.0 + else: + piece = self._board.get_piece(pid) + theta = piece.orientation_deg if piece is not None else 0.0 + self._board.update_piece_position(pid, nx, ny, theta) + self.position_updated.emit(pid, nx, ny, theta, 0.0) + return + + # Finalizing: hold on the last waypoint, turn in place to the final heading. + all_aligned = True + for pid, pts in paths.items(): + fx, fy = pts[last] + piece = self._board.get_piece(pid) + cur_theta = piece.orientation_deg if piece is not None else 0.0 + tgt = self._traj_final_theta.get(pid) + if tgt is None: + theta = cur_theta + else: + diff = (tgt - cur_theta + 180.0) % 360.0 - 180.0 + if abs(diff) <= max(rot_step, _FINAL_HEADING_TOL_DEG): + theta = tgt % 360.0 + else: + theta = (cur_theta + math.copysign(rot_step, diff)) % 360.0 + all_aligned = False + self._board.update_piece_position(pid, fx, fy, theta) + self.position_updated.emit(pid, fx, fy, theta, 0.0) + + if all_aligned: + ids = list(paths.keys()) + self._trajectory = None + self._traj_final_theta = {} + self._timer.stop() + for pid in ids: + self.move_complete.emit(pid) + self.log_message.emit("SIM: trajectory playback complete")