Introduction
In the rapidly evolving field of robotics and autonomous systems, finding optimal paths through complex environments remains a fundamental challenge. Traditional path planning algorithms excel at discovering a single shortest route, but modern applications demand something more sophisticated: the ability to identify multiple distinct optimal paths that navigate around obstacles, high-cost regions, and topological features simultaneously.
Consider a tethered quadrotor drone exploring a multi-room building structure, or autonomous robots coordinating movement to avoid congestion. These scenarios require algorithms that don’t merely optimize for distance but understand the geometric and topological nuances of space. This is where neighborhood-augmented graphs (NAGs) represent a breakthrough approach.
Unlike existing topological path planning methods that require complex geometric constructions and are difficult to implement in three dimensions, NAGs offer an elegant solution that works across varying configuration spaces without necessitating advance knowledge of environmental topology. Researchers at Lehigh University have developed this innovative framework, demonstrating practical applications from multi-robot coordination to real-world tethered drone experiments.
This article explores how neighborhood-augmented graphs work, why they matter for modern robotics, and how they solve the k-geodesic path planning problem—finding k multiple optimal paths that are genuinely distinct both topologically and geometrically.
The Challenge: Why Traditional Path Planning Falls Short
Understanding the Limitations of Current Approaches
Modern path planning algorithms fall into two primary categories: graph-search methods and sampling-based algorithms. Graph-search approaches like Dijkstra’s algorithm and A* are complete and optimal in lower dimensions but typically find only a single shortest path. Sampling-based methods like RRT* offer scalability to higher dimensions but provide only probabilistic completeness and cannot guarantee optimal solutions.
Neither approach effectively addresses the k-geodesic path planning problem—finding k distinct optimal paths connecting a start and goal configuration.
The traditional paradigm has three critical limitations:
- Single-path bias: Conventional algorithms maintain only one optimal path per configuration, discarding alternatives
- Topological fixation: Existing topological path planning (TPP) methods can distinguish between nonhomotopic paths but fail to identify distinct geodesics within the same homotopy class
- 3D complexity burden: TPP methods require increasingly complex geometric constructions in three dimensions, involving surface computations and group-theoretic reasoning that become computationally prohibitive
Consider Figure 1 from the research: in 2D environments, rays emanating from obstacle representatives generate what’s called h-signatures for homotopy classification. But in 3D, these rays must be replaced by surfaces with topology bounded by obstacles—structures that are exponentially more difficult to construct and analyze.
Real-World Implications
These limitations create practical problems:
- Multi-robot teams cannot efficiently distribute across alternative routes to minimize congestion
- Tethered robots in 3D environments cannot account for cable configurations that belong to the same topological class but violate length constraints differently
- Search-and-rescue operations cannot present operators with meaningful alternative paths around complex obstacles
What Are Neighborhood-Augmented Graphs?
The Foundational Concept
A neighborhood-augmented graph extends the traditional discrete graph representation of configuration space by augmenting each vertex with path neighborhood information. Rather than storing only a configuration point q, each vertex stores a tuple (q, U) where U is a path neighborhood set (PNS)—essentially a collection of vertices representing the spatial neighborhood around the path leading to that configuration.
Core principle: Two vertices at identical coordinates but reached via different path branches can now be distinguished and maintained as separate entities within the graph, enabling the discovery of multiple optimal paths.
How Path Neighborhoods Work
The mathematical foundation relies on a elegant observation about geodesics:
Definition: Two distinct geodesic paths connecting start and goal points must intersect at some point where their tangent vectors differ, causing the paths to diverge at that location.
Rather than computing tangent vectors directly (computationally expensive and complex), the NAG approach uses path neighborhoods as proxies. The tangent space of a configuration space at a point is reasonably represented by a small neighborhood around that point. Therefore, the tangent vector to a path at a point can be approximated by examining a neighborhood around the path itself. Path Neighborhood≈Tangent Vector Information\text{Path Neighborhood} \approx \text{Tangent Vector Information}Path Neighborhood≈Tangent Vector Information
This insight transforms an abstract geometric problem into a discrete, computable process suitable for graph search algorithms.
Creating and Maintaining Multiple Vertices
During wavefront propagation (the core of graph search algorithms):
- Primary search begins from a start configuration, expanding vertices using standard Dijkstra’s or A* algorithms
- Secondary search (neighborhood search) is performed for each newly generated vertex, computing the path neighborhood set up to distance rn
- Vertex equivalence check compares both configuration coordinates AND path neighborhoods using the relation: (q, U) ≡ (q’, U’) if q ≡ q’ AND U ∩ U’ ≠ ∅
When two search branches meet at the same configuration but have disjoint path neighborhoods, they create distinct vertices in the NAG, enabling the algorithm to maintain and separately develop distinct paths.
The Mathematical Framework Behind NAGs
Topo-Geometrically Distinct Paths
Definition: Two distinct geodesic paths connecting start and goal points are topo-geometrically distinct if there exists a common point where they possess different path neighborhoods.
This definition captures something more nuanced than traditional homotopy classes:
- Topological distinctness: Paths in different homotopy classes (separated by obstacles they cannot cross)
- Geometric distinctness: Paths in the same homotopy class but separated by high-cost regions, curved surfaces, or obstacles without holes
This dual distinction allows NAGs to find paths that existing TPP methods miss entirely.
Key Theoretical Results
Proposition 1 provides a sufficient condition for creating topo-geometrically distinct branches:
\[ \text{If } \text{sep}(\gamma’_1, \gamma’_2) > \frac{4r_n}{1-\omega} \text{ then } U_1 \cap U_2 = \emptyset \]Where:
- sep(γ’₁, γ’₂) represents the separation between path segments
- rn is the neighborhood radius parameter
- ω is the heuristic weight controlling neighborhood shape
Proposition 2 offers a more practical guideline using closed geodesics:
\[ \text{Picking } r_n < \frac{l(c^*)}{2} \text{ ensures } U_1 \cap U_2 = \emptyset \]Where l(c*) is the length of the shortest closed geodesic around obstacles or cost regions. This result is crucial because it relates the neighborhood radius parameter to intrinsic properties of the configuration space rather than requiring path-specific information.
Neighborhood Shape and Heuristic Weighting
The research identifies that path neighborhoods can take different shapes:
- Disk-shaped neighborhoods (ω = 0): Simple circular regions around vertices, but more sensitive to size parameters
- Path-hugging neighborhoods (ω > 0): Follow the actual path more closely, providing better distinction between nearby paths with smaller parameter sensitivity
The heuristic weight ω ∈ [0,1] controls this behavior by influencing the secondary A* search through the equation:
\[ \tilde{f}(v) = \tilde{g}(v) + \omega \cdot g(v) \]Where g(v) is the g-score from the primary search, acting as a heuristic to guide the secondary search toward the path.
Algorithm Design and Implementation
The Primary Search Process
Algorithm 1 (Incremental Construction and Dijkstra’s Search in NAG) forms the core:
Input:
- Start configuration qs
- Adjacency function A (describing graph connectivity)
- Cost function C
- Stopping criteria (function)
Key Process:
- Initialize start vertex vs = (qs, {&vs}) with self-reference in its PNS
- Maintain open and closed lists using heap data structure
- For each expanded vertex v = (q, U):
- Compute new PNS U’ using secondary search
- Generate adjacent vertices (q’, U’)
- Check vertex equivalence: if (q’, U’) ≡ w in VN, don’t create duplicate
- Otherwise, add as new vertex with updated g-scores
- Continue until reaching goal k times
Computational Complexity: O(kn|V| log|V|), where k_n is constant-time complexity of PNS generation. This equals the complexity of the underlying search algorithm with a constant multiple—a significant advantage over methods requiring complex geometric constructions.
Neighborhood Generation via Secondary Search
Algorithm 3 (PNS Computation Using A* Search) computes the path neighborhood for each vertex:
Key parameters:
- rn (neighborhood radius): Distance threshold defining PNS extent
- ω (heuristic weight): Controls path-hugging vs. disk-shaped neighborhoods
- rb (rollback amount): When starting secondary search (accommodates certain primary algorithms like S*)
The secondary search runs on the partially-constructed NAG, using the stopping condition that g-score ≤ rn. This ensures neighborhoods remain computationally tractable—they expand only locally rather than globally.
Handling Pathological Cases: Cut Point Identification
In extremely low-curvature environments (flat regions with slight cost variations, prismatic obstacle corners), the main NAG algorithm struggles because path neighborhoods maintain overlap even for significantly different paths.
Solution: Cut Point Identification and CPR (Cut Point Region) Generation
When two paths with nearly-overlapping neighborhoods meet:
- Detect near-cut point by measuring neighborhood intersection ratio α
- Verify similarity of path lengths using g-score differences
- Check separation between path segments at appropriate distance levels
- Generate CPR as an artificial obstacle around the detected cut point
- Re-run search allowing separate branches to form downstream
This adaptation handles scenarios where geodesic paths nearly coincide, extracting distinctions that would otherwise be missed.
Applications: Tethered Robot Motion Planning in 3D
Problem Formulation
Tethered Robot Shortest Path Planning Problem: Given a mobile robot connected to a fixed base via an inextensible tether of maximum length L, find the globally shortest path to a goal pose that satisfies the tether length constraint without wrapping violations.
This problem is fundamentally different from unconstrained path planning because:
- Infinite-dimensional configuration space: The space of all tether configurations (curves of length L) is infinite-dimensional
- Multiple valid configurations per pose: The same robot position can be reached with different tether routings, each with different length requirements
- Homotopy inadequacy: In 3D, topological equivalence (homotopy) doesn’t guarantee that tether length constraints remain satisfied when deforming between paths
Solution Strategy: Two-Stage Search
Stage 1: Tethered Configuration Space Construction
- Construct a length-constrained NAG starting from base vertex vb
- Expand every neighborhood-augmented vertex reachable within tether length L
- Store topological and geometric configuration information in each vertex’s PNS
- Use S* algorithm to ensure accurate shortest path computation
Stage 2: Length Constrained Search (LCS)
- Perform search on the pre-constructed length-constrained NAG
- Start from initial robot pose and tether configuration
- Expand until reaching goal pose with any valid tether configuration
- Extract shortest path satisfying constraints
This two-stage approach elegantly handles the infinite-dimensional tether configuration space by discretizing it through geodesic path representations.
Real-World Validation
The research includes experimental validation using a Crazyflie 2.1 quadrotor in a building-like environment:
- Physical setup: Styrofoam blocks forming structure with rooms and windows; wool thread as tether
- Planning algorithm: NAG-based planning with inflated obstacles for safety
- Trajectory optimization: External trajectory optimizer converts discrete path to smooth flight path
- Control: OptiTrack motion capture with Crazyswarm framework
Results demonstrated:
- Robot successfully navigates through constrained tether length
- Selects alternative routes when globally shortest path violates constraints
- Executes multi-goal sequences with dynamic tether configuration management
- Configuration space construction: 52 seconds; Length constrained search: 3 seconds
Performance Characteristics and Practical Considerations
Computational Efficiency
| Environment Type | Dimension | Expanded Vertices | Computation Time | Paths Found |
|---|---|---|---|---|
| Building-like (obstacles) | 2D | 15,297 | 7s | 3 |
| Multiple hills (high-cost) | 2D | 51,779 | 35s | 3 |
| Genus-1 object | 3D | 3,585 | 9s | 2 |
| Genus-2 object | 3D | 8,540 | 24s | 3 |
| Chain-like structure | 3D | 34,247 | 76s | 4 |
Memory requirements: Approximately 30 kB per vertex in 2D, 168 kB per vertex in 3D. This represents a substantial but manageable overhead compared to traditional graph search.
Computational time breakdown: Approximately 68% spent on neighborhood search and comparison, with remaining time on primary search operations. This ratio highlights that PNS computation is the computational bottleneck but remains tractable due to constant-time per-vertex complexity.
Parameter Selection Guidelines
Using Proposition 2 for rn selection:
- Identify artifacts: obstacles, high-cost regions, or topological features
- Estimate closed geodesic length:
- For obstacles: use perimeter
- For high-cost regions: compute costs around region boundaries
- For topological features: use relevant characteristic distances
- Set rn < l (c) / 2*: Choose neighborhood radius below half the shortest closed geodesic length
- Account for rollback: If using rollback amount r_b, reduce r_n further to maintain theoretical guarantees
Practical tuning parameters:
- ω = 0.6 found effective for balanced path-hugging behavior
- Rollback of rb = 3 suitable for S* algorithm
- Lower bound on neighborhood search depth prevents excessive sparsity in high-cost regions
Advantages Over Existing Methods
Versus Topological Path Planning
| Aspect | Traditional TPP | NAG Approach |
|---|---|---|
| Geometric constructions required | Yes (complex in 3D) | None |
| Distinguishes same-homotopy paths | No | Yes |
| Scalability to 3D | Poor | Excellent |
| Handles high-cost regions | Limited | Excellent |
| Local vs. global information | Requires global topology | Local only |
Versus Sampling-Based Methods (RRT*)
NAGs provide systematic path discovery, not random sampling. When tested on identical environments:
- NAG: Found 3 distinct geodesics in 20 seconds on a 2D environment with obstacles and high-cost regions
- RRT*: Executed 1000 iterations (57 seconds total) but found only 2 homotopy classes, missing geometric distinctions entirely
RRT* succeeded at optimal path discovery in 999/1000 runs but provided no systematic mechanism for multiple distinct paths.
Fundamental Innovation
The core innovation isn’t merely finding multiple paths—it’s doing so through local information only. NAGs require only:
- Adjacency relationships between nearby configurations
- Local cost calculations
- No global visibility checks or coordinate chart dependencies
This locality principle makes the approach:
- Configuration-space agnostic: Works on Euclidean subsets, manifolds, and even non-Euclidean spaces (demonstrated on cylindrical surfaces)
- Scalable: Complexity grows predictably with dimension
- Robust: Doesn’t depend on user-specified global parameters about environment structure
Limitations and Future Directions
Current Limitations
- Heuristic restriction: Theoretical results assume uniform wavefront propagation, limiting use of heuristic functions that could accelerate primary search
- Discretization dependency: Performance depends on graph resolution; very coarse discretization may create suboptimality
- Parameter tuning: While Proposition 2 provides guidance, accurate closed geodesic estimation remains challenging in complex environments
- Computational overhead: 68% of computation time on neighborhood operations may become limiting in very high dimensions
Promising Future Research
- Heuristic adaptation: Extending theoretical results to accommodate goal-directed heuristics in primary search
- Sampling-based integration: Combining neighborhood augmentation with RRT* and PRM approaches for higher-dimensional problems
- Robust parameter learning: Machine learning techniques to automatically select r_n and ω for unknown environments
- Dynamic environments: Adaptation for moving obstacles and time-varying cost landscapes
- Constraint handling: Generalization to problems with additional constraints beyond tether length
Conclusion: Enabling Intelligent Multi-Path Planning
Neighborhood-augmented graphs represent a paradigm shift in how we approach path planning for autonomous systems. By maintaining rich topological and geometric information at each configuration, NAGs enable algorithms to discover not just one optimal path, but multiple geodesically and topologically distinct alternatives—all without requiring the complex geometric constructions that have limited traditional methods.
Key takeaways for practitioners:
- NAGs solve the k-geodesic problem systematically, not through randomized sampling
- Local information suffices, making the approach scalable and generalizable
- Real-world applications are feasible, as demonstrated by tethered quadrotor experiments
- Theoretical guarantees are available through propositions relating parameters to configuration space properties
From multi-robot coordination avoiding congestion to tethered robots navigating complex 3D structures, neighborhood-augmented graphs unlock motion planning capabilities previously inaccessible to deterministic algorithms.
Ready to Explore Advanced Path Planning for Your Robotics Projects?
Whether you’re developing autonomous systems, optimizing multi-robot coordination, or tackling constrained motion planning challenges, understanding neighborhood-augmented graphs provides a powerful new tool for your algorithm development toolkit.
What’s your current path planning challenge? Share your specific application or constraint in the comments below, and let’s discuss how NAG-based approaches might solve it. For researchers interested in implementation details, the open-source code is available on GitHub—link provided in the original paper. Don’t miss the opportunity to advance beyond single-path planning into the sophisticated multi-geodesic realm that modern autonomous systems demand.
The full paper is available here. (https://ieeexplore.ieee.org/document/10746348)
Here is a comprehensive end-to-end implementation of the Neighborhood-Augmented Graph (NAG) path planning algorithm.
import numpy as np
import heapq
from dataclasses import dataclass, field
from typing import List, Tuple, Set, Dict, Optional, Callable
from collections import defaultdict
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, Circle
from matplotlib.collections import LineCollection
# ============================================================================
# DATA STRUCTURES
# ============================================================================
@dataclass
class Vertex:
"""Represents a vertex in the configuration space"""
config: Tuple[float, ...] # Configuration coordinates
pns: Set['Vertex'] = field(default_factory=set) # Path Neighborhood Set
g_score: float = float('inf') # Cost from start
came_from: Optional['Vertex'] = None
vertex_id: int = 0
def __hash__(self):
return id(self)
def __eq__(self, other):
return id(self) == id(other)
def __lt__(self, other):
return self.g_score < other.g_score
def __repr__(self):
return f"V({self.config}, g={self.g_score:.2f})"
class NAGVertex:
"""Neighborhood-Augmented Graph vertex: (config, PNS)"""
_id_counter = 0
def __init__(self, config: Tuple[float, ...], pns: Set[int] = None):
self.config = config
self.pns = pns if pns is not None else set() # Set of vertex IDs
self.g_score = float('inf')
self.came_from = None
self.vertex_id = NAGVertex._id_counter
NAGVertex._id_counter += 1
def __hash__(self):
return self.vertex_id
def __eq__(self, other):
return self.vertex_id == other.vertex_id
def __lt__(self, other):
return self.g_score < other.g_score
def is_equivalent(self, other: 'NAGVertex') -> bool:
"""Check if two vertices are equivalent: same config AND overlapping PNS"""
if self.config != other.config:
return False
return len(self.pns & other.pns) > 0
def __repr__(self):
return f"NAG({self.config}, g={self.g_score:.2f}, pns_size={len(self.pns)})"
# ============================================================================
# CONFIGURATION SPACE GRAPH
# ============================================================================
class ConfigurationSpaceGraph:
"""Discrete graph representation of configuration space"""
def __init__(self, bounds: List[Tuple[float, float]], resolution: float):
"""
Initialize configuration space graph
Args:
bounds: List of (min, max) tuples for each dimension
resolution: Grid resolution
"""
self.bounds = bounds
self.resolution = resolution
self.dimensions = len(bounds)
self.vertices = {} # Dict[config tuple -> vertex]
self.edges = defaultdict(list) # Dict[config -> adjacent configs]
self.obstacles = []
self.cost_map = {} # Dict[config -> cost multiplier]
self._build_grid()
def _build_grid(self):
"""Build grid vertices within bounds"""
ranges = [np.arange(b[0], b[1] + self.resolution, self.resolution)
for b in self.bounds]
if self.dimensions == 2:
for x in ranges[0]:
for y in ranges[1]:
config = (round(x, 4), round(y, 4))
self.vertices[config] = Vertex(config)
elif self.dimensions == 3:
for x in ranges[0]:
for y in ranges[1]:
for z in ranges[2]:
config = (round(x, 4), round(y, 4), round(z, 4))
self.vertices[config] = Vertex(config)
def add_obstacle(self, center: Tuple[float, ...], radius: float):
"""Add circular/spherical obstacle"""
self.obstacles.append((center, radius))
self._mark_obstacle_vertices(center, radius)
def _mark_obstacle_vertices(self, center: Tuple[float, ...], radius: float):
"""Mark vertices inside obstacle as invalid"""
for config in list(self.vertices.keys()):
dist = np.linalg.norm(np.array(config) - np.array(center))
if dist < radius:
del self.vertices[config]
def add_high_cost_region(self, center: Tuple[float, ...], radius: float,
multiplier: float):
"""Add high-cost region (doesn't block, but increases traversal cost)"""
for config in self.vertices.keys():
dist = np.linalg.norm(np.array(config) - np.array(center))
if dist < radius:
self.cost_map[config] = multiplier
def get_adjacency(self, config: Tuple[float, ...]) -> List[Tuple[float, ...]]:
"""Return valid adjacent configurations (8-connected in 2D)"""
if config not in self.vertices:
return []
adjacent = []
offsets = self._get_neighbor_offsets()
for offset in offsets:
neighbor = tuple(round(c + o * self.resolution, 4)
for c, o in zip(config, offset))
if neighbor in self.vertices:
adjacent.append(neighbor)
return adjacent
def _get_neighbor_offsets(self) -> List[Tuple[int, ...]]:
"""Get neighbor offsets based on dimension"""
if self.dimensions == 2:
return [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]
elif self.dimensions == 3:
offsets = []
for dx in [-1, 0, 1]:
for dy in [-1, 0, 1]:
for dz in [-1, 0, 1]:
if (dx, dy, dz) != (0, 0, 0):
offsets.append((dx, dy, dz))
return offsets
def get_cost(self, config1: Tuple[float, ...],
config2: Tuple[float, ...]) -> float:
"""Calculate edge cost with high-cost region multiplier"""
euclidean = np.linalg.norm(
np.array(config2) - np.array(config1)
)
# Apply cost multiplier if in high-cost region
avg_config = tuple((c1 + c2) / 2 for c1, c2 in zip(config1, config2))
multiplier = self.cost_map.get(avg_config, 1.0)
return euclidean * multiplier
# ============================================================================
# NEIGHBORHOOD-AUGMENTED GRAPH
# ============================================================================
class NeighborhoodAugmentedGraph:
"""Neighborhood-Augmented Graph for computing k geodesic paths"""
def __init__(self, cs_graph: ConfigurationSpaceGraph,
r_n: float = 8.0, omega: float = 0.6, r_b: int = 0):
"""
Initialize NAG
Args:
cs_graph: Configuration space graph
r_n: Neighborhood radius
omega: Heuristic weight (0=disk, 1=full path-hugging)
r_b: Rollback amount for grandparent start
"""
self.cs_graph = cs_graph
self.r_n = r_n
self.omega = omega
self.r_b = r_b
self.vertices_nag: List[NAGVertex] = []
self.vertex_map: Dict[int, NAGVertex] = {} # vertex_id -> NAGVertex
self.open_list = [] # Priority queue (g_score, vertex_id, NAGVertex)
self.closed_list: Set[int] = set()
self.paths_found: List[List[Tuple[float, ...]]] = []
def compute_pns(self, v_parent: NAGVertex,
all_vertices: Dict[int, NAGVertex]) -> Set[int]:
"""
Compute Path Neighborhood Set using A* search
Algorithm 3 from paper
"""
pns = set()
open_pns = []
closed_pns = set()
# Rollback to appropriate ancestor
start_vertex = v_parent
for _ in range(min(self.r_b, 1)):
if start_vertex.came_from:
start_vertex = start_vertex.came_from
heapq.heappush(open_pns, (0, start_vertex.vertex_id, start_vertex))
pns.add(start_vertex.vertex_id)
visited_g = {start_vertex.vertex_id: 0}
while open_pns:
_, v_id, v = heapq.heappop(open_pns)
if v_id in closed_pns:
continue
closed_pns.add(v_id)
g_score = visited_g.get(v_id, float('inf'))
if g_score > self.r_n:
continue
# Get adjacent vertices in current NAG
for neighbor_config in self.cs_graph.get_adjacency(v.config):
cost = self.cs_graph.get_cost(v.config, neighbor_config)
new_g = g_score + cost
# Find matching neighbor in NAG
for other in all_vertices.values():
if other.config == neighbor_config and new_g < visited_g.get(other.vertex_id, float('inf')):
visited_g[other.vertex_id] = new_g
# A* with heuristic weight
h_score = other.g_score if other.g_score != float('inf') else 0
f_score = new_g + self.omega * h_score
heapq.heappush(open_pns, (f_score, other.vertex_id, other))
pns.add(other.vertex_id)
break
return pns
def search(self, start: Tuple[float, ...], goal: Tuple[float, ...],
k: int = 3) -> List[List[Tuple[float, ...]]]:
"""
Main NAG search using Dijkstra's algorithm
Algorithm 1 from paper
"""
# Initialize start vertex
v_start = NAGVertex(start, {0})
v_start.g_score = 0
self.vertices_nag.append(v_start)
self.vertex_map[0] = v_start
heapq.heappush(self.open_list, (0, v_start.vertex_id, v_start))
goal_vertices = []
while self.open_list:
_, v_id, v = heapq.heappop(self.open_list)
if v_id in self.closed_list:
continue
self.closed_list.add(v_id)
# Check stopping criteria
if v.config == goal:
goal_vertices.append(v)
if len(goal_vertices) >= k:
break
# Compute PNS for current vertex
pns_new = self.compute_pns(v, self.vertex_map)
v.pns = pns_new
# Expand neighbors
for neighbor_config in self.cs_graph.get_adjacency(v.config):
cost = self.cs_graph.get_cost(v.config, neighbor_config)
g_new = v.g_score + cost
# Create potential new vertex
v_new = NAGVertex(neighbor_config, pns_new)
v_new.g_score = g_new
v_new.came_from = v
# Check if equivalent to existing vertex
found_equivalent = False
for other in self.vertices_nag:
if other.config == neighbor_config:
if v_new.is_equivalent(other):
found_equivalent = True
if g_new < other.g_score:
other.g_score = g_new
other.came_from = v
other.pns = pns_new
heapq.heappush(self.open_list, (g_new, other.vertex_id, other))
break
if not found_equivalent:
self.vertices_nag.append(v_new)
self.vertex_map[v_new.vertex_id] = v_new
heapq.heappush(self.open_list, (g_new, v_new.vertex_id, v_new))
# Reconstruct k paths
self.paths_found = []
for goal_v in goal_vertices[:k]:
path = self._reconstruct_path(goal_v)
if path:
self.paths_found.append(path)
return self.paths_found
def _reconstruct_path(self, vertex: NAGVertex) -> List[Tuple[float, ...]]:
"""Reconstruct path from start to vertex following came_from pointers"""
path = []
current = vertex
while current:
path.append(current.config)
current = current.came_from
path.reverse()
return path
# ============================================================================
# TETHERED ROBOT PATH PLANNING
# ============================================================================
class TetheredRobotPlanner:
"""Two-stage tethered robot path planning using NAG"""
def __init__(self, nag: NeighborhoodAugmentedGraph, tether_length: float):
self.nag = nag
self.tether_length = tether_length
self.length_constrained_nag = None
def construct_tethered_config_space(self, base: Tuple[float, ...]):
"""
Stage 1: Construct length-constrained NAG
Build NAG with all vertices reachable within tether_length
"""
nag = self.nag
# Start from base
v_base = NAGVertex(base, {0})
v_base.g_score = 0
nag.vertices_nag = [v_base]
nag.vertex_map[0] = v_base
open_list = [(0, v_base.vertex_id, v_base)]
closed = set()
while open_list:
_, v_id, v = heapq.heappop(open_list)
if v_id in closed:
continue
closed.add(v_id)
# Check tether length constraint
if v.g_score > self.tether_length:
continue
# Compute PNS
pns_new = nag.compute_pns(v, nag.vertex_map)
v.pns = pns_new
# Expand neighbors
for neighbor_config in nag.cs_graph.get_adjacency(v.config):
cost = nag.cs_graph.get_cost(v.config, neighbor_config)
g_new = v.g_score + cost
if g_new > self.tether_length:
continue
v_new = NAGVertex(neighbor_config, pns_new)
v_new.g_score = g_new
v_new.came_from = v
# Check equivalence
found = False
for other in nag.vertices_nag:
if other.config == neighbor_config and v_new.is_equivalent(other):
found = True
if g_new < other.g_score:
other.g_score = g_new
other.came_from = v
heapq.heappush(open_list, (g_new, other.vertex_id, other))
break
if not found:
nag.vertices_nag.append(v_new)
nag.vertex_map[v_new.vertex_id] = v_new
heapq.heappush(open_list, (g_new, v_new.vertex_id, v_new))
self.length_constrained_nag = nag
def length_constrained_search(self, start: Tuple[float, ...],
goal: Tuple[float, ...]) -> Optional[List[Tuple[float, ...]]]:
"""
Stage 2: Search on length-constrained NAG
Find shortest path from start to goal respecting tether constraint
"""
nag = self.length_constrained_nag
# Find start vertex with initial configuration
start_vertex = None
for v in nag.vertices_nag:
if v.config == start:
start_vertex = v
break
if not start_vertex:
return None
# Dijkstra search from start
open_list = [(start_vertex.g_score, start_vertex.vertex_id, start_vertex)]
visited = set()
while open_list:
_, v_id, v = heapq.heappop(open_list)
if v_id in visited:
continue
visited.add(v_id)
# Check if reached goal
if v.config == goal:
return nag._reconstruct_path(v)
# Expand neighbors in length-constrained NAG
for neighbor_config in nag.cs_graph.get_adjacency(v.config):
for other in nag.vertices_nag:
if other.config == neighbor_config and other.vertex_id not in visited:
heapq.heappush(open_list, (other.g_score, other.vertex_id, other))
return None
# ============================================================================
# VISUALIZATION
# ============================================================================
def plot_2d_planning_result(cs_graph: ConfigurationSpaceGraph,
paths: List[List[Tuple[float, ...]]],
start: Tuple[float, ...],
goal: Tuple[float, ...]):
"""Visualize 2D path planning results"""
fig, ax = plt.subplots(figsize=(12, 10))
# Draw obstacles
for center, radius in cs_graph.obstacles:
circle = Circle(center, radius, color='red', alpha=0.5, label='Obstacle')
ax.add_patch(circle)
# Draw high-cost regions
for config, multiplier in cs_graph.cost_map.items():
if multiplier > 1.0:
circle = Circle(config, cs_graph.resolution/2,
color='orange', alpha=0.3)
ax.add_patch(circle)
# Draw paths with different colors
colors = ['blue', 'green', 'purple', 'brown', 'pink']
for i, path in enumerate(paths):
path_array = np.array(path)
ax.plot(path_array[:, 0], path_array[:, 1],
color=colors[i % len(colors)], linewidth=2,
label=f'Path {i+1} (length: {sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path)-1)):.2f})',
marker='o', markersize=3)
# Draw start and goal
ax.plot(start[0], start[1], 'g*', markersize=20, label='Start')
ax.plot(goal[0], goal[1], 'r*', markersize=20, label='Goal')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('Neighborhood-Augmented Graph Path Planning Results')
ax.legend(loc='best')
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
plt.tight_layout()
return fig
# ============================================================================
# EXAMPLE USAGE
# ============================================================================
if __name__ == "__main__":
print("=" * 70)
print("Neighborhood-Augmented Graph Path Planning Implementation")
print("=" * 70)
# Create 2D configuration space
print("\n[1] Creating 2D Configuration Space...")
bounds_2d = [(0, 100), (0, 100)]
cs_graph = ConfigurationSpaceGraph(bounds_2d, resolution=1.0)
# Add obstacles
print("[2] Adding obstacles...")
cs_graph.add_obstacle((50, 30), 15) # Left obstacle
cs_graph.add_obstacle((50, 70), 15) # Right obstacle
# Add high-cost region
print("[3] Adding high-cost region...")
cs_graph.add_high_cost_region((50, 50), 10, multiplier=3.0)
# Create NAG
print("[4] Initializing Neighborhood-Augmented Graph...")
nag = NeighborhoodAugmentedGraph(
cs_graph,
r_n=8.0,
omega=0.6,
r_b=0
)
# Plan k geodesic paths
start = (10, 50)
goal = (90, 50)
k_paths = 3
print(f"\n[5] Planning {k_paths} geodesic paths...")
print(f" Start: {start}")
print(f" Goal: {goal}")
paths = nag.search(start, goal, k=k_paths)
print(f"\n[6] Results:")
for i, path in enumerate(paths):
path_length = sum(
np.linalg.norm(np.array(path[j+1]) - np.array(path[j]))
for j in range(len(path)-1)
)
print(f" Path {i+1}: {len(path)} waypoints, length = {path_length:.2f}")
# Visualize
print("\n[7] Generating visualization...")
fig = plot_2d_planning_result(cs_graph, paths, start, goal)
plt.savefig('nag_path_planning_2d.png', dpi=150, bbox_inches='tight')
print(" Saved as 'nag_path_planning_2d.png'")
# Tethered robot example
print("\n" + "=" * 70)
print("TETHERED ROBOT PATH PLANNING EXAMPLE")
print("=" * 70)
print("\n[8] Setting up tethered robot planning...")
tether_length = 60.0
base = (10, 50)
planner = TetheredRobotPlanner(nag, tether_length)
print("[9] Constructing length-constrained configuration space...")
planner.construct_tethered_config_space(base)
print(f" Reachable vertices: {len(nag.vertices_nag)}")
print("[10] Searching for valid tethered path...")
tethered_path = planner.length_constrained_search(start, goal)
if tethered_path:
tether_cost = sum(
np.linalg.norm(np.array(tethered_path[j+1]) - np.array(tethered_path[j]))
for j in range(len(tethered_path)-1)
)
print(f" Valid tethered path found!")
print(f" Path length: {tether_cost:.2f}")
print(f" Tether constraint: {tether_length}")
print(f" Constraint satisfied: {tether_cost <= tether_length}")
else:
print(" No valid tethered path found within tether length")
print("\n" + "=" * 70)
print("Execution completed successfully!")
print("=" * 70)Related posts, You May like to read
- 7 Shocking Truths About Knowledge Distillation: The Good, The Bad, and The Breakthrough (SAKD)
- MOSEv2: The Game-Changing Video Object Segmentation Dataset for Real-World AI Applications
- MedDINOv3: Revolutionizing Medical Image Segmentation with Adaptable Vision Foundation Models
- SurgeNetXL: Revolutionizing Surgical Computer Vision with Self-Supervised Learning
- How AI is Learning to Think Before it Segments: Understanding Seg-Zero’s Reasoning-Driven Image Analysis
- SegTrans: The Breakthrough Framework That Makes AI Segmentation Models Vulnerable to Transfer Attacks
- Universal Text-Driven Medical Image Segmentation: How MedCLIP-SAMv2 Revolutionizes Diagnostic AI
- Towards Trustworthy Breast Tumor Segmentation in Ultrasound Using AI Uncertainty
- DVIS++: The Game-Changing Decoupled Framework Revolutionizing Universal Video Segmentation
- Radar Gait Recognition Using Swin Transformers: Beyond Video Surveillance
