```python
import numpy as np
import matplotlib.pyplot as plt
from sympy import Symbol, integrate
%matplotlib notebook
```

### Smooth local paths
We will use cubic spirals to generate smooth local paths. Without loss of generality, as $\theta$ smoothly changes from 0 to 1, we impose a condition on the curvature as follows

$\kappa = f'(x) = K(x(1-x))^n $

This ensures curvature vanishes at the beginning and end of the path. Integrating, the yaw changes as
$\theta = \int_0^x f'(x')dx'$

With $n = 1$ we get a cubic spiral, $n=2$ we get a quintic spiral and so on. Let us use the sympy package to find the family of spirals

1. Declare $x$ a Symbol

2. You want to find Integral of $f'(x)$

3. You can choose $K$ so that all coefficients are integers

Verify if $\theta(0) = 0$ and $\theta(1) = 1$


```python
K =  30 #choose for cubic/quintic
n =  2 #choose for cubic/ quintic
x =  Symbol('x') #declare as Symbol
print(integrate(K*(x*(1-x))**n, x)) # complete the expression
```

    6*x**5 - 15*x**4 + 10*x**3



```python
x = np.linspace(0, 1, num=100)
thetas = -2*x**3 + 3*x**2
plt.figure()
plt.plot(x, thetas,'.')
thetas = 6*x**5 - 15*x**4 + 10*x**3
plt.plot(x, thetas,'.')
```


```python
#write function to compute a cubic spiral
#input can be any theta_i and theta_f (not just 0 and 1)
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 quintic_spiral(theta_i, theta_f, n=10):
    x = np.linspace(0, 1, num=n)   
    #6*x**5 - 15*x**4 + 10*x**3 
    return (theta_f-theta_i)*(6*x**5 - 15*x**4 + 10*x**3) + theta_i
```

### Plotting
Plot cubic, quintic spirals along with how $\theta$ will change from $\pi/2$ to $0$ when moving in a circular arc. Remember circular arc is when  $\omega $ is constant



```python
num_pts = 100
plt.figure()
plt.plot(np.pi/2*(1-np.linspace(0,1,num_pts)), label='Circular')
plt.plot(cubic_spiral(np.pi/2, 0, num_pts), label='Cubic')
plt.plot(quintic_spiral(np.pi/2, 0, num_pts),label='Quintic')
plt.grid()
plt.legend()
```

## Trajectory

Using the spirals, convert them to trajectories $\{(x_i,y_i,\theta_i)\}$. Remember the unicycle model 

$dx = v\cos \theta dt$

$dy = v\sin \theta dt$

$\theta$ is given by the spiral functions you just wrote. Use cumsum() in numpy to calculate {(x_i, y_i)}

What happens when you change $v$?


```python
num_pts = 50
v = 1
dt = 0.1

#cubic
theta = cubic_spiral(np.pi/2, np.pi, num_pts)
x = np.cumsum(v*np.cos(theta)*dt)
y = np.cumsum(v*np.sin(theta)*dt)

#Quintic
theta = quintic_spiral(np.pi/2, np.pi, num_pts+2)
xq = np.cumsum(v*np.cos(theta)*dt)
yq = np.cumsum(v*np.sin(theta)*dt)

#Circular
theta = np.pi/2*(1+np.linspace(0,1,num_pts-2))
xc = np.cumsum(v*np.cos(theta)*dt)
yc = np.cumsum(v*np.sin(theta)*dt)

# plot trajectories for circular/ cubic/ quintic
plt.figure()
plt.plot(xc, yc, label='Circular')
plt.plot(x, y, label='Cubic')
plt.plot(xq, yq, label='Quintic')
plt.legend()
plt.grid()
```

## Symmetric poses

We have been doing only examples with $|\theta_i - \theta_f| = \pi/2$. 

What about other orientation changes? Given below is an array of terminal angles (they are in degrees!). Start from 0 deg and plot the family of trajectories


```python
dt = 0.1
thetas = np.deg2rad([15, 30, 45, 60, 90, 120, 150, 180]) #convert to radians
plt.figure()
for tf in thetas:
    t = cubic_spiral(0, tf,50)
    x = np.cumsum(np.cos(t)*dt)
    y = np.cumsum(np.sin(t)*dt)
    plt.plot(x, y)

# On the same plot, move from 180 to 180 - theta
thetas = np.pi - np.deg2rad([15, 30, 45, 60, 90, 120, 150, 180])
for tf in thetas:
    t = cubic_spiral(np.pi, tf, 50)
    x = np.cumsum(np.cos(t)*dt)
    y = np.cumsum(np.sin(t)*dt)
    plt.plot(x, y)

plt.grid()
```

Modify your code to print the following for the positive terminal angles $\{\theta_f\}$
1. Final x, y position in corresponding trajectory: $x_f, y_f$ 
2. $\frac{y_f}{x_f}$ and $\tan \frac{\theta_f}{2}$

What do you notice? 
What happens when $v$ is doubled?


```python
dt = 0.05
v = 2.0
thetas = np.deg2rad([15, 30, 45, 60, 90, 120, 150, 180]) #convert to radians
for tf in thetas:
    t = cubic_spiral(0, tf,100)
    x = np.cumsum(v*np.cos(t)*dt)
    y = np.cumsum(v*np.sin(t)*dt)
    print(f"tf:{np.rad2deg(tf):0.1f} xf:{x[-1]:0.3f} yf:{y[-1]:0.3f} yf/xf:{y[-1]/x[-1]:0.3f} tan(theta/2):{np.tan(tf/2):0.3f}")
```

    tf:15.0 xf:9.873 yf:1.300 yf/xf:0.132 tan(theta/2):0.132
    tf:30.0 xf:9.497 yf:2.545 yf/xf:0.268 tan(theta/2):0.268
    tf:45.0 xf:8.892 yf:3.683 yf/xf:0.414 tan(theta/2):0.414
    tf:60.0 xf:8.087 yf:4.669 yf/xf:0.577 tan(theta/2):0.577
    tf:90.0 xf:6.041 yf:6.041 yf/xf:1.000 tan(theta/2):1.000
    tf:120.0 xf:3.743 yf:6.484 yf/xf:1.732 tan(theta/2):1.732
    tf:150.0 xf:1.610 yf:6.010 yf/xf:3.732 tan(theta/2):3.732
    tf:180.0 xf:-0.000 yf:4.812 yf/xf:-7880729543884461.000 tan(theta/2):16331239353195370.000


These are called *symmetric poses*. With this spiral-fitting approach, only symmetric poses can be reached. 

In order to move between any 2 arbitrary poses, you will have to find an intermediate pose that is pair-wise symmetric to the start and the end pose. 

What should be the intermediate pose? There are infinite possibilities. We would have to formulate it as an optimization problem. As they say, that has to be left for another time!


```python

```

d7b92229213d9a91b53628560cfe1db83a4d81e8
ipynb
Jupyter Notebook
week3/maithili/Q2 - 1/Attempt1_filesubmission_cubic_quintic_spirals.ipynb
naveenmoto/lablet102
24de9daa4ae75cbde93567a3239ede43c735cf03
MIT
2021-07-09T16:48:44.000Z
2021-07-09T16:48:44.000Z
week3/maithili/Q2 - 1/Attempt1_filesubmission_cubic_quintic_spirals.ipynb
naveenmoto/lablet102
24de9daa4ae75cbde93567a3239ede43c735cf03
MIT
week3/maithili/Q2 - 1/Attempt1_filesubmission_cubic_quintic_spirals.ipynb
naveenmoto/lablet102
24de9daa4ae75cbde93567a3239ede43c735cf03
MIT
Qwen/Qwen-72B
1. YES
2. YES