# Basic Pure Pursuit

Originally written by Sarah Xiang from VRC team 97963A and VEXU team ILLINI

- Basic programming knowledge (arrays and loops)
- High school level math such as trigonometry

If you have any comments/suggestions, encountered issues with the code, or have questions about the document, please contact Sarah (vexforum

`@sarah_97963a`

, discord `@Sarah | iCRY | 97963A#2509`

)*Wiki dev's note: GitBook doesn't have a way to run code natively (at least that I know of), but there are some references to running/writing the code throughout this article. If you'd like to try pure pursuit yourself, or just see what the output of the code should look like,*

*here's a link*

*to Sarah's original Google Colab.*

import numpy as np

import matplotlib.pyplot as plt

import math

import matplotlib.animation as animation

from IPython import display

- Below are some helper functions to help visualize the output of pure pursuit and path generator
- Feel free to modify these functions to graph lines in the style you like

def add_line (path) :

for i in range (0,len(path)):

plt.plot(path[i][0],path[i][1],'.',color='red',markersize=10)

for i in range(0,len(path)-1):

plt.plot([path[i][0],path[i+1][0]],[path[i][1],path[i+1][1]],color='b')

plt.axis('scaled')

# plt.show()

def add_complicated_line (path,lineStyle,lineColor,lineLabel) :

for i in range (0,len(path)):

plt.plot(path[i][0],path[i][1],'.',color='red',markersize=10)

for i in range(0,len(path)-1):

if(i == 0):

# plt.plot([path[i][0],path[i+1][0]],[path[i][1],path[i+1][1]],color='b')

plt.plot([path[i][0],path[i+1][0]],[path[i][1],path[i+1][1]],lineStyle,color=lineColor,label=lineLabel)

else:

plt.plot([path[i][0],path[i+1][0]],[path[i][1],path[i+1][1]],lineStyle,color=lineColor)

plt.axis('scaled')

def highlight_points (points, pointColor):

for point in points :

plt.plot(point[0], point[1], '.', color = pointColor, markersize = 10)

def draw_circle (x, y, r, circleColor):

xs = []

ys = []

angles = np.arange(0, 2.2*np.pi, 0.2)

for angle in angles :

xs.append(r*np.cos(angle) + x)

ys.append(r*np.sin(angle) + y)

plt.plot(xs, ys, '-', color = circleColor)

The pure pursuit controller is an automatic steering method for wheeled mobile robots. It is a steering method, which means it computes the necessary angular velocity for a wheeled robot to stay on pre-computed paths. Linear velocity is assumed to be constant. Therefore, an additional velocity controller of your choice is needed if you wish to slow down the robot as it approaches the target (something as simple as a proportional controller could work). During the 2020-2021 VRC season, our team used the pure pursuit controller for the programming skills challenge and achieved great results. Videos of our robot in action can be found here.

A quick demonstration of path tracking using the pure pursuit controller:

In the animation shown above, the dotted gray line is the pre-computed path that the robot needs to follow and the solid black line is the robot's trajectory. The big yellow dot and the short red line represent the robot's current position and heading. There is another solid line connecting the point the robot is "chasing after" and the robot's current position, but it's very hard to see in this demo. Although the robot's trajectory does not match with the path perfectly, the pure pursuit controller still performed decently well.

In the sections below, we will implement a basic pure pursuit controller for a differential drive (NOT the type of differential commonly used for power sharing in VRC), a robot model fairly similar to the 4 wheel tank drives in VRC.

As shown above, the pure pursuit controller cannot trace a path perfectly due to the non-zero look ahead distance. The more sharp corners the path contains, the worse the performance. There are two ways to achieve better performance under the presence of sharp corners:

- Optimize the path during the path generation process
- Make improvements to the pure pursuit controller itself

For most circumstances in the VRC competition, the basic pure pursuit controller will suffice even without the above two improvements.

The key of the pure pursuit controller involves calculating a goal point on the path that is a certain distance

$l_d$

, which is called the look ahead distance, from the robot's current position. Then, the goal point is used to calculate the appropriate angular velocity needed for the robot to move toward that point. As the robot moves forward, a different goal point on the path is chosen and the robot's angular velocity gets updated. Since the robot can never reach this goal point and the goal point stays on the path, the end result would be the robot tracing the path.To find the appropriate goal point for the robot to follow mathematically, a circle centered at the robot's current position with radius

$l_d$

is made. The circle's intersections with the path are the potential goal points. We will return to this part of the algorithm later in the document.A more intuitive way of understanding and visualizing the pure pursuit controller would be the GIF below:

The carrot is always at a constant distance away from the donkey so the donkey moves in the direction the rider wants by chasing after the carrot.

In our implementation of the pure pursuit controller, the path is a 2D array represented by equally spaced points (1D array). The basic algorithm is to use a for loop to go through each pair of points to determine if there are any intersections within each line segments. If goal points are found, follow the most appropriate one. These steps will be repeated until the end of the path is reached.

Let's start by looking at the basic math behind choosing goal points -- the line-circle intersection. There are multiple ways to find the intersection, we have found that this method (the screenshot below) was the easiest to implement and debug. (Side note: the math here might look a bit terrifying, but it's okay if you don't fully understand how it works. This statement only applies to this section.)

In the method presented above, the circle is assumed to be centered at the the origin. In our application, the circle would be centered at the robot's current position

`[currentX, currentY]`

and the two points that define the line would be arbitrary points `pt1 = [x1, y1]`

and `pt2 = [x2, y2]`

. Therefore, to apply this method, we first need to subtract `currentX`

and `currentY`

from `[x1, y1]`

and `[x2, y2]`

to center the system at origin. Again, we perform such offsets so that we can use the method from the screenshot above, which requires the circle to be centered at the origin.In the code cell below, you are encouraged to code such line-circle intersection algorithm yourself. If you do not know python at all, complete code with comments is provided.

# helper function: sgn(num)

# returns -1 if num is negative, 1 otherwise

def sgn (num):

if num >= 0:

return 1

else:

return -1

# currentPos: [currentX, currentY]

# pt1: [x1, y1]

# pt2: [x2, y2]

def line_circle_intersection (currentPos, pt1, pt2, lookAheadDis):

# extract currentX, currentY, x1, x2, y1, and y2 from input arrays

# DO NOT modified these variables!

currentX = currentPos[0]

currentY = currentPos[1]

x1 = pt1[0]

y1 = pt1[1]

x2 = pt2[0]

y2 = pt2[1]

# boolean variable to keep track of if intersections are found

# remember to set this correctly for the graphing functions to work!

intersectFound = False

# your code goes here

# output (intersections found) should be stored in arrays sol1 and sol2 in the form of sol1 = [sol1_x, sol1_y]

# if two solutions are the same, store the same values in both sol1 and sol2

# graphing functions to visualize the outcome

# ---------------------------------------------------------------------------------------------------------------------------------------

plt.plot ([x1, x2], [y1, y2], '--', color='grey')

draw_circle (currentX, currentY, lookAheadDis, 'orange')

if intersectFound == False :

print ('No intersection Found!')

else:

print ('Solution 1 found at [{}, {}]'.format(sol1[0], sol1[1]))

print ('Solution 2 found at [{}, {}]'.format(sol2[0], sol2[1]))

plt.plot ([sol1[0]], [sol1[1]], '.', weight=8, color='red', label='sol1')

plt.plot ([sol2[0]], [sol2[1]], '.', weight=8, color='blue', label='sol2')

plt.legend()

plt.axis('scaled')

plt.show()

# now call this function and see the results!

line_circle_intersection ([0, 1], [2, 3], [-2, -4], 0.9)

# helper function: sgn(num)

# returns -1 if num is negative, 1 otherwise

def sgn (num):

if num >= 0:

return 1

else:

return -1

# currentPos: [currentX, currentY]

# pt1: [x1, y1]

# pt2: [x2, y2]

def line_circle_intersection (currentPos, pt1, pt2, lookAheadDis):

# extract currentX, currentY, x1, x2, y1, and y2 from input arrays

currentX = currentPos[0]

currentY = currentPos[1]

x1 = pt1[0]

y1 = pt1[1]

x2 = pt2[0]

y2 = pt2[1]

# boolean variable to keep track of if intersections are found

intersectFound = False

# output (intersections found) should be stored in arrays sol1 and sol2

# if two solutions are the same, store the same values in both sol1 and sol2

# subtract currentX and currentY from [x1, y1] and [x2, y2] to offset the system to origin

x1_offset = x1 - currentX

y1_offset = y1 - currentY

x2_offset = x2 - currentX

y2_offset = y2 - currentY

# calculate the discriminant using equations from the wolframalpha article

dx = x2_offset - x1_offset

dy = y2_offset - y1_offset

dr = math.sqrt (dx**2 + dy**2)

D = x1_offset*y2_offset - x2_offset*y1_offset

discriminant = (lookAheadDis**2) * (dr**2) - D**2

# if discriminant is >= 0, there exist solutions

if discriminant >= 0:

intersectFound = True

# calculate the solutions

sol_x1 = (D * dy + sgn(dy) * dx * np.sqrt(discriminant)) / dr**2

sol_x2 = (D * dy - sgn(dy) * dx * np.sqrt(discriminant)) / dr**2

sol_y1 = (- D * dx + abs(dy) * np.sqrt(discriminant)) / dr**2

sol_y2 = (- D * dx - abs(dy) * np.sqrt(discriminant)) / dr**2

# add currentX and currentY back to the solutions, offset the system back to its original position

sol1 = [sol_x1 + currentX, sol_y1 + currentY]

sol2 = [sol_x2 + currentX, sol_y2 + currentY]

# graphing functions to visualize the outcome

# ---------------------------------------------------------------------------------------------------------------------------------------

plt.plot ([x1, x2], [y1, y2], '--', color='grey')

draw_circle (currentX, currentY, lookAheadDis, 'orange')

if intersectFound == False :

print ('No intersection Found!')

else:

print ('Solution 1 found at [{}, {}]'.format(sol1[0], sol1[1]))

print ('Solution 2 found at [{}, {}]'.format(sol2[0], sol2[1]))

plt.plot (sol1[0], sol1[1], '.', markersize=10, color='red', label='sol1')

plt.plot (sol2[0], sol2[1], '.', markersize=10, color='blue', label='sol2')

plt.legend()

plt.axis('scaled')

plt.show()

# now call this function and see the results!

line_circle_intersection ([0, 1], [2, 3], [-2, -4], 1)

You might have already noticed something not quite right when messing with the algorithm above. Under certain condition, although the intersections found are on the infinite line defined by

`pt1`

and `pt2`

, they are not exactly within the range of `[x1, y1]`

and `[x2, y2]`

. Consider the situation illustrated below:

Although intersections are found, they are not within any of the line segments in the path. Those are not the points we want the robot to follow! So how do we prevent such situation from happening?

The solution is simple. After intersections are found, we check to see if their x values are within the range of

`[min(x1, x2), max(x1, x2)]`

and their y values are within the range of `[min(y1, y2), max(y1, y2)]`

. That is, for a solution to be valid and `intersectFound`

to be `True`

, the following conditions need to be satisfied:discriminant >= 0

min(x1, x2)<= sol_x <= max(x1, x2)

min(y1, y2) <= sol_y <= max(y1, y2)

You can modify your

`line_circle_intersection`

function in the code cell below. Completed and commented code is also provided.(Side note: use

`(a >= x) && (x >= b)`

in VEXCode.)# helper function: sgn(num)

# returns -1 if num is negative, 1 otherwise

def sgn (num):

if num >= 0:

return 1

else:

return -1

# currentPos: [currentX, currentY]

# pt1: [x1, y1]

# pt2: [x2, y2]

def line_circle_intersection (currentPos, pt1, pt2, lookAheadDis):

# extract currentX, currentY, x1, x2, y1, and y2 from input arrays

# DO NOT modify these variables!

currentX = currentPos[0]

currentY = currentPos[1]

x1 = pt1[0]

y1 = pt1[1]

x2 = pt2[0]

y2 = pt2[1]

# boolean variable to keep track of if intersections are found

# remember to set this correctly for the graphing functions to work!

intersectFound = False

# your code goes here

# output (intersections found) should be stored in arrays sol1 and sol2 in the form of sol1 = [sol1_x, sol1_y]

# hint: if two solutions are the same, store the same values in both sol1 and sol2

# add boundary checking code here

# for now, the function should print out 'solution _ is valid!' ( _ can be 1 or 2) if a solution point is within the range

# we will use this outcome more in the next section

# graphing functions to visualize the outcome

# ---------------------------------------------------------------------------------------------------------------------------------------

plt.plot ([x1, x2], [y1, y2], '--', color='grey')

draw_circle (currentX, currentY, lookAheadDis, 'orange')

if intersectFound == False :

print ('No intersection Found!')

else:

print ('Solution 1 found at [{}, {}]'.format(sol1[0], sol1[1]))

print ('Solution 2 found at [{}, {}]'.format(sol2[0], sol2[1]))

plt.plot (sol1[0], sol1[1], '.', markersize=10, color='red', label='sol1')

plt.plot (sol2[0], sol2[1], '.', markersize=10, color='blue', label='sol2')

plt.legend()

plt.axis('scaled')

plt.show()

# now call this function and see the results!

line_circle_intersection ([0, 1], [2, 3], [-2, -4], 0.9)

# helper function: sgn(num)

# returns -1 if num is negative, 1 otherwise

def sgn (num):

if num >= 0:

return 1

else:

return -1

# currentPos: [currentX, currentY]

# pt1: [x1, y1]

# pt2: [x2, y2]

def line_circle_intersection (currentPos, pt1, pt2, lookAheadDis):

# extract currentX, currentY, x1, x2, y1, and y2 from input arrays

currentX = currentPos[0]

currentY = currentPos[1]

x1 = pt1[0]

y1 = pt1[1]

x2 = pt2[0]

y2 = pt2[1]

# boolean variable to keep track of if intersections are found

intersectFound = False

# output (intersections found) should be stored in arrays sol1 and sol2

# if two solutions are the same, store the same values in both sol1 and sol2

# subtract currentX and currentY from [x1, y1] and [x2, y2] to offset the system to origin

x1_offset = x1 - currentX

y1_offset = y1 - currentY

x2_offset = x2 - currentX

y2_offset = y2 - currentY

# calculate the discriminant using equations from the wolframalpha article

dx = x2_offset - x1_offset

dy = y2_offset - y1_offset

dr = math.sqrt (dx**2 + dy**2)

D = x1_offset*y2_offset - x2_offset*y1_offset

discriminant = (lookAheadDis**2) * (dr**2) - D**2

# if discriminant is >= 0, there exist solutions

if discriminant >= 0:

# calculate the solutions

sol_x1 = (D * dy + sgn(dy) * dx * np.sqrt(discriminant)) / dr**2

sol_x2 = (D * dy - sgn(dy) * dx * np.sqrt(discriminant)) / dr**2

sol_y1 = (- D * dx + abs(dy) * np.sqrt(discriminant)) / dr**2

sol_y2 = (- D * dx - abs(dy) * np.sqrt(discriminant)) / dr**2

# add currentX and currentY back to the solutions, offset the system back to its original position

sol1 = [sol_x1 + currentX, sol_y1 + currentY]

sol2 = [sol_x2 + currentX, sol_y2 + currentY]

# find min and max x y values

minX = min(x1, x2)

maxX = max(x1, x2)

minY = min(y1, y2)

maxY = max(y1, y2)

# check to see if any of the two solution points are within the correct range

# for a solution point to be considered valid, its x value needs to be within minX and maxX AND its y value needs to be between minY and maxY

# if sol1 OR sol2 are within the range, intersection is found

if (minX <= sol1[0] <= maxX and minY <= sol1[1] <= maxY) or (minX <= sol2[0] <= maxX and minY <= sol2[1] <= maxY) :

intersectFound = True

# now do a more detailed check to determine which point is valid, which is not

if (minX <= sol1[0] <= maxX and minY <= sol1[1] <= maxY) :

print ('solution 1 is valid!')

if (minX <= sol2[0] <= maxX and minY <= sol2[1] <= maxY) :

print ('solution 1 is valid!')

# graphing functions to visualize the outcome

# ----------------------------------------------------------------------------------------------------------------------------------------

plt.plot ([x1, x2], [y1, y2], '--', color='grey')

draw_circle (currentX, currentY, lookAheadDis, 'orange')

if intersectFound == False :

print ('No intersection Found!')

else:

print ('Solution 1 found at [{}, {}]'.format(sol1[0], sol1[1]))

print ('Solution 2 found at [{}, {}]'.format(sol2[0], sol2[1]))

plt.plot (sol1[0], sol1[1], '.', markersize=10, color='red', label='sol1')

plt.plot (sol2[0], sol2[1], '.', markersize=10, color='blue', label='sol2')

plt.legend()

plt.axis('scaled')

plt.show()

# now call this function and see the results!

line_circle_intersection ([0, 1], [2, 3], [0, 1], 1)

All possible outcomes of the line-circle intersections (with a single line segment) are the following:

**Situation 1:**No intersection. If this is the case, the discriminant will be negative.**Situation 2:**Intersections are found, but they are not in between`(x1, y1)`

and`(x2, y2)`

. The discriminant is positive, but we should still consider this situation as “no intersection”.**Situation 3:**There is one and only one intersection inside the range of`(x1, y1)`

and`(x2, y2)`

. The discriminant is positive, and the robot should follow the intersection found.**Situation 4:**There are two intersections and both are in between`(x1, y1)`

and`(x2, y2)`

. In this situation, we have to determine which point is better for the robot to follow. One method we can to use is to calculate the distance between the intersections and the second point`(x2, y2)`

(the path goes in the direction of`(x1, y1) -> (x2, y2)`

) and pick the point that is closer to the second point (in other words, the point closer to the end of the path).- In some extreme cases where the robot is traveling through sharp corners, it might create multiple intersections with multiple line segments (as shown in the picture below). We can code our program in a way that the robot would follow the first valid point it found. This way, in extreme cases where the path overlaps itself or there exists multiple sharp corners, the robot would not skip a portion of the path altogether. In order to prevent the robot from going backwards in the path, we can create a variable
`lastFoundIndex`

to store the index of the point it just passed. Every time the loop runs, it will only check the points that are located after`path[lastFoundIndex]`

. This way,**the segments the robot has already traveled through will not be checked again for intersections**. In the cases that no new intersection has been found (robot deviates from the path), the robot will follow the point at`lastFoundIndex`

.

Let's take a closer look at how

`lastFoundIndex`

and the goal point choosing function should work.When the robot first entered the path (the 1st loop iteration),

`lastFoundIndex = 0`

. The line-circle intersection search starts from `path[0]`

, an intersection is found and selected as the goal point for the robot to move toward.

At the end of the 1st loop iteration,

`lastFoundIndex`

is updated to 0 since the goal point was found in between `path[0]`

and `path[1]`

. When the 2nd loop iteration starts, the robot has moved closer to `path[1]`

. Since `lastFoundIndex`

is still 0, the line-circle intersection starts from `path[0]`

again. This time, there are two intersections with the path: one located in between `path[0]`

and `path[1]`

and the other located in between `path[1]`

and `path[2]`

.

Following normal procedure, the algorithm would choose the intersection in between

`path[0]`

and `path[1]`

as the goal point and break out of the search loop. As we can tell from the picture below, it's not a very good choice since it will cause the robot to go backward. To avoid such bad goal point choice, we can add an additional condition to evaluate the goal point found by the algorithm: **the search loop break statement can only be reached if the distance between the goal point chosen and the next point in path is shorter than the distance between the robot's current position and the next point in path**. If the above statement is not true, the search loop continues. We can also increment`lastFoundIndex`

in case the robot fails to find intersections in the next line segment. This will prevent the robot from going backward since `path[lastFoundIndex]`

will become the goal point when no intersection can be found.Equivalent pseudo code:

if pt_to_pt_distance (goalPt, path[i+1]) < pt_to_pt_distance (currentPos, path[i+1]) :

break

else:

lastFoundIndex += 1

continue

In the situation illustrated below, it is obvious that the distance between the

`goalPt`

(the left intersection) and `path[1]`

is greater that the distance between `currentPos`

and `path[1]`

(both distances are marked with dotted red line). Hence, the search loop continues and the intersection between `path[1]`

and `path[2]`

will be chosen in the next search loop iteration.

At the end of the 2nd loop iteration,

`lastFoundIndex`

is updated to 1. When the 3rd loop iteration starts, the next search will start from `path[1]`

so the intersection in between `path[0]`

and `path[1]`

will be omitted. The portion of the path omitted for goal point searching is marked in brown.

The code cell below can be used for implementing the

`goal_pt_search`

algorithm yourself (for people that are not familiar enough with python, commented code is provided as before). A sample path has been provided.# implement the goal_pt_search algorithm

# last section of the visualization code needs to be uncommented when the function is done!!

# you will need the line-circle intersection with bounds algorithm completed in previous sections

path1 = [[0.0, 0.0], [0.011580143395790051, 0.6570165243709267], [0.07307496243411533, 1.2724369146199181], [0.3136756819515748, 1.7385910188236868], [0.8813313906933087, 1.9320292911046681], [1.6153051608455251, 1.9849785681091774], [2.391094224224885, 1.9878393390954208], [3.12721333474683, 1.938831731115573], [3.685011039017028, 1.7396821576569221], [3.9068092597113266, 1.275245079016133], [3.9102406525571713, 0.7136897450501469], [3.68346383786099, 0.2590283720040381], [3.1181273273535957, 0.06751996250999465], [2.3832776875784316, 0.013841087641154892], [1.5971423891000605, 0.0023698980178599423], [0.7995795475309813, 0.0003490964043320208], [0, 0]]

# helper functions

def pt_to_pt_distance (pt1,pt2):

distance = np.sqrt((pt2[0] - pt1[0])**2 + (pt2[1] - pt1[1])**2)

return distance

# returns -1 if num is negative, 1 otherwise

def sgn (num):

if num >= 0:

return 1

else:

return -1

def goal_pt_search (path, currentPos, lookAheadDis, lastFoundIndex) :

# initialize goalPt in case no intersection is found

goalPt = [None, None]

# use for loop to search intersections

startingIndex = lastFoundIndex

for i in range (startingIndex, len(path1)) :

# your code goes here

# store the final goal point in array goalPt = [goalPt_x, goalPt_y]

# right now you can just run the function as is to see the visualization of the path

# but when you are ready to code, the pass below needs to be removed

pass

# visualize outcome

# traveled path (path omitted for searching) will be marked in brown

field = plt.figure()

xscale,yscale = (1.5, 1.5) # <- modify these values to scale the plot

path_ax = field.add_axes([0,0,xscale,yscale])

add_complicated_line(path[0:lastFoundIndex+1],'--','brown','traveled path')

add_complicated_line(path[lastFoundIndex:len(path)],'--','grey','remaining path')

highlight_points(path[0:lastFoundIndex], 'brown')

highlight_points(path[lastFoundIndex:len(path)], 'grey')

xMin, yMin, xMax, yMax = (-1, -1, 5, 3) # <- modify these values to set plot boundaries

# (minX, minY, maxX, maxY)

# plot field

path_ax.plot([xMin,xMax],[yMin,yMin],color='black')

path_ax.plot([xMin,xMin],[yMin,yMax],color='black')

path_ax.plot([xMax,xMax],[yMin,yMax],color='black')

path_ax.plot([xMax,xMin],[yMax,yMax],color='black')

# set grid

xTicks = np.arange(xMin, xMax+1, 2)

yTicks = np.arange(yMin, yMax+1, 2)

path_ax.set_xticks(xTicks)

path_ax.set_yticks(yTicks)

path_ax.grid(True)

path_ax.set_xlim(xMin-0.25,xMax+0.25)

path_ax.set_ylim(yMin-0.25,yMax+0.25)

# plot start and end

path_ax.plot(path[0][0],path[0][1],'.',color='blue',markersize=15,label='start')

path_ax.plot(path[-1][0],path[-1][1],'.',color='green',markersize=15,label='end')

# plot current position and goal point

draw_circle (currentPos[0], currentPos[1], lookAheadDis, 'orange')

plt.plot (currentPos[0], currentPos[1], '.', markersize=15, color='orange', label='current position')

if goalPt != [None, None] :

plt.plot (goalPt[0], goalPt[1], '.', markersize=15, color='red', label='goal point')

add_complicated_line([currentPos, goalPt], '-', 'black', 'look ahead distance')

path_ax.legend()

# call the function to see the results

goal_pt_search (path1, [1, 2.2], 0.8, 3)

path1 = [[0.0, 0.0], [0.011580143395790051, 0.6570165243709267], [0.07307496243411533, 1.2724369146199181], [0.3136756819515748, 1.7385910188236868], [0.8813313906933087, 1.9320292911046681], [1.6153051608455251, 1.9849785681091774], [2.391094224224885, 1.9878393390954208], [3.12721333474683, 1.938831731115573], [3.685011039017028, 1.7396821576569221], [3.9068092597113266, 1.275245079016133], [3.9102406525571713, 0.7136897450501469], [3.68346383786099, 0.2590283720040381], [3.1181273273535957, 0.06751996250999465], [2.3832776875784316, 0.013841087641154892], [1.5971423891000605, 0.0023698980178599423], [0.7995795475309813, 0.0003490964043320208], [0, 0]]

# helper functions

def pt_to_pt_distance (pt1,pt2) :

distance = np.sqrt((pt2[0] - pt1[0])**2 + (pt2[1] - pt1[1])**2)

return distance

# returns -1 if num is negative, 1 otherwise

def sgn (num):

if num >= 0:

return 1

else:

return -1

def goal_pt_search (path, currentPos, lookAheadDis, lastFoundIndex) :

# extract currentX and currentY

currentX = currentPos[0]

currentY = currentPos[1]

# initialize goalPt in case no intersection is found

goalPt = [None, None]

# use for loop to search intersections

intersectFound = False

startingIndex = lastFoundIndex

for i in range (startingIndex, len(path)-1):

# beginning of line-circle intersection code

x1 = path[i][0] - currentX

y1 = path[i][1] - currentY

x2 = path[i+1][0] - currentX

y2 = path[i+1][1] - currentY

dx = x2 - x1

dy = y2 - y1

dr = math.sqrt (dx**2 + dy**2)

D = x1*y2 - x2*y1

discriminant = (lookAheadDis**2) * (dr**2) - D**2

if discriminant >= 0:

sol_x1 = (D * dy + sgn(dy) * dx * np.sqrt(discriminant)) / dr**2

sol_x2 = (D * dy - sgn(dy) * dx * np.sqrt(discriminant)) / dr**2

sol_y1 = (- D * dx + abs(dy) * np.sqrt(discriminant)) / dr**2

sol_y2 = (- D * dx - abs(dy) * np.sqrt(discriminant)) / dr**2

sol_pt1 = [sol_x1 + currentX, sol_y1 + currentY]

sol_pt2 = [sol_x2 + currentX, sol_y2 + currentY]

# end of line-circle intersection code

minX = min(path[i][0], path[i+1][0])

minY = min(path[i][1], path[i+1][1])

maxX = max(path[i][0], path[i+1][0])

maxY = max(path[i][1], path[i+1][1])

# if one or both of the solutions are in range

if ((minX <= sol_pt1[0] <= maxX) and (minY <= sol_pt1[1] <= maxY)) or ((minX <= sol_pt2[0] <= maxX) and (minY <= sol_pt2[1] <= maxY)):

foundIntersection = True

# if both solutions are in range, check which one is better

if ((minX <= sol_pt1[0] <= maxX) and (minY <= sol_pt1[1] <= maxY)) and ((minX <= sol_pt2[0] <= maxX) and (minY <= sol_pt2[1] <= maxY)):

# make the decision by comparing the distance between the intersections and the next point in path

if pt_to_pt_distance(sol_pt1, path[i+1]) < pt_to_pt_distance(sol_pt2, path[i+1]):

goalPt = sol_pt1

else:

goalPt = sol_pt2

# if not both solutions are in range, take the one that's in range

else:

# if solution pt1 is in range, set that as goal point

if (minX <= sol_pt1[0] <= maxX) and (minY <= sol_pt1[1] <= maxY):

goalPt = sol_pt1

else:

goalPt = sol_pt2

# only exit loop if the solution pt found is closer to the next pt in path than the current pos

if pt_to_pt_distance (goalPt, path[i+1]) < pt_to_pt_distance ([currentX, currentY], path[i+1]):

# update lastFoundIndex and exit

lastFoundIndex = i

break

else:

# in case for some reason the robot cannot find intersection in the next path segment, but we also don't want it to go backward

lastFoundIndex = i+1

# if no solutions are in range

else:

foundIntersection = False

# no new intersection found, potentially deviated from the path

# follow path[lastFoundIndex]

goalPt = [path[lastFoundIndex][0], path[lastFoundIndex][1]]

# visualize outcome

# traveled path (path omitted for searching) will be marked in brown

field = plt.figure()

xscale,yscale = (1.5, 1.5) # <- modify these values to scale the plot

path_ax = field.add_axes([0,0,xscale,yscale])

add_complicated_line(path[0:lastFoundIndex+1],'--','brown','traveled path')

add_complicated_line(path[lastFoundIndex:len(path)],'--','grey','remaining path')

highlight_points(path[0:lastFoundIndex], 'brown')

highlight_points(path[lastFoundIndex:len(path)], 'grey')

xMin, yMin, xMax, yMax = (-1, -1, 5, 3) # <- modify these values to set plot boundaries

# (minX, minY, maxX, maxY)

# plot field

path_ax.plot([xMin,xMax],[yMin,yMin],color='black')

path_ax.plot([xMin,xMin],[yMin,yMax],color='black')

path_ax.plot([xMax,xMax],[yMin,yMax],color='black')

path_ax.plot([xMax,xMin],[yMax,yMax],color='black')

# set grid

xTicks = np.arange(xMin, xMax+1, 2)

yTicks = np.arange(yMin, yMax+1, 2)

path_ax.set_xticks(xTicks)

path_ax.set_yticks(yTicks)

path_ax.grid(True)

path_ax.set_xlim(xMin-0.25,xMax+0.25)

path_ax.set_ylim(yMin-0.25,yMax+0.25)

# plot start and end

path_ax.plot(path[0][0],path[0][1],'.',color='blue',markersize=15,label='start')

path_ax.plot(path[-1][0],path[-1][1],'.',color='green',markersize=15,label='end')

# plot current position and goal point

draw_circle (currentX, currentY, lookAheadDis, 'orange')

plt.plot (currentX,currentY, '.', markersize=15, color='orange', label=