# Sampling-Based Path Planning Algorithm for a Plug & Produce Environment
## 2 Implementation of Path Planning Algorithms
### Adaptive RRT  
An adaptive RRT algorithm is a modified version of the RRT algorithm and its sampling value, which is adapted based on the constraints present in the environment. An adaptive RRT works exactly like RRT, up to the collision check module. During the collision check, if there is  
Figure 2: Plug & Produce concept-based industrial testbed created using the Robotstudio simulation. (A) Process modules; (B) docking station modules; and (C) tool holders. no collision between \(q_{near}\) and \(q_{new}\), it connects the edge like in RRT. If there is a collision, the RRT discards the \(q_{new}\) point and samples another \(q_{new}\), but in adaptive RRT, it divides the \(\delta\) value by 2 (that point is marked as \(q_{mid}\)) and checks for the collision. If there is no collision between the \(q_{near}\) and \(q_{mid}\), it discards the \(q_{new}\) point and creates an edge between \(q_{near}\) and \(q_{mid}\), then adds it to the tree, \(T\). By dividing the \(\delta\) value by 2, it discards the obstacle between \(q_{mid}\) and \(q_{new}\). Here, each time the obstacle is between \(q_{near}\) and \(q_{new}\), it is creating an extra node called \(q_{nearest}\). Due to this, the number of nodes created by adaptive RRT will be higher than RRT. Because of this, the energy consumed by the path generated by adaptive RRT and the computational time of adaptive RRT will be higher compared to RRT. But this adaptive RRT algorithm is useful when we have more obstacles/constraints in the environment. Algorithm 2 shows captures of the adaptive RRT algorithm. ```
Algorithm to Build RRT tree \(T\): functionRRT\(\left(q_{start},q_{goal},\,K,\,\delta,\,C\right)\,:\,T\) \(\_init\,(q_{start})\) for\(k\)do \(q_{rand}\ \leftarrow\)\(RAND\_CONF\,(\,)\) in\(C_{free}\) \(q_{near}\ \leftarrow\)\(NEAREST\_CONFIG\,(q_{rand},\,T)\) \(q_{new}\ \leftarrow\)\(EXTEND\,(q_{near},\,q_{rand},\,\delta)\) if obstacleFree\((q_{near},\,q_{new})\)then \(T\ \leftarrow\ T\ \cup\ \{q_{new}\}\) \(E\ \leftarrow\ \cup\ \{(q_{near},q_{new})\ \}\) endif endfor return\(T=(V,E)\) endfunction
```  
**Algorithm 1** Captures of the RRT algorithm  
```
Algorithm to build Adaptive RRT tree (\(T\)): functionRRT\(\left(\,q_{start},q_{goal},\,K,\,\delta,\,C\right)\,:\,T\) \(\_init\,(q_{start})\) for\(k=1\) to \(K\)do \(q_{rand}\ \leftarrow\)\(RAND_{CONF}\,(\,)\) in \(C_{free}\); \(q_{near}\ \leftarrow\)\(NEAREST\_CONFIG\,(q_{rand},\,\delta)\); ifCollisionfree\((q_{near},\,q_{rand})\)then \(T\ \leftarrow\ T\ \cup\ \{q_{new}\}\); \(E\ \leftarrow\ E\ \cup\ \{(q_{near},q_{new})\ \}\);  endif else  Find \(q_{mid}\) point between \(q_{near}\) and \(q_{rand}\) ifCollisionfree\((q_{near},\,q_{mid})\)then  generate a \(q_{new}\)\(between\)\(q_{near}\) and \(q_{mid}\)  find \(q_{nearest}\) the nearest point to \(q_{new}\) in the tree! choose \(q_{min}\) the best parent out of \(q_{nearest}\) and \(q_{near}\) for \(q_{new}\)  insert \(dist_{min}\) and \(q_{new}\) in the \(T\). rewire \(T\) for \(q_{new}\) endif end else  endfor return\(T\) endfunction
```  
**Algorithm 2** Captures of the adaptive RRT algorithm  
```
Algorithm to build Adaptive RRT tree (\(T\)): functionRRT\(\left(\,q_{start},q_{goal},\,K,\,\delta,\,C\right)\,:\,T\) \(\_init\,(q_{start})\) for\(k=1\) to \(K\)do \(q_{rand}\ \leftarrow\)\(RAND_{CONF}\,(\,)\) in \(C_{free}\); \(q_{rand}\ \leftarrow\)\(NEAREST\_CONFIG\,(q_{rand},\,\delta)\); ifCollisionfree\((q_{near},\,q_{rand})\)then \(T\ \leftarrow\



### PRM with the Dijkstra Algorithm

PRM works in two phases, i.e., the construction phase and a query phase. During the construction phase, it constructs the roadmap in the form of an undirected graph \(G=(V,E)\) in the configuration-free space \((C_{free})\). It randomly generates uniform sample configurations \((q_{start},\ q_{rand},\ q_{near}\ and\ q_{goal})\) in \(C_{free}\). The _edge in \(E\)_ corresponds to the collision-free, connecting the random samples \(q_{start}\) and \(q_{goal}\). \(E\) is a straight line which is referred to as a local path, computed by a local planner.