Algorithm 3 Combined RRT and elliptical path

Input: Initial and desired configuration of the robot, the maximum number of samples, Nmax

Output: Tree, T

Add initial configuration to the tree, T

1.For i=1 to Nmax:

a.Randomly choose a,b, direction of ellipse (clockwise/anti-clockwise), direction of ellipse     b.Sample a configuration, Cs from the tree, T     c.Apply Equation (2) to determine center coordinate of the ellipse (h,k) based on:

$\{\begin{array}{l}\tan \left(\theta \right)=\frac{-b}{{a\times \theta }_s}\\ x_n=h+a·cos\left(\theta \right)\\ y_n=k+b·sin\left(\theta \right)\end{array}$

(xn,yn) is the coordinate of the head of Cs and θs is the tangent of the orientation of Cs

d.Execute a set of points from Cs which the robot can follow and closely satisfy the ellipse to generate a new set of configurations {C}     e.Find best of {C} based on Equations (4) or (5) and add it to T

2.Return T