<jupyter_start><jupyter_text>In the previous notebook you simulated the Dubins Car model by choosing random steering angles. In this case, you'll consider the case where you have a destination `x2` in mind, and you guess controls that move you toward your destination (maybe ;).<jupyter_code>import numpy as np
import matplotlib.pyplot as plt

%matplotlib inline
plt.rcParams['figure.figsize'] = 12, 12<jupyter_output><empty_output><jupyter_text>Implement a function called `steer()`, which takes as inputs two states `x1` and `x2` and returns a control input (steering angle) that "tries" to make progress from `x1` to `x2`. This method should consist roughly of the following:1. Get a sense of where `x2` is relative to the state of `x1` by calculating the orientation difference between the current orientation, $\theta$, and the direction toward `x2`.2. Generate a random steering angle by sampling from a gaussian that is centered roughly on some direction that will move you from `x1` to `x2`.3. Clip the angle if it is outside the range allowed by `MAX_STEERING_ANGLE`.This might all sound a little fuzzy, but that's the idea here! We're just going to try some guessing and see if we can find a simple method for making progress from `x1` to `x2`.<jupyter_code># limit the steering angle range
MAX_STEERING_ANGLE = np.deg2rad(30)
# Set the width of the Gaussian we'll draw angles from
ANGLE_STDDEV = np.deg2rad(3)

def steer(x1, x2):
    theta = x1[2]
    
    # Calculate the vector between the two points
    vec = np.array(x2[:2]) - np.array(x1[:1])
    # Convert that vector to an angle
    ang = np.arctan2(vec[1], vec[0])
    
    # Subtract the current orientation, and wrap the angle between
    # [-pi, pi]
    delta = np.fmod(ang - theta + np.pi, 2*np.pi) - np.pi
    
    # TODO: return steering angle
    return np.clip(np.random.normal(delta, ANGLE_STDDEV), 
                   -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE)<jupyter_output><empty_output><jupyter_text>Modification of the Dubin's car simulation from the previous notebook.<jupyter_code>def simulate(state, angle, v, dt):
    x = state[0]
    y = state[1]
    theta = state[2]    
    
    nx = x + v*np.cos(theta)*dt
    ny = y + v*np.sin(theta)*dt
    ntheta = theta + v*np.tan(angle)*dt
    
    return [nx, ny, ntheta]<jupyter_output><empty_output><jupyter_text>Initially, we place the car at the center of the coordinate system (0, 0) and give heading of $0$ which is facing in the positive direction along the $x$ axis.<jupyter_code># travel in time increments of `dt` up until `T`.
dt = 0.2
T = 10

start = [0, 0, 0]
goal = [10, 3, 0]

# Run the simulation 100 times to get a feel for the output
angles = [steer(start, goal) for _ in range(100)]
lines = []

for angle in angles:
    line = [start]
    state = np.copy(start)
    v = np.random.uniform(0, 1)
    for _ in np.arange(0, T, dt):
        state = simulate(state, angle, v, dt)
        line.append(state)
    lines.append(line)
    
lines = np.array(lines)
print(lines.shape)<jupyter_output>(100, 51, 3)<jupyter_text>Now let's visualize the path! It should be a smooth path from the start location to the goal location.<jupyter_code>for i in range(lines.shape[0]):
    plt.plot(lines[i, :, 0], lines[i, :, 1], 'b-')
plt.plot(start[0], start[1], 'bo')
plt.plot(goal[0], goal[1], 'ro')
plt.axis('equal')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.11055864287803452, 'fraction_numerical': 0.02193623866627669, 'mean_word_length': 3.9565217391304346, 'pattern_counts': {'":': 0, '<': 20, '<?xml version=': 0, '>': 20, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '5', 'dir': 'jupyter-structured-clean-dedup', 'id': '597591', 'license': 'MIT', 'n_tokens_mistral': 1202, 'n_tokens_neox': 1127, 'n_words': 424, 'path': 'jupyter_notebooks/2_Planning/5_RealWorldPlanning/Steering.ipynb', 'repo_name': 'miker2/FCND-udacity', 'size': '3419'}