# Sampling-Based Path Planning Algorithm for a Plug & Produce Environment
## 2 Implementation of Path Planning Algorithms
### Rrt*  
RRT* is an incremental sampling-based planning algorithm which brings an initial solution based on the principle of RRT and optimizes the path using two planning stages, i.e., the best parent search and rewiring. RRT* also considers both the incoming and outgoing edges of each new vertex, which helps improve the cost function in terms of the distance of existing vertices. Thereby, it locally minimizes the cost function and creates a complete tree (\(T\)).  
RRT* constructs the tree (\(T\)) initially by adding \(q_{start}\) as the starting point. It randomly generates the \(q_{rand}\) point in the configuration-free (\(C_{free}\)) space. \(C_{free}\) is derived from configuration spaces \((C)\) without obstacles (\(C_{obs}\)), so \(C_{free}\) is created using \(C\cap C_{obs}\). From \(q_{rand}\), it finds the nearest node \(q_{nearest}\). Then, it interpolates between \(q_{rand}\) and \(q_{nearest}\). The new interpolated node is called \(q_{new}\). It checks for the obstacles between \(q_{new}\) and \(q_{nearest}\) by enabling the collision check function. Collision check is performed in three stages. The first one is checking the collision between \(q_{new}\) and \(q_{nearest}\), then between the robot tool and the environment, and finally it checks the collision between the robot and the environment. If there is no collision, \(q_{new}\) will be added to the trajectory and update the cost of reaching \(q_{new}\) from the \(q_{nearest}\). Then, \(q_{nearest}\) will act as a parent node of \(q_{new}\), find the next nearest neighbor from \(q_{new}\), and mark it as \(q_{near}\). From all the neighbors of \(q_{near}\), it checks if \(q_{new}\) can be reached shortly with the distance value (\(dist_{min}\)) for the node \(q_{min}\) from any other parent node, without any collision with the obstacles. The node which gives the least \(dist_{min}\) value is selected and updated as \(q_{new}\). \(q_{new}\) will then be added to the tree as one of the nodes. This is the first optimal step, and it is called the best parent search of RRT* algorithms. The second optimal step is rewiring; it finds the new lower-cost path to reach among the nodes that are already existing in the tree (\(T\)). Rewiring is carried out by searching the node \(q_{near}\) and checking it by changing its parent to \(q_{new}\) to see will there be a chance to lower the cost of the path. If so, the tree is rewired, added as the child of \(q_{new}\), and the shortest nodes of the path updated.  
Iteration continues until \(q_{goal}\) or the maximum number of nodes is reached. Until the maximum number of nodes is reached, it is not finding the \(q_{goal}\); it terminates the loop and comes out as no available collision-free path found. Algorithm 5 shows captures of the RRT* algorithm.  
```
Algorithm to build RRT* tree: functionRRT* \(\left(q_{start},\,q_{goal},\,n(samples),\,C\right):T\) create empty \(\Upsilon\) with \(q_{start}\) for n do  generate a random node \(q_{rand}\) in \(C_{free}\)  find the nearest node in the tree \(q_{nearest}\) from \(q_{rand}\) if collision free (\(q_{nearest}\), \(q_{rand}\)) then  generate \(q_{new}\) between \(q_{nearest}\) and \(q_{rand}\)  add the \(q_{new}\) to \(T\)  find \(q_{near}\) in the vicinity of the \(q_{new}\)  calculate the cost in terms of distance. \(q_{min}\gets dist_{min}=d(nearest,\,new)+d(new,\,near)\) if\(d_{new}<d(nearest,\,near)\)then update the parent of \(q_{near}\) to \(q_{new}\)  rewire \(T\) for \(q_{new}\) end if end for return T endfunction
```  
**Algorithm 5** Captures of the RRT* algorithm