3D Motion Planning

Overview

Motion planning is the problem of finding a collision-free path from a start configuration to a goal in a space cluttered with obstacles. This project implements and compares two fundamentally different strategies — sampling-based (Bi-RRT) and graph-search (Weighted A*) — across seven 3D environments of varying complexity: a single cube, a maze, flappy-bird-style corridors, pillars, a window gap, a tower, and a room.

Collision detection uses vectorized axis-aligned bounding box (AABB) segment tests via the slab method. The improved Bi-RRT with a greedy-connect extension now successfully solves all seven environments, including topologically complex ones. However, a key trade-off remains: Bi-RRT finds significantly longer paths on narrow-passage maps (maze: 135.3 vs. A*'s 91.9), while Weighted A* reliably produces near-optimal paths at the cost of higher computation time on complex environments.

Method

🌳

Bi-directional RRT

Grows two random trees simultaneously — one from the start, one from the goal — using goal-biased sampling (p_goal = 0.05). After each extension step, a greedy-connect phase steps the opposite tree toward the new node until blocked or connected (gap < step/2). Trees swap roles each iteration for balanced growth. Fast on open spaces; finds longer paths on narrow passages.

x_new = steer(x_rand, x_near, step=0.5)

⭐

Weighted A*

Searches a voxelized grid with resolution 0.5 m and 26-neighbor connectivity (±1 in each axis). The weight parameter w ∈ {1.0, 1.2, 1.5} scales the heuristic: w = 1.0 is optimal A*, higher weights trade path quality for speed. Guarantees a solution in all tested environments; slower on mazes due to exhaustive grid expansion.

f(n) = g(n) + w · ‖n − goal‖

3D Map
AABB blocks

→

Sample / Grid
RRT / A*

→

Collision AABB
slab method

→

Path Graph
nodes / edges

→

Backtrack
goal → start

→

Final Path
3D waypoints

Algorithm Flowcharts

Step-by-step logic for both planners, including the greedy-connect extension in Bi-RRT.

🌳 Bi-directional RRT

flowchart TD S([Start]) --> I["Init: Va ← {start}, Vb ← {goal}"] I --> L{"iter < max_iter?"} L -- No --> N([Return None]) L -- Yes --> SA["Sample q_rand\n(p=0.05 toward Vb root)"] SA --> NA["q_near_a = nearest(Va, q_rand)"] NA --> ST["q_new = steer(q_near_a, q_rand)\nstep = 0.5 m"] ST --> CF{"Inside boundary &\ncollision-free?"} CF -- "No → Swap" --> L CF -- Yes --> ADD["Add q_new to Va"] ADD --> GC["Greedy-connect: step Vb → q_new\nuntil blocked or gap < step/2"] GC --> CO{"Trees\nconnected?"} CO -- Yes --> P([Return Path ✓]) CO -- No --> SW["Swap Va ↔ Vb"] SW --> L style S fill:#eff6ff,stroke:#2563eb,color:#1d4ed8 style N fill:#fef2f2,stroke:#dc2626,color:#b91c1c style P fill:#f0fdf4,stroke:#16a34a,color:#15803d style CF fill:#fffbeb,stroke:#d97706,color:#92400e style CO fill:#fffbeb,stroke:#d97706,color:#92400e style L fill:#fffbeb,stroke:#d97706,color:#92400e style ADD fill:#f0fdf4,stroke:#86efac,color:#15803d

⭐ Weighted A*

flowchart TD S([Start]) --> I["Init heap: push (w·h(start), start)\ng(start) = 0"] I --> HE{"Heap\nempty?"} HE -- Yes --> N([Return None]) HE -- No --> PO["Pop cur with min f-score"] PO --> GE{"cur == goal?"} GE -- Yes --> P([Return Path ✓]) GE -- No --> EX["For each of 26 neighbors nbr\n(±1 in x, y, z) × res"] EX --> CF{"Collision-free?\n(AABB slab test)"} CF -- No --> MORE{"More\nneighbors?"} CF -- Yes --> TG["tentative_g = g(cur) + res"] TG --> BT{"tentative_g < g(nbr)?"} BT -- No --> MORE BT -- Yes --> UP["Update came_from(nbr), g(nbr)\nf = tentative_g + w·‖nbr−goal‖\nPush (f, nbr) to heap"] UP --> MORE MORE -- Yes --> EX MORE -- No --> HE style S fill:#eff6ff,stroke:#2563eb,color:#1d4ed8 style N fill:#fef2f2,stroke:#dc2626,color:#b91c1c style P fill:#f0fdf4,stroke:#16a34a,color:#15803d style HE fill:#fffbeb,stroke:#d97706,color:#92400e style GE fill:#fffbeb,stroke:#d97706,color:#92400e style CF fill:#fffbeb,stroke:#d97706,color:#92400e style BT fill:#fffbeb,stroke:#d97706,color:#92400e style MORE fill:#fffbeb,stroke:#d97706,color:#92400e style UP fill:#f0fdf4,stroke:#86efac,color:#15803d

Results

Bi-RRT vs. Weighted A* (w = 1.0) across all seven test environments. All runs successful.

Environment	Bi-RRT	RRT Time (s)	RRT Length	Weighted A*	A* Time (s)	A* Length
Single Cube	✓	0.008	8.39	✓	0.010	7.98
Maze	✓	8.566	135.31	✓	8.918	91.99
Flappy Bird	✓	0.074	42.69	✓	2.500	28.80
Pillars	✓	0.010	36.23	✓	0.170	32.49
Window	✓	0.012	28.11	✓	4.183	29.72
Tower	✓	0.213	44.86	✓	2.116	32.85
Room	✓	0.037	19.75	✓	0.242	13.68

Weighted A* — effect of weight parameter on planning time and path length.

Environment	w = 1.0 Time	w = 1.0 Length	w = 1.2 Time	w = 1.2 Length	w = 1.5 Time	w = 1.5 Length
Single Cube	0.010 s	7.98	0.011 s	7.98	0.010 s	7.98
Maze	8.918 s	91.99	11.086 s	91.99	12.757 s	91.99
Flappy Bird	2.500 s	28.80	2.144 s	28.80	1.232 s	29.41
Pillars	0.170 s	32.49	0.073 s	32.08	0.060 s	31.89
Window	4.183 s	29.72	0.138 s	28.68	0.093 s	28.63
Tower	2.116 s	32.85	2.583 s	32.85	1.149 s	33.43
Room	0.242 s	13.68	0.186 s	13.68	0.164 s	13.68

Bi-RRT — Path Visualizations

single_cube — 0.008 s · L = 8.39

maze — 8.566 s · L = 135.31

flappy_bird — 0.074 s · L = 42.69

pillars — 0.010 s · L = 36.23

window — 0.012 s · L = 28.11

tower — 0.213 s · L = 44.86

room — 0.037 s · L = 19.75

performance — time & path length comparison

Weighted A* (w = 1.0) — Path Visualizations

single_cube — 0.010 s · L = 7.98

maze — 8.918 s · L = 91.99

flappy_bird — 2.500 s · L = 28.80

pillars — 0.170 s · L = 32.49

window — 4.183 s · L = 29.72

tower — 2.116 s · L = 32.85

room — 0.242 s · L = 13.68

performance — time & path length comparison

Tech Stack

Python 3.10

Runtime

NumPy

Spatial computations

Matplotlib

Path visualization

Custom AABB

Vectorized collision detection

Configuration

Parameter	Value	Description
`max_iter`	500,000	Bi-RRT max iterations
`step_size`	0.5 m	RRT steer & greedy-connect step
`p_goal`	0.05	Goal-bias sampling probability
`connect_thr`	step / 2	Tree connection gap threshold
`grid_res`	0.5 m	A* voxel grid resolution
Connectivity	26-neighbor	±1 in x, y, z axes
`weight w`	1.0 / 1.2 / 1.5	A* heuristic inflation factor