| """ |
| Conversion between Spooky's native matrix convention and robotics/Cartesian |
| convention. |
| |
| Spooky's core — map.py, pathFormulation.py, the builders, and every solver's |
| QUBO index encoding/decoding — works exclusively in (row, col) matrix indices: |
| row 0 is the top row, row increases downward, col increases rightward. That |
| internal invariant never changes, regardless of what convention a caller uses. |
| |
| Robotics/Cartesian convention (Y-up) instead has y increase upward from an |
| origin at the bottom-left. Converting between the two requires knowing the |
| grid's row count (M) to flip the vertical axis; column and x are equivalent. |
| |
| Callers may still specify/receive positions in cartesian — see |
| `RobotConfig.coordinate_format` (quantum/robotConfiguration.py), which calls |
| `to_matrix_rc` once on ingest (`resolve_coordinates`) and `to_robotics_xy` on |
| every read (`format_position`), and `quantum/visualizer.py`'s `convention` |
| param for display. These functions are the primitives those call; use them |
| directly for anything outside that path (e.g. converting a plain path list |
| before handing it to a robotics stack that expects Y-up). |
| """ |
| import math |
| from typing import List, Sequence, Tuple |
|
|
|
|
| def to_robotics_xy(row: int, col: int, num_rows: int) -> Tuple[int, int]: |
| """Convert a single (row, col) matrix cell to (x, y) robotics/Y-up coordinates.""" |
| x = col |
| y = (num_rows - 1) - row |
| return x, y |
|
|
|
|
| def to_matrix_rc(x: int, y: int, num_rows: int) -> Tuple[int, int]: |
| """Convert a single (x, y) robotics/Y-up cell back to (row, col) matrix coordinates.""" |
| row = (num_rows - 1) - y |
| col = x |
| return row, col |
|
|
|
|
| def path_to_robotics_xy(path: Sequence[Sequence[int]], num_rows: int) -> List[Tuple[int, int]]: |
| """Convert a path of [row, col] (or [row, col, t]) cells to (x, y) robotics/Y-up tuples.""" |
| return [to_robotics_xy(cell[0], cell[1], num_rows) for cell in path] |
|
|
|
|
| def path_to_matrix_rc(path: Sequence[Sequence[int]], num_rows: int) -> List[Tuple[int, int]]: |
| """Convert a path of [x, y] robotics/Y-up cells back to (row, col) matrix tuples.""" |
| return [to_matrix_rc(cell[0], cell[1], num_rows) for cell in path] |
|
|
|
|
| def flip_region_cartesian_to_matrix( |
| start: Sequence[int], end: Sequence[int], num_rows: int |
| ) -> Tuple[Tuple[int, int], Tuple[int, int]]: |
| """ |
| Convert an axis-aligned rectangle's corners from cartesian (x, y) to matrix |
| (row, col). Flipping the vertical axis inverts which corner has the smaller |
| row, so the two converted corners are re-sorted into a valid (start, end) |
| pair with start <= end on both axes — needed for e.g. map YAML `region` |
| blocks (see quantum/maps/yaml2HDF5.py's flip_map_config_to_matrix), which |
| are iterated as an inclusive range from start to end. |
| """ |
| r0, c0 = to_matrix_rc(start[0], start[1], num_rows) |
| r1, c1 = to_matrix_rc(end[0], end[1], num_rows) |
| return (min(r0, r1), min(c0, c1)), (max(r0, r1), max(c0, c1)) |
|
|
|
|
| def world_to_grid_cell( |
| x: float, y: float, num_rows: int, origin: Sequence[float], resolution: float |
| ) -> Tuple[int, int]: |
| """ |
| Convert a world-frame point (meters, ROS "map" frame -- e.g. an AMCL pose, |
| an rviz-clicked goal, a waypoint from a semantic map) into a Spooky matrix |
| (row, col) grid cell. |
| |
| `origin` is (x, y, yaw): the world pose of grid cell [num_rows-1, 0] |
| (bottom-left, ROS's OccupancyGrid index-0 convention) -- what a ROS map |
| .yaml's `origin` field, or an .h5 written by `quantum.maps.pgm2HDF5`, |
| already carries. `row` is Spooky's convention (row 0 = top), the |
| vertical mirror of ROS's bottom-up index. |
| |
| Raises ValueError if the point falls outside the grid -- most likely |
| because it's outside the mapped area, not because of a bug here. |
| """ |
| ox, oy, oyaw = origin |
| dx, dy = x - ox, y - oy |
| c, s = math.cos(oyaw), math.sin(oyaw) |
| local_x = dx * c + dy * s |
| local_y = -dx * s + dy * c |
|
|
| col = int(math.floor(local_x / resolution)) |
| row_from_bottom = int(math.floor(local_y / resolution)) |
| row = num_rows - 1 - row_from_bottom |
|
|
| if not (0 <= row < num_rows) or col < 0: |
| raise ValueError( |
| f"World point ({x}, {y}) maps to grid cell (row={row}, col={col}), " |
| f"which is outside this map -- check it's actually within the mapped area" |
| ) |
| return row, col |
|
|
|
|
| def grid_cell_to_world( |
| row: int, col: int, num_rows: int, origin: Sequence[float], resolution: float |
| ) -> Tuple[float, float]: |
| """ |
| Inverse of `world_to_grid_cell`: the world-frame (x, y) meters, map frame, |
| of the center of grid cell (row, col) -- e.g. to turn a decoded Spooky |
| path back into poses for a ROS controller. |
| """ |
| ox, oy, oyaw = origin |
| row_from_bottom = num_rows - 1 - row |
| local_x = (col + 0.5) * resolution |
| local_y = (row_from_bottom + 0.5) * resolution |
|
|
| c, s = math.cos(oyaw), math.sin(oyaw) |
| x = ox + local_x * c - local_y * s |
| y = oy + local_x * s + local_y * c |
| return x, y |
|
|
