# Sampling-Based Path Planning Algorithm for a Plug & Produce Environment
## 2 Implementation of Path Planning Algorithms
### Adaptive RRT
One pair of neighboring configurations is identified, and using a simple local planner, the connections are established between these two configurations. To establish the connection, an incremental collision detection function is enabled and it checks for the collision between the configurations. If there is no collision found, the edge will be created between the configurations. Each configuration can have a maximum number of possible connections, and it can be fixed with the value \(k\), which is called \(k\)-nearest neighbor value. During the construction phase, the user can fix the number of samples \(\left(n_{samples}\right)\) to be created, which can increase or decrease the complexity of the created map. Algorithm 3 shows captures of the PRM algorithm. When the connections are established between all the random configurations, the formation of the road map in the entire \(C-space\) is finished, i.e., the learning phase of PRM is finished. The purpose of the learning phase is to construct a roadmap in the form of an undirected graph. The time it takes to create the graph mainly depends on two values: \(k\) and \(n_{samples}\). If these two values are increased, the density of the graph increases. Thus, the learning phase is always slow. In the query phase, the start \((q_{start})\) and goal \((q_{goal})\) points are assigned within the \(C_{free}\)_space_ in the created dense graph. ``` Algorithm to build roadmap with PRM: function \(PRM\) \(\left(n_{samples},\ k\ \right):\ G\) \(V\leftarrow\varnothing\) and \(\dot{E}\leftarrow\varnothing\) while\(|V|<n\)do  
The purpose of this phase is to plan the path from the initial configuration (\(q_{start}\)) to the goal configuration (\(q_{goal}\)) in the roadmap. Initially, the graph \(G=(V,E)\) is empty. The local planner (\(\Delta\)) connects from one random configuration (\(q_{rand}\)) to the next nearest random configuration (\(q_{near}\)) via a straight line in \(C_{free}\). If there is no collision, between \(q_{rand}\) and \(q_{near}\) else \(NIL\). \(\Delta\) is assumed as symmetric and deterministic. This \(q_{near}\) is chosen based on the Euclidean distance from \(q_{rand}\). This iteration continues until it connects all the randomly generated \(n_{samples}\) configurations with a minimum \(k\)-number of connections for all the configurations. This is the end of generating the _road map_\((V,E)\) as a dense graph. [PERSON] finds the optimal path in terms of distance, meaning the cost function is chosen as the distance. It marks the distance as zero from the current vertex to the next vertex, then finds all vertices leading to the current vertex and calculates the distance for all the vertices. It finds the smallest value vertex, saves it and marks that vertex as current. It then finds all the vertices which lead to the current vertex and calculates the distance value. This process repeats until marking all the vertices as visited, meaning that the algorithm iterates the entire _roadmap_\((V,E)\). Algorithm 4 shows the captures of [PERSON] optimization algorithm. Algorithm of Dijkstra Optimization algorithm:  
1. Initialize the starting node as the source. 2. set source_node. distance = 0
3. set source_node. parent = None
4. Initialize a priority queue \(Q\) with all nodes in \(G\)
5. set \(Q=\{v:\ v\ in\ V\}\)
6. While \(Q\) is not empty:
7. Extract the node u with the smallest distance from \(Q\)
8. For each neighbor v of u :
9. If \(v\) is in \(Q\) : # Compute the Euclidean distance from \(u\) to \(v\), \(d\)(u,\(v\)), via the roadmap G. \(i.\ d(u,\ v)=sqrt((u.x-v.x)^{2}+(u.y-v.y)^{2})\) \(ii.\ f\ d(u,\ v)+u.distance<v.distance:\) 1. Update \(v.distance=d(u,v)+u.distance\)
10. Update \(v.parent=u\)
11. Remove u from \(Q\)
12. If the target node is reachable from the source node:
13. Construct the shortest path from the target node to the source node with the best parent search based on distance. 14. Return to the shortest path. 15. Otherwise, return to the target node that is unreachable from the source node.