01.

The Concept

This project was developed for the Planning and Decision Making course (RO47005) in the Robotics MSc at TU Delft. The objective was to engineer a robust global planner capable of guiding a quadrotor safely through diverse, obstacle-ridden environments inside a physics-based simulation.

The core challenge was implementing the Dijkstra algorithm from scratch and integrating it cleanly into the PyBullet simulation environment to enable autonomous navigation in 3D space.

02.

Technical Deep Dive

Smart Grid Generation

The foundation of the planner is a voxel-based grid. The simulation space is discretized into nodes, and each node is validated against the environment boundaries and obstacle positions. What remains is a navigable graph the pathfinding algorithm can search over.

def filter_grid(XYZ):
    indices_to_remove = []
    for i, point in enumerate(XYZ):
        # Check bounds
        if (X_BOUNDS[0] <= point[0] <= X_BOUNDS[1]) and \
           (Y_BOUNDS[0] <= point[1] <= Y_BOUNDS[1]) and \
           (Z_BOUNDS[0] <= point[2] <= Z_BOUNDS[1]):
            # Check collision
            if not euclideanDistance(point):
                indices_to_remove.append(i)
        else:
            indices_to_remove.append(i)

    return np.delete(XYZ, indices_to_remove, axis=0)

The grid resolution is a direct trade-off: a fine grid produces smoother, safer paths at a higher search cost, while a coarse grid is fast but blunt around obstacles.

Once the grid is built, Dijkstra expands outward from the start node, always visiting the lowest-cost frontier node next, until it reaches the goal — guaranteeing the shortest collision-free path through the discretized space.