# 2D Dubins path algorithm

%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import leastsq

x = pos[0] + radius * np.cos(2 * np.pi * u + offset)
y = pos[1] + radius * np.sin(2 * np.pi * u + offset)
return x, y

u = np.linspace(0, 1, 50)

def get_frame(angle):
return np.array([[np.cos(angle), -np.sin(angle)],[np.sin(angle), np.cos(angle)]])

wp_1 = np.array([2, 3])
heading_1 = 10 * np.pi / 180
wp_2 = np.array([20, 32])
heading_2 = 130 * np.pi / 180

center_1 = dict(R=wp_1 - radius * frame_1[:, 1].flatten(),
L=wp_1 + radius * frame_1[:, 1].flatten())

center_2 = dict(R=wp_2 - radius * frame_2[:, 1].flatten(),
L=wp_2 + radius * frame_2[:, 1].flatten())

def plot_base():
fig = plt.figure(figsize=(10,8))

# Plot first waypoint's L and R circles
ax.plot([wp_1[0], wp_1[0] + 5 * np.cos(heading_1)], [wp_1[1], wp_1[1] + 5 * np.sin(heading_1)], color='xkcd:neon pink', linewidth=3)
ax.plot([wp_1[0]], [wp_1[1]], '.r', markersize=10)

ax.plot(x, y, '-.r')
ax.plot(x, y, '--r')

# Plot second waypoint's L and R circles
ax.plot([wp_2[0], wp_2[0] + 5 * np.cos(heading_2)], [wp_2[1], wp_2[1] + 5 * np.sin(heading_2)], color='xkcd:neon pink', linewidth=3)
ax.plot([wp_2[0]], [wp_2[1]], '.b', markersize=10)

ax.plot(x, y, '-.b')
ax.plot(x, y, '--b')

ax.axis('equal')
ax.grid(True)

return ax

plot_base()

<matplotlib.axes._subplots.AxesSubplot at 0x7fb7ec058b38>


print('RSR')

output = dict()

phi_1 = 2 * np.pi * u1 * delta_1 + heading_1 - delta_1 * np.pi / 2
phi_2 = 2 * np.pi * u2 * delta_2 + heading_2 - delta_2 * np.pi / 2

u1_func = lambda angle: (angle - heading_1 + delta_1 * np.pi / 2) / (delta_1 * 2 * np.pi)
u2_func = lambda angle: (angle - heading_2 + delta_2 * np.pi / 2) / (delta_2 * 2 * np.pi)
# Make tangents vector functions

# Make circle functions
circle_1_func = center_1 + radius_1 * np.array([np.cos(phi_1), np.sin(phi_1)])
circle_2_func = center_2 + radius_2 * np.array([np.cos(phi_2), np.sin(phi_2)])

# Plot the circles
ax = plot_base()

## Plot the circle center points
ax.plot([center_1[0]], [center_1[1]], marker='.', color='xkcd:baby blue', markersize=10)
ax.plot([center_2[0]], [center_2[1]], marker='.', color='xkcd:baby blue', markersize=10)

## Plot a couple of tangent vectors
for i in np.linspace(0.2, 0.8, 5):
ax.plot([c1[0], c1[0] + t1[0]],
[c1[1], c1[1] + t1[1]], 'k')

ax.plot([c2[0], c2[0] + t2[0]],
[c2[1], c2[1] + t2[1]], 'k')

## Plot line connecting the circle centers
ax.plot([center_1[0], center_2[0]], [center_1[1], center_2[1]], linestyle='--', color='xkcd:lavender')

## Plot the starting point of each circle
ax.plot([c1[0]], [c1[1]], marker='o', color='xkcd:reddish orange', markersize=15)
ax.plot([c2[0]], [c2[1]], marker='o', color='xkcd:reddish orange', markersize=15)

# Compute the line connecting the circle's centers
d = center_2 - center_1
# Calculate normal vector to the connecting line
n = np.dot(get_frame(np.pi / 2), d / np.linalg.norm(d))

## Plotting the normal vectors
ax.plot([center_1[0], center_1[0] + radius_1 * n[0]], [center_1[1], center_1[1] + radius_1 * n[1]], linestyle='--', color='xkcd:hot pink')
ax.plot([center_2[0], center_2[0] + radius_2 * n[0]], [center_2[1], center_2[1] + radius_2 * n[1]], linestyle='--', color='xkcd:hot pink')

##########################################################
# Compute the first tangent
## Compute the normal vector's angle
n_angle = np.arctan2(n[1], n[0])
## Compute the parameter for the tangent points on both circles
u1_opt = u1_func(n_angle)
if u1_opt < 0:
u1_opt = u1_opt + 1
u2_opt = u2_func(n_angle)
if u2_opt < 0:
u2_opt = u2_opt + 1

## Compute the points on the circles for the first tangent

tangent_1 = c2 - c1

ax.plot([c1[0], c2[0]], [c1[1], c2[1]], linestyle='--', color='xkcd:kelly green')

## Compute the tangent vectors on the circles

if np.isclose(diff, 0):
u = np.arange(0, u1_opt, 0.001)
u = np.arange(u2_opt, 1, 0.001)

## Plot the tangent vectors on the circles that are parallel to the first tangent
ax.plot([c1[0], c1[0] + radius_1 * t1[0]], [c1[1], c1[1] + radius_1 * t1[1]], linestyle='-', color='xkcd:bright purple')
ax.plot([c2[0], c2[0] + radius_2 * t2[0]], [c2[1], c2[1] + radius_2 * t2[1]], linestyle='-', color='xkcd:bright purple')

##########################################################
# Compute the second tangent
n_angle = np.arctan2(-n[1], -n[0])
u1_opt = u1_func(n_angle)
if u1_opt < 0:
u1_opt = u1_opt + 1
u2_opt = u2_func(n_angle)
if u2_opt < 0:
u2_opt = u2_opt + 1

tangent_2 = c2 - c1

## Plotting the second tangent
ax.plot([c1[0], c2[0]], [c1[1], c2[1]], linestyle='--', color='xkcd:kelly green')

## Compute the tangent vectors on the circles

if np.isclose(diff, 0):
u = np.arange(0, u1_opt, 0.001)
u = np.arange(u2_opt, 1, 0.001)

## Plot the tangent vectors on the circles that are parallel to the second tangent
ax.plot([c1[0], c1[0] + radius_1 * t1[0]], [c1[1], c1[1] + radius_1 * t1[1]], linestyle='-', color='xkcd:bright purple')
ax.plot([c2[0], c2[0] + radius_2 * t2[0]], [c2[1], c2[1] + radius_2 * t2[1]], linestyle='-', color='xkcd:bright purple')

##########################################################
# Computing inner tangents
# Calculate the intersection point of the two tangent lines

ax.plot([xp], [yp], '.r', markersize=10)
# Third and fourth tangents
xt1 = (radius_1**2 * (xp - center_1[0]) + radius_1 * (yp - center_1[1]) * np.sqrt((xp - center_1[0])**2 + (yp - center_1[1])**2 - radius_1**2)) / ((xp - center_1[0])**2 + (yp - center_1[1])**2) + center_1[0]
xt2 = (radius_1**2 * (xp - center_1[0]) - radius_1 * (yp - center_1[1]) * np.sqrt((xp - center_1[0])**2 + (yp - center_1[1])**2 - radius_1**2)) / ((xp - center_1[0])**2 + (yp - center_1[1])**2) + center_1[0]

yt1 = ((radius_1**2 * (yp - center_1[1])) - radius_1 * (xp - center_1[0]) * np.sqrt((xp - center_1[0])**2 + (yp - center_1[1])**2 - radius_1**2)) / ((xp - center_1[0])**2 + (yp - center_1[1])**2) + center_1[1]
yt2 = ((radius_1**2 * (yp - center_1[1])) + radius_1 * (xp - center_1[0]) * np.sqrt((xp - center_1[0])**2 + (yp - center_1[1])**2 - radius_1**2)) / ((xp - center_1[0])**2 + (yp - center_1[1])**2) + center_1[1]

## Plotting the tangent points on the first circle
ax.plot([xt1, xt2], [yt1, yt2], '.r', markersize=10)

xt3 = (radius_2**2 * (xp - center_2[0]) + radius_2 * (yp - center_2[1]) * np.sqrt((xp - center_2[0])**2 + (yp - center_2[1])**2 - radius_2**2)) / ((xp - center_2[0])**2 + (yp - center_2[1])**2) + center_2[0]
xt4 = (radius_2**2 * (xp - center_2[0]) - radius_2 * (yp - center_2[1]) * np.sqrt((xp - center_2[0])**2 + (yp - center_2[1])**2 - radius_2**2)) / ((xp - center_2[0])**2 + (yp - center_2[1])**2) + center_2[0]

yt3 = ((radius_2**2 * (yp - center_2[1])) - radius_2 * (xp - center_2[0]) * np.sqrt((xp - center_2[0])**2 + (yp - center_2[1])**2 - radius_2**2)) / ((xp - center_2[0])**2 + (yp - center_2[1])**2) + center_2[1]
yt4 = ((radius_2**2 * (yp - center_2[1])) + radius_2 * (xp - center_2[0]) * np.sqrt((xp - center_2[0])**2 + (yp - center_2[1])**2 - radius_2**2)) / ((xp - center_2[0])**2 + (yp - center_2[1])**2) + center_2[1]

## Plotting the tangent points on the second circle
ax.plot([xt3, xt4], [yt3, yt4], '.r', markersize=10)

# Third tangent
u1_opt = u1_func(np.arctan2(yt1 - center_1[1], xt1 - center_1[0]))
if u1_opt < 0:
u1_opt = u1_opt + 1
u2_opt = u2_func(np.arctan2(yt3 - center_2[1], xt3 - center_2[0]))
if u2_opt < 0:
u2_opt = u2_opt + 1

## Plot the tangent vectors on the circles that are parallel to the third tangent
ax.plot([c1[0], c1[0] + radius_1 * t1[0]], [c1[1], c1[1] + radius_1 * t1[1]], linestyle='-', color='xkcd:bright purple')
ax.plot([c2[0], c2[0] + radius_2 * t2[0]], [c2[1], c2[1] + radius_2 * t2[1]], linestyle='-', color='xkcd:bright purple')

tangent_3 = np.array([xt3 - xt1, yt3 - yt1])
tangent_3 /= np.linalg.norm(tangent_3)

if np.isclose(diff, 0):
u = np.arange(0, u1_opt, 0.001)
u = np.arange(u2_opt, 1, 0.001)

# Fourth tangent
u1_opt = u1_func(np.arctan2(yt2 - center_1[1], xt2 - center_1[0]))
if u1_opt < 0:
u1_opt = u1_opt + 1
u2_opt = u2_func(np.arctan2(yt4 - center_2[1], xt4 - center_2[0]))
if u2_opt < 0:
u2_opt = u2_opt + 1

## Plot the tangent vectors on the circles that are parallel to the fourth tangent
ax.plot([c1[0], c1[0] + radius_1 * t1[0]], [c1[1], c1[1] + radius_1 * t1[1]], linestyle='-', color='xkcd:bright purple')
ax.plot([c2[0], c2[0] + radius_2 * t2[0]], [c2[1], c2[1] + radius_2 * t2[1]], linestyle='-', color='xkcd:bright purple')

tangent_4 = np.array([xt4 - xt2, yt4 - yt2])
tangent_4 /= np.linalg.norm(tangent_4)

if np.isclose(diff, 0):
u = np.arange(0, u1_opt, 0.001)
u = np.arange(u2_opt, 1, 0.001)

ax.plot([xt1, xt3], [yt1, yt3], '--c')
ax.plot([xt2, xt4], [yt2, yt4], '--c')

#########################################################
# Plot the path
ax.plot([x[0] for x in output['C1']], [x[1] for x in output['C1']], color='xkcd:golden yellow', linewidth=3)
ax.plot([x[0] for x in output['S']], [x[1] for x in output['S']], color='xkcd:vermillion', linewidth=3)
ax.plot([x[0] for x in output['C2']], [x[1] for x in output['C2']], color='xkcd:bright magenta', linewidth=3)


RSR


print('LSR')


LSR


print('RSL')


RSL

print('LSL')

LSL