The robot navigates from a start to a destination while avoiding static and moving obstacles, with MP4 video output. A 2D robot navigation simulation with A* global pathfinding, quadtree spatial indexing, multi-direction evasion, velocity profiling, and isometric 3D rendering.
This simulation demonstrates a complete robot navigation pipeline in a 2D world with isometric 3D visualization. The robot uses A* pathfinding over a discretized grid to compute a global path from start to destination, with three inflation tiers ensuring a path exists even in dense obstacle configurations. A quadtree spatial index provides efficient obstacle queries for both path planning and collision avoidance. At runtime, the robot follows the pre-planned path waypoints with a velocity profile, accelerating to maximum speed (2.0 u/tick) when the path is clear, and decelerating when moving obstacles are detected nearby. Obstacle detection uses point-to-segment distance from moving obstacle centers to the remaining path segments, with a safety margin (sm = ROBOT_SPEED + robot_half + obst_half + INFLATION) that accounts for both robot and obstacle dimensions plus the robot's per-tick displacement.
When a moving obstacle is detected within the safety margin, the robot evaluates 8 candidate directions (relative to path heading) plus staying in place. Each candidate is scored by: (1) proximity to the original path, (2) forward motion bonus, and (3) distance from the nearest moving obstacle. Candidates intersecting static or moving obstacles are rejected via quadtree queries and rect intersection tests. If all candidates are unsafe, a hard collision resolution step pushes the robot out of any residual overlap along the shortest separating axis. If the path remains blocked for WAIT_TICKS (5) consecutive ticks, the robot triggers an A* replan from its current position, computing a new global path that routes around the congested area. The evasion uses _path_inflation clearance from static obstacles, ensuring A* can always find the start cell unblocked during replanning. The simulation outputs MP4 video via imageio-ffmpeg, with a side panel displaying live metrics: time, distance, current speed, replan count, wait/move time distribution, path efficiency, and status.
graph TB
CFG[config.py<br/>Constants & Parameters]
subgraph Core
QT[quadtree.py<br/>Rect + QuadTree]
MG[map_generator.py<br/>Static & Moving Obstacles]
AS[astar.py<br/>A* Pathfinding]
SIM[simulation.py<br/>RobotNavigationSimulation<br/>+ Path Following + Evasion]
end
subgraph Renderer
SC[scene.py<br/>SimulationApp<br/>Isometric 3D]
PN[panel.py<br/>InfoPanel]
CP[capture.py<br/>VideoCapture]
end
subgraph Entry
M[main.py<br/>Orchestrator]
end
CFG --> MG
CFG --> SIM
CFG --> SC
MG --> QT
MG --> AS
QT --> AS
QT --> SIM
AS --> SIM
SIM --> SC
SC --> PN
SC --> CP
M --> SIM
M --> SC
CP --> MP4[test_runs/RS-*.mp4]
Robot-Navigation 2/
├── main.py # Entry point / orchestrator
├── config.py # Centralized configuration
├── core/
│ ├── __init__.py
│ ├── quadtree.py # Rect + QuadTree spatial index
│ ├── map_generator.py # Static & moving obstacle generation
│ ├── astar.py # A* pathfinding algorithm
│ └── simulation.py # Simulation engine + evasion + velocity control
├── renderer/
│ ├── __init__.py
│ ├── scene.py # Pygame isometric 3D renderer
│ ├── panel.py # Simulation info overlay panel
│ └── capture.py # MP4 video export
├── test_runs/ # Generated MP4 output
└── DOCUMENTATION.md # This file
classDiagram
class Rect {
+float x, y, w, h
+intersects(other) bool
+contains_point(p) bool
+center() tuple
+shares_edge(other) bool
}
class QuadTree {
+Rect bounds
+int capacity
+list objects
+bool divided
+list[N W, NE, SW, SE] children
+insert(rect)
+query(rect) list
+subdivide()
+get_leaves() list
+all_obstacles() list
}
class MovingObstacle {
+Rect rect
+list pos, _start, _end
+float speed, dx, dy
+update()
+pick_new_destination()
}
class RobotNavigationSimulation {
+Config c
+int tick, replan_count, waited_ticks
+float total_distance, robot_speed
+float robot_max_speed, robot_accel, robot_decel
+bool done, arrived
+list static_obstacles, moving_obstacles, path
+QuadTree quadtree
+list robot_pos
+tuple robot_dest
+int path_index
+robot_rect() Rect
+find_path() list
+_smooth_path(path) list
+_is_path_blocked() bool
+_position_free(x, y) tuple~None~
+_resolve_overlap()
+step() bool
+get_state() dict
}
class SimulationApp {
+RobotNavigationSimulation sim
+Config cfg
+InfoPanel panel
+VideoCapture capture
+_isom(x, y, z) tuple
+_draw_tile(x, y)
+_draw_box(...)
+_render(state)
+run()
}
class InfoPanel {
+Rect rect
+list _rows, _values
+tuple status
+update(state)
+draw(surface)
}
class VideoCapture {
+bool _enabled
+list _frames
+record(rgb_array)
+finalize()
}
QuadTree o--> Rect : bounds + objects
MovingObstacle --> Rect : contains
RobotNavigationSimulation --> QuadTree : queries
RobotNavigationSimulation --> MovingObstacle : updates
SimulationApp --> RobotNavigationSimulation : renders state
SimulationApp --> InfoPanel : displays metrics
SimulationApp --> VideoCapture : records frames
sequenceDiagram
participant M as main.py
participant S as simulation.py
participant MG as map_generator.py
participant QT as quadtree.py
participant A as astar.py
participant R as renderer/scene.py
M->>S: RobotNavigationSimulation(config, seed)
activate S
Note over S: Retry loop (up to 50 attempts)
S->>MG: generate_static_obstacles(…)
MG-->>S: List[Rect] (with .height)
S->>QT: QuadTree(bounds)
S->>QT: insert(rect) for each obstacle
S->>MG: generate_moving_obstacles(…)
MG-->>S: List[MovingObstacle]
loop For each inflation tier [2.0, 1.75, 1.5]
S->>A: astar(start, goal, quadtree, moving, inflation)
A-->>S: List[(x,y)] path or None
end
S->>S: _smooth_path(path) if enabled
S-->>M: simulation instance
deactivate S
M->>R: SimulationApp(sim, config)
activate R
R-->>M: app instance
loop Every tick
M->>R: app.run() → sim.step()
activate R
R->>S: step()
activate S
S->>MG: mo.update() for each moving obstacle
S->>S: _resolve_overlap() — hard separation
S->>S: _is_path_blocked() — point-to-segment check
alt Path clear
S->>S: Accelerate, follow waypoints
else Path blocked
S->>S: Decelerate, 8-direction evasion
alt Blocked > WAIT_TICKS
S->>A: find_path() — replan from current position
A-->>S: new path
end
end
S-->>R: done flag
alt FRAME_SKIP tick or done
R->>S: get_state()
S-->>R: state dict
R->>R: _render(state) — isometric 3D
R->>R: capture.record(frame)
end
deactivate S
end
deactivate R
The simulation runs in a synchronous tick loop at TIME_PER_TICK = 0.05s per tick. Each tick executes in this order:
-
Moving obstacles update — each
MovingObstaclemoves along its patrol path atMOVING_SPEEDper tick. If the new position collides with a static obstacle, it tries X-slide then Y-slide, or picks a new destination. Moving obstacles do not check against the robot — the robot is solely responsible for collision avoidance. -
Hard collision resolution — after all obstacles move,
_resolve_overlap()checks if any moving obstacle rect intersects the robot's rect. If so, it computes the overlap depth on each axis and pushes the robot out along the shortest separating axis. The new position is verified against both static (quadtree) and moving obstacles. This guarantees no overlap persists after any tick. -
Path blockage detection —
_is_path_blocked()computes the minimum point-to-segment distance from each moving obstacle's center to every remaining path segment (robot position → waypoints → destination). If any distance is≤ ROBOT_SPEED + robot_half + obst_half + INFLATION(≈ 5.5), the path is considered blocked. This margin is large enough that the robot has >1 tick of reaction time even at full speed. -
Decision:
- Path clear: robot accelerates (+0.1/tick) toward max speed (2.0) and moves toward the next waypoint at its current speed.
- Path blocked: robot decelerates (−0.2/tick) toward 0 and evaluates 8 candidate evasion directions plus staying in place. Candidates are filtered by static obstacle safety (quadtree query with
_path_inflationclearance) and moving obstacle safety (rect intersection). Remaining candidates are scored by proximity to original path, forward motion bonus, and distance from nearest moving obstacle. The robot moves to the best candidate. If blocked forWAIT_TICKS(5) consecutive ticks, A* replans from the current position.
-
State tracking — total distance, robot speed, waited ticks, and replan count are updated.
-
Rendering — every
FRAME_SKIPticks, the frame is captured and recorded for MP4 export.
Standard A* over a discretized grid.
Grid:
Heuristic (Euclidean, admissible):
Edge cost: $$ \text{cost}(a, b) = \begin{cases} \text{cell_size} & \text{if cardinal move} \ \text{cell_size} \times \sqrt{2} & \text{if diagonal move} \end{cases} $$
f-score:
Obstacle checking: Each cell is tested against the quadtree (static) and all moving obstacles with inflation padding:
8-directional neighbor expansion. Returns None if start or goal is blocked, or if no path exists.
flowchart TD
START([A* call]) --> CLAMP["Clamp start/goal to grid"]
CLAMP --> CHECK{"Start or goal\nblocked?"}
CHECK -->|Yes| NONE["Return None"]
CHECK -->|No| INIT["open_set = {start: f=0}"]
INIT --> LOOP{"open_set\nempty?"}
LOOP -->|Yes| NONE
LOOP -->|No| POP["Pop node with lowest f"]
POP --> GOAL{"node == goal?"}
GOAL -->|Yes| TRACE["Reconstruct path"]
TRACE --> SNAP["Snap path to cell centers"]
SNAP --> FIX["Fix start→exact, end→exact"]
FIX --> RET["Return path waypoints"]
GOAL -->|No| EXPAND["Expand 8 neighbors"]
EXPAND --> SKIP{"In bounds, unblocked,\nunvisited?"}
SKIP -->|No| LOOP
SKIP -->|Yes| UPDATE["Compute g, h, f"]
UPDATE --> PUSH["Push to open_set"]
PUSH --> LOOP
Uses point-to-segment distance to detect when a moving obstacle is near the robot's planned route.
Points:
For each moving obstacle, compute the minimum distance from its center to any segment
Safety margin:
If
The _point_seg_dist function computes the minimum distance from point
When the path is blocked, the robot evaluates 8 directions relative to the path heading, plus staying in place:
| Direction | Vector |
|---|---|
| Forward | |
| Forward-Right | |
| Right | |
| Backward-Right | |
| Backward | |
| Backward-Left | |
| Left | |
| Forward-Left |
Each candidate is generated at distance
Safety filters (candidate is rejected if any fails):
- Clamp to map bounds with
robot_half + _path_inflationmargin - Static obstacles: query quadtree with
_path_inflationclearance on candidate's rect - Moving obstacles: candidate rect intersects any
mo.rect
Scoring for valid candidates:
Where:
-
$\text{dist_to_path}$ — sum of point-to-segment distances to all path segments -
$\text{forward_comp} = \text{direction} \cdot F$ — alignment with path heading -
$\text{min_obs_dist}$ — Euclidean distance to nearest moving obstacle center
The candidate with the highest score is selected. If no candidate is valid (all blocked), the robot stays in place.
The robot uses acceleration/deceleration instead of constant speed:
| Situation | Speed Update | Time to Max/Stop |
|---|---|---|
| Path clear |
|
|
| Path blocked |
|
Where:
$v_{\max} = \text{ROBOT_SPEED} = 2.0$ -
$a = 0.1$ (acceleration per tick) -
$d = 0.2$ (deceleration per tick)
When following a waypoint, the robot moves:
When evading, it moves at
After every mo.update(), the simulation checks for robot-obstacle overlap:
-
For each moving obstacle intersecting
robot_rect(), compute overlap on each axis:$$o_x = \min(\text{rr.right} - \text{mo.left}, \text{mo.right} - \text{rr.left})$$ $$o_y = \min(\text{rr.bottom} - \text{mo.top}, \text{mo.top} - \text{rr.bottom})$$ -
Push out along the shortest axis:
- If
$o_x \leq o_y$ : horizontal push (left or right) - Otherwise: vertical push (up or down)
- If
-
Verify the new position via
_position_free()— checks against static obstacles (quadtree with_path_inflationclearance) and all moving obstacles. -
Fallback: try the other axis, then both-axes combinations. If no position is free, the robot stays in its current (overlapping) position.
Two-pass greedy shortcut algorithm:
- Forward pass: Walk from first waypoint; skip intermediate waypoints that have direct line-of-sight clearance (checked via quadtree + moving obstacles with
_path_inflationclearance) - Backward pass: Same line-of-sight shortcut removal on the remaining waypoints
Line-of-sight check: Sample points along the segment at
Point-region quadtree for efficient spatial queries.
Subdivision: When a leaf node exceeds capacity (default 4), it subdivides into 4 equal children (NW, NE, SW, SE quadrants).
Insert: Objects spanning boundaries are inserted into all intersecting children.
Query: Average
Guard: MIN_LEAF_SIZE = 1.0 prevents infinite subdivision.
graph TD
subgraph QuadTree Levels
R[Root: 0,0,ML,MB]
R --> A[NW: 0, MB/2, ML/2, MB/2]
R --> B[NE: ML/2, MB/2, ML/2, MB/2]
R --> C[SW: 0, 0, ML/2, MB/2]
R --> D[SE: ML/2, 0, ML/2, MB/2]
C --> CA[SW-NW: 0, MB/4, ML/4, MB/4]
C --> CB[SW-NE: ML/4, MB/4, ML/4, MB/4]
C --> CC[SW-SW: 0, 0, ML/4, MB/4]
C --> CD[SW-SE: ML/4, 0, ML/4, MB/4]
end
Static obstacles (generate_static_obstacles):
- Define exclusion zones (inflated by
$2 \times \max(RL, RB) + \text{inflation}$ ) around start and destination - Two placement modes (40% cluster / 60% single):
-
Cluster mode: Generate
$N$ small cells ($N \in [3, 6]$ ) around a random center with spacing 3, 70% fill probability -
Single mode: Random rectangle
$w, h \in [4, 12]$
-
Cluster mode: Generate
- Rejection sampling (up to 3000 attempts) until target
$\text{area} = M_L \times M_B \times \text{coverage}$ is reached - Each obstacle assigned a random height
$h \in [\text{height_min}, \text{height_max}]$
Moving obstacles (MovingObstacle class):
- Patrol between two random free positions
- Velocity:
$\mathbf{v} = \frac{\mathbf{end} - \mathbf{pos}}{|\mathbf{end} - \mathbf{pos}|} \times \text{speed}$ - Collision handling: Try X-slide, then Y-slide; if both fail, pick new destination
- On arrival (within 1.0u): pick new destination
-
find_free_position: Random rejection sampling with inflation checking (up to 200 attempts)
To ensure the A* planner finds a path even in dense maps, three inflation tiers are tried:
| Tier | Inflation | Margin | Purpose |
|---|---|---|---|
| 1 | INFLATION + robot_half (2.0) |
1.0 | Preferred — widest corridor |
| 2 | 1.75 | 0.75 | Fallback — moderate corridor |
| 3 | 1.5 | 0.5 | Minimum — tight corridor |
The first tier that produces a valid path is used for the entire simulation (_path_inflation). Evasion uses this same inflation for static obstacle clearance, ensuring A* can always find the start cell unblocked during replanning.
Uses a 2:1 dimetric projection (commonly called "isometric" in games).
With
Depth sorting (painter's algorithm): Obstacles are rendered in front-to-back order by their
Box rendering (
- Top face (base color)
- Left/right faces (darker shades)
- Front/back faces (intermediate shades)
Height coloring: Obstacle color is a function of its height, sampled from a 10-color gradient:
stateDiagram-v2
[*] --> Initializing
Initializing --> Running : Path found
Initializing --> Failed : No path after 50 attempts
Running --> PathClear : _is_path_blocked() false
Running --> PathBlocked : _is_path_blocked() true
PathClear --> FollowPath : Accelerate + move to waypoint
FollowPath --> Running : Next waypoint or arrived
PathBlocked --> Evade : Decelerate + 8-dir evaluation
Evade --> Running : Moved to safe position
PathBlocked --> Replan : Blocked for WAIT_TICKS
Replan --> Running : A* found new path
Running --> Arrived : Within 2.0u of destination
Running --> TimedOut : tick ≥ MAX_STEPS
Arrived --> [*]
TimedOut --> [*]
Failed --> [*]
| Constant | Default | Description |
|---|---|---|
| Map | ||
M_L, M_B |
250, 250 | Map dimensions (world units) |
| Robot | ||
RL, RB |
3, 3 | Robot length × breadth |
ROBOT_HEIGHT |
3 | Robot extrusion height (visual) |
ROBOT_SPEED |
2.0 | Maximum robot speed |
RS_X, RS_Y |
3, 3 | Start position |
RD_X, RD_Y |
245, 245 | Destination position |
| Static Obstacles | ||
OBSTACLE_COVERAGE |
0.2 | Fraction of map area occupied |
OBJECT_HEIGHT_MIN |
1 | Min obstacle height |
OBJECT_HEIGHT_MAX |
7 | Max obstacle height |
CLUSTER_ENABLED |
True | Enable cluster placement |
CLUSTER_MIN, CLUSTER_MAX |
3, 6 | Obstacles per cluster |
CLUSTER_SPACING |
3 | Spacing within cluster |
OBSTACLE_INFLATION |
0.5 | Obstacle padding during placement |
| Moving Obstacles | ||
OL, OB |
3, 3 | Moving obstacle dimensions |
MOVING_HEIGHT |
10 | Moving obstacle extrusion (visual) |
NUM_MOVING |
12 | Number of moving obstacles |
MOVING_SPEED |
1.5 | Speed per tick |
| Path Planning | ||
INFLATION |
0.5 | A* base cell inflation clearance |
CELL_SIZE |
2.0 | A* grid cell size |
SMOOTH_PATH |
True | Apply line-of-sight smoothing |
PATH_LOOKAHEAD |
4 | Unused (reserved) |
| Simulation | ||
MAX_STEPS |
2000 | Max ticks before timeout |
WAIT_TICKS |
5 | Consecutive blocked ticks before replan |
TIME_PER_TICK |
0.05 | Simulated seconds per tick |
FRAME_SKIP |
2 | Record every Nth frame |
| Rendering | ||
PANEL_WIDTH |
250 | Info panel width (px) |
SHOW_MOVING_TRAJECTORIES |
True | Show moving obstacle paths |
| Capture | ||
CAPTURE_MP4 |
True | Enable MP4 output |
CAPTURE_FPS |
10 | Output video framerate |
CAPTURE_BITRATE |
2000 | Video bitrate (kbps) |
CAPTURE_DIR |
"test_runs" | Output directory |
| Pattern | Location | Usage |
|---|---|---|
| Spatial Partitioning | core/quadtree.py |
Quadtree for O(log n) collision queries |
| State Machine | core/simulation.py |
Simulation states: path clear, path blocked, replan, arrived, timeout |
| Model-View Separation | core/, renderer/ |
Simulation logic independent of rendering |
| Strategy | core/simulation.py |
Multi-inflation tier fallback strategy |
| Factory | core/map_generator.py |
Obstacle generation functions |
| Rejection Sampling | core/map_generator.py |
Random placement with collision rejection |
| Velocity Profile | core/simulation.py:step |
Acceleration/deceleration based on path state |
| Multi-Direction Evaluation | core/simulation.py:step |
Score 8 candidates + stay for evasion |
| Painter's Algorithm | renderer/scene.py:188 |
Depth-sorted obstacle rendering |
| Video Pipeline | renderer/capture.py |
Frame buffering + FFmpeg encoding |
The simulation tracks and displays (via InfoPanel):
| Metric | Derivation |
|---|---|
| Time elapsed | tick × TIME_PER_TICK |
| Total distance | Sum of per-tick Euclidean displacement |
| Current speed | Instantaneous robot speed (0.0–2.0) |
| Re-plans | Count of A* re-invocations due to persistent blockage |
| Wait / Move time | Waited ticks × TIME_PER_TICK / elapsed |
| Path efficiency | total_distance / straight_line_distance × 100% |
| Status | ARRIVED, TIMEOUT, or IN PROGRESS |
| Library | Usage |
|---|---|
| Pygame | Isometric 3D rendering, event loop |
| imageio | MP4 video writing via FFmpeg |
| imageio-ffmpeg | FFmpeg backend for imageio |
| numpy | Frame array manipulation |
Standard library: math, heapq, random, os, glob, re, sys, datetime
cd "Robot-Navigation 2" && python main.pyOutput MP4s are saved to test_runs/RS-{serial}-{YYYYMMDD}_{HHMMSS}.mp4. Each run uses a random seed (printed to stdout for reproducibility). Press ESC or close the window to stop early.