# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3
#     language: python
#     name: python3
# ---

# + id="Zafyd6PzV4es" executionInfo={"status": "ok", "timestamp": 1625919199215, "user_tz": -330, "elapsed": 381, "user": {"displayName": "<NAME>", "photoUrl": "", "userId": "11106137188993670892"}}
import numpy as np
import matplotlib.pyplot as plt

# + [markdown] id="VlmbOiJlV4fi"
# Remember that in week 1 we had generated open-loop commands for a set of manoeuvres such as
# $[("straight", 5), ("right", 90), ("straight", 6), ("left", 90)]$
#
# Let us do repeat, but with a change. Instead of left/ right, simply use turn and a signed angle.
# $[("straight", 5), ("turn", -90), ("straight", 6), ("turn", 90)]$
#
# You can use cubic_spiral() from previous notebook

# + id="_sWlVet8V4fz" executionInfo={"status": "ok", "timestamp": 1625919199258, "user_tz": -330, "elapsed": 322, "user": {"displayName": "<NAME>", "photoUrl": "", "userId": "11106137188993670892"}}
v = 1
dt = 0.1
# num_st_pts = 10
num_st_pts = int(v/dt)
num_pts = 50

def cubic_spiral(theta_i, theta_f, n=10):
    x = np.linspace(0, 1, num=n)
    #-2*x**3 + 3*x**2
    return (theta_f-theta_i)*(-2*x**3 + 3*x**2) + theta_i

def straight(dist, curr_pose, n=num_st_pts):
    # the straight-line may be along x or y axis
    #curr_theta will determine the orientation
    x0, y0, t0 = curr_pose
    xf, yf = x0 + dist*np.cos(t0), y0 + dist*np.sin(t0)
    x = (xf - x0) * np.linspace(0, 1, n) + x0
    y = (yf - y0) * np.linspace(0, 1, n) + y0
    return x, y, t0*np.ones_like(x)

def turn(change, curr_pose, n = num_pts):
    #adjust scaling constant for desired turn radius
    x0, y0, t0 = curr_pose
    theta = cubic_spiral(t0, t0 + np.deg2rad(change), n)
    x = x0 + np.cumsum(v*np.cos(theta)*dt)
    y = y0 + np.cumsum(v*np.sin(theta)*dt)
    return x, y, theta

def generate_trajectory(route, init_pose = (0, 0, np.pi/2)):
    curr_pose = init_pose
    func = {'straight': straight, 'turn': turn}
    x, y, t = np.array([]), np.array([]),np.array([])
    for manoeuvre, command in route:
        px, py, pt = func[manoeuvre](command, curr_pose) #calling function
        curr_pose = px[-1], py[-1], pt[-1]
        # update x, y, t using np.concatenate and px,py,pt
        x = np.concatenate([x, px])
        y = np.concatenate([y, py])
        t = np.concatenate([t, pt])
        
    return x, y, t


# + [markdown] id="Jyj6POG6V4gH"
# ### Plot the trajectory
# plot the trajectory and the change in orientation in separate plots

# + id="S5mcxQYhV4gT" colab={"base_uri": "https://localhost:8080/", "height": 282} executionInfo={"status": "ok", "timestamp": 1625919200871, "user_tz": -330, "elapsed": 1913, "user": {"displayName": "<NAME>", "photoUrl": "", "userId": "11106137188993670892"}} outputId="5443d060-5232-4802-eda7-23da9e297295"
route = [("straight", 5),("turn", -90), ("straight", 6), ("turn", 90)]
x, y, _ = generate_trajectory(route)
plt.figure()
plt.plot(x, y)

# + [markdown] id="XDKxqthIV4gd"
# ## Convert 
#
# A* or Djikstra gives a sequence of $\{(x_i, y_i)\}$. We need to convert it to a sequence of {"straight", "turn"} if we are use generate_trajectory()
#
# Let us look at a simple method. Assume that the successive line segments are orthogonal (reasonable in the grid world). If we find the corner point, we can demarcate. 
#
# For 3 consecutive points $(x_1,y_1), (x_2, y_2), (x_3, y_3)$ if 
# $(x_1 - x_2)(y_3-y2) - (x_3-x_2)(y_2-y_1) \neq 0$, then $(x_2, y_2)$ is a corner point
#
# Think about what is happening if
#
# 1. $(x_1 - x_2)(y_3-y2) - (x_3-x_2)(y_2-y_1) > 0$
#
# 2. $(x_1 - x_2)(y_3-y2) - (x_3-x_2)(y_2-y_1) < 0$

# + id="fzLm1PdsV4gn" colab={"base_uri": "https://localhost:8080/", "height": 265} executionInfo={"status": "ok", "timestamp": 1625919589517, "user_tz": -330, "elapsed": 2412, "user": {"displayName": "<NAME>", "photoUrl": "", "userId": "11106137188993670892"}} outputId="1bdb8b1d-5e16-4ae3-874e-ca97514aded0"
# here is a code to generate 2 orthogonal 
# line segments of lengths 6

s1, s2 = 6, 6
y1 = list(range(s1))
x1 = [0]*s1

x2 = list(range(s2))
y2 = [y1[-1]]*s2

x, y = x1[:-1]+x2, y1[:-1]+y2
plt.figure()
plt.plot(x, y)
plt.grid()

#find the corner point and plot it
corners = []
for x1,x2,x3, y1,y2,y3 in zip(x[:-2], x[1:-1], x[2:], y[:-2], y[1:-1], y[2:]):
  slope = (x2-x1)*(y3-y2) - (x3-x2)*(y2-y1)
  if np.abs(slope) > 0.0:
    corners.append([x2, y2])

# Fix a turn radius r
# Shorten the straight segments by r
# convert this into {("straight", s1), ("turn", +/- 90), ("straight", s2)}
route = [("straight", s1),("turn", -90), ("straight", s2)]

# use generate_trajectory() and plot the smooth path

# + [markdown] id="GzqeXmuGV4hA"
# # More complex example
# Borrow the Grid world code from week 2 notebook. Get the A* path and smoothen it using the routine from above

# + id="IdMIFMGkV4hM"


# + [markdown] id="41Q3kPd2V4hl"
# This approach of path planning with 90 deg turns juxtaposed between straight segments works well in structured environments.
#
# In the general case, where $A^*$/ $RRT^*$ path is a sequence of piecewise linear segments, we will perform a path optimization routine directly. 

# + [markdown] id="Ht_TSe76V4h7"
# There are 3 more advanced manouevres that you may need
#
# 1. Lane-change: Robot has to move laterally but without change to the orientation
#
# 2. Inplace: Robot has to turn around itself
#     
# 3. Reverse: Straights or turns in reverse
#
# Lane-change has to be applied as a combination of 2 cubic spirals (90 to 0 and 0 to 90). Inplace and Reverse are situational constructs

# + id="ycCWbEICV4iH"


StarcoderdataJupyterScriptsDedupFiltered