Motion Planning Algorithms Implementation

Breadth First Search

BFS takes a grid, the start node, and the goal node as input. It returns a list of two values [path, steps], where path is a list of coordinates from start to goal, and steps is the number of nodes visited before finding the path.

A*

A* algorithm is a shortest path algorithm that uses both the cost from the source node and an estimation of the remaining cost to the destination node, called the heuristic, to find the shortest path. It works by maintaining a priority queue of nodes, starting with the source node, and repeatedly visiting the node with the lowest cost, which is calculated as the sum of the cost from the source node and the heuristic.

The algorithm updates the cost and the heuristic of the neighbors of the current node and adds them to the priority queue, repeating this process until the destination node is reached or no more nodes are left in the priority queue.

Documentation

GitHub

Probabilistic Roadmap

The PRM algorithm is a sampling-based approach that constructs a roadmap of the configuration space, which is the space of all possible configurations of the robot or object. The roadmap consists of a set of nodes connected by edges, where each node represents a configuration and each edge represents a feasible path between two configurations.

The choice of sampling method depends on the problem at hand, and each method has its own advantages and disadvantages. Uniform sampling is easy to implement but may lead to poor coverage of the space. Random sampling is suitable for problems that require a specific distribution and can generate points in areas of high density. Gaussian sampling can generate points with a natural distribution but can be computationally expensive. Bridge sampling can be used to connect subspaces but can be difficult to implement, particularly in high-dimensional spaces.

Documentation

GitHub

Rapidly Exploring Random Tree

In RRT, the search space is explored by adding random nodes to the tree and connecting them to the nearest existing node. The algorithm continues to explore the search space until it reaches the goal region. However, the resulting path may not be optimal as it only guarantees to find a feasible solution, but not necessarily the shortest path.

Documentation

GitHub