Created
December 14, 2017 08:50
-
-
Save serser/0babf245005d90fec83c828cb848299d to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# ----------- | |
# User Instructions | |
# | |
# Implement a PD controller by running 100 iterations | |
# of robot motion. The steering angle should be set | |
# by the parameter tau so that: | |
# | |
# steering = -tau_p * CTE - tau_d * diff_CTE | |
# where differential crosstrack error (diff_CTE) | |
# is given by CTE(t) - CTE(t-1) | |
# | |
# Your code should print output that looks like | |
# the output shown in the video. | |
# | |
# Only modify code at the bottom! | |
# ------------ | |
import random | |
import numpy as np | |
import matplotlib.pyplot as plt | |
#%matplotlib inline | |
# ------------------------------------------------ | |
# | |
# this is the Robot class | |
# | |
class Robot(object): | |
def __init__(self, length=20.0): | |
""" | |
Creates robot and initializes location/orientation to 0, 0, 0. | |
""" | |
self.x = 0.0 | |
self.y = 0.0 | |
self.orientation = 0.0 | |
self.length = length | |
self.steering_noise = 0.0 | |
self.distance_noise = 0.0 | |
self.steering_drift = 0.0 | |
def set(self, x, y, orientation): | |
""" | |
Sets a robot coordinate. | |
""" | |
self.x = x | |
self.y = y | |
self.orientation = orientation % (2.0 * np.pi) | |
def set_noise(self, steering_noise, distance_noise): | |
""" | |
Sets the noise parameters. | |
""" | |
# makes it possible to change the noise parameters | |
# this is often useful in particle filters | |
self.steering_noise = steering_noise | |
self.distance_noise = distance_noise | |
def set_steering_drift(self, drift): | |
""" | |
Sets the systematical steering drift parameter | |
""" | |
self.steering_drift = drift | |
def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0): | |
""" | |
steering = front wheel steering angle, limited by max_steering_angle | |
distance = total distance driven, most be non-negative | |
""" | |
if steering > max_steering_angle: | |
steering = max_steering_angle | |
if steering < -max_steering_angle: | |
steering = -max_steering_angle | |
if distance < 0.0: | |
distance = 0.0 | |
# make a new copy | |
# res = Robot() | |
# res.length = self.length | |
# res.steering_noise = self.steering_noise | |
# res.distance_noise = self.distance_noise | |
# res.steering_drift = self.steering_drift | |
# apply noise | |
steering2 = random.gauss(steering, self.steering_noise) | |
distance2 = random.gauss(distance, self.distance_noise) | |
# apply steering drift | |
steering2 += self.steering_drift | |
# Execute motion | |
turn = np.tan(steering2) * distance2 / self.length | |
if abs(turn) < tolerance: | |
# approximate by straight line motion | |
self.x += distance2 * np.cos(self.orientation) | |
self.y += distance2 * np.sin(self.orientation) | |
self.orientation = (self.orientation + turn) % (2.0 * np.pi) | |
else: | |
# approximate bicycle model for motion | |
radius = distance2 / turn | |
cx = self.x - (np.sin(self.orientation) * radius) | |
cy = self.y + (np.cos(self.orientation) * radius) | |
self.orientation = (self.orientation + turn) % (2.0 * np.pi) | |
self.x = cx + (np.sin(self.orientation) * radius) | |
self.y = cy - (np.cos(self.orientation) * radius) | |
def __repr__(self): | |
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) | |
############## ADD / MODIFY CODE BELOW #################### | |
# ------------------------------------------------------------------------ | |
# | |
# run - does a single control run | |
# previous P controller | |
def run_p(robot, tau, n=100, speed=1.0): | |
x_trajectory = [] | |
y_trajectory = [] | |
for i in range(n): | |
cte = robot.y | |
steer = -tau * cte | |
robot.move(steer, speed) | |
x_trajectory.append(robot.x) | |
y_trajectory.append(robot.y) | |
return x_trajectory, y_trajectory | |
robot = Robot() | |
robot.set(0, 1, 0) | |
def run(robot, tau_p, tau_d, n=100, speed=1.0): | |
x_trajectory = [] | |
y_trajectory = [] | |
# TODO: your code here | |
last_cte = robot.y | |
x_trajectory.append(robot.x) | |
y_trajectory.append(robot.y) | |
for i in range(n): | |
steer = - tau_p * robot.y - tau_d * (robot.y - y_trajectory[-1]) | |
x_trajectory.append(robot.x) | |
y_trajectory.append(robot.y) | |
robot.move(steer, speed) | |
last_cte = robot.y | |
print (robot, steer) | |
return x_trajectory, y_trajectory | |
#x_trajectory, y_trajectory = run_p(robot, 0.2) | |
x_trajectory, y_trajectory = run(robot, 0.2, 3.0) | |
n = len(x_trajectory) | |
plt.plot(x_trajectory, y_trajectory, 'g', label='PD controller') | |
plt.plot(x_trajectory, np.zeros(n), 'r', label='reference') | |
plt.legend() | |
plt.show() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment