# -----------# User Instructions## Define a function smooth that takes a path as its input# (with optional parameters for weight_data, weight_smooth,# and tolerance) and returns a smooth path. The first and # last points should remain unchanged.## Smoothing should be implemented by iteratively updating# each entry in newpath until some desired level of accuracy# is reached. The update should be done according to the# gradient descent equations given in the instructor's note# below (the equations given in the video are not quite # correct).# -----------from copy import deepcopy
# thank you to EnTerr for posting this on our discussion forumdefprintpaths(path,newpath):for old,new in zip(path,newpath):
print'['+ ', '.join('%.3f'%x for x in old) + \
'] -> ['+ ', '.join('%.3f'%x for x in new) +']'# Don't modify path inside your function.
path = [[0, 0],
[0, 1],
[0, 2],
[1, 2],
[2, 2],
[3, 2],
[4, 2],
[4, 3],
[4, 4]]
defsmooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001):# Make a deep copy of path into newpath
newpath = deepcopy(path)
########################## ENTER CODE HERE ##########################
st = 1000while(st > tolerance):
newpath_pre = deepcopy(newpath)
st = 0for i in range(1, len(path)-1):
for j in range(len(path[0])):
newpath[i][j] += weight_data*(path[i][j]-newpath[i][j]) + \
weight_smooth*(newpath[i-1][j] + newpath[i+1][j]- 2.0*newpath[i][j])
st += pow(newpath[i][j]-newpath_pre[i][j], 2)
return newpath # Leave this line for the grader!
printpaths(path,smooth(path))
Constrained Smooth
# -------------# User Instructions## Now you will be incorporating fixed points into# your smoother. ## You will need to use the equations from gradient# descent AND the new equations presented in the# previous lecture to implement smoothing with# fixed points.## Your function should return the newpath that it# calculates. ## Feel free to use the provided solution_check function# to test your code. You can find it at the bottom.#from copy import deepcopy
######################## ENTER CODE BELOW HERE #########################defsmooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.00001):## Enter code here. # The weight for each of the two new equations should be 0.5 * weight_smooth#
newpath = deepcopy(path)
st = 1000while(st > tolerance):
newpath_pre = deepcopy(newpath)
st = 0for i in range(0, len(path)):
if fix[i]:
continuefor j in range(len(path[0])):
# newpath[i][j] += weight_data*(path[i][j]-newpath[i][j]) + \# weight_smooth*(newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j]- 2.0*newpath[i][j])
newpath[i][j] += weight_data*(path[i][j]-newpath[i][j]) + \
weight_smooth*(newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j]- 2.0*newpath[i][j]) + \
(weight_smooth/2.0)*(2.0*newpath[(i-1)%len(path)][j] - newpath[(i-2)%len(path)][j] - newpath[i][j]) + \
(weight_smooth/2.0)*(2.0*newpath[(i+1)%len(path)][j] - newpath[(i+2)%len(path)][j] - newpath[i][j])
# st += pow(newpath[i][j]-newpath_pre[i][j], 2)
st += abs(newpath[i][j]-newpath_pre[i][j])
return newpath
# --------------# Testing Instructions# # To test your code, call the solution_check function with the argument smooth:# solution_check(smooth)#defsolution_check(smooth, eps = 0.0001):deftest_case_str(path, fix):assert( len(path) == len(fix) )
if len(path) == 0:
return'[]'if len(path) == 1:
s = '[' + str(path[0]) + ']'if fix[0]: s += ' #fix'return s
s = '[' + str(path[0]) + ','if fix[0]: s += ' #fix'for pt,f in zip(path[1:-1],fix[1:-1]):
s += '\n ' + str(pt) + ','if f: s += ' #fix'
s += '\n ' + str(path[-1]) + ']'if fix[-1]: s += ' #fix'return s
testpaths = [[[0, 0],[1, 0],[2, 0],[3, 0],[4, 0],[5, 0],[6, 0],[6, 1],[6, 2],[6, 3],[5, 3],[4, 3],[3, 3],[2, 3],[1, 3],[0, 3],[0, 2],[0, 1]],
[[0, 0],[2, 0],[4, 0],[4, 2],[4, 4],[2, 4],[0, 4],[0, 2]]]
testfixpts = [[1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0],
[1, 0, 1, 0, 1, 0, 1, 0]]
pseudo_answers = [[[0, 0],[0.7938620981547201, -0.8311168821106101],[1.8579052986461084, -1.3834788165869276],[3.053905318597796, -1.5745863173084],[4.23141390533387, -1.3784271816058231],[5.250184859723701, -0.8264215958231558],[6, 0],[6.415150091996651, 0.9836951698796843],[6.41942442687092, 2.019512290770163],[6, 3],[5.206131365604606, 3.831104483245191],[4.142082497497067, 4.383455704596517],[2.9460804122779813, 4.5745592975708105],[1.768574219397359, 4.378404668718541],[0.7498089205417316, 3.826409771585794],[0, 3],[-0.4151464728194156, 2.016311854977891],[-0.4194207879552198, 0.9804948340550833]],
[[0, 0],[2.0116767115496095, -0.7015439080661671],[4, 0],[4.701543905420104, 2.0116768147460418],[4, 4],[1.9883231877640861, 4.701543807525115],[0, 4],[-0.7015438099112995, 1.9883232808252207]]]
true_answers = [[[0, 0],[0.7826068175979299, -0.6922616156406778],[1.826083356960912, -1.107599209206985],[2.999995745732953, -1.2460426422963626],[4.173909508264126, -1.1076018591282746],[5.217389489606966, -0.6922642758483151],[6, 0],[6.391305105067843, 0.969228211275216],[6.391305001845138, 2.0307762911524616],[6, 3],[5.217390488523538, 3.6922567975830876],[4.17391158149052, 4.107590195596796],[2.9999982969959467, 4.246032043344827],[1.8260854997325473, 4.107592961155283],[0.7826078838205919, 3.692259569132191],[0, 3],[-0.3913036785959153, 2.030774470796648], [-0.3913035729270973, 0.9692264531461132]],
[[0, 0],[1.9999953708444873, -0.6666702980585777],[4, 0],[4.666670298058577, 2.000005101453379],[4, 4],[1.9999948985466212, 4.6666612524128],[0, 4],[-0.6666612524127998, 2.000003692691148]]]
newpaths = map(lambda p: smooth(*p), zip(testpaths,testfixpts))
correct = Truefor path,fix,p_answer,t_answer,newpath in zip(testpaths,testfixpts,
pseudo_answers,true_answers,
newpaths):
if type(newpath) != list:
print"Error: smooth did not return a list for the path:"print test_case_str(path,fix) + '\n'
correct = Falsecontinueif len(newpath) != len(path):
print"Error: smooth did not return a list of the correct length for the path:"print test_case_str(path,fix) + '\n'
correct = Falsecontinue
good_pairs = Truefor newpt,pt in zip(newpath,path):
if len(newpt) != len(pt):
good_pairs = Falsebreakifnot good_pairs:
print"Error: smooth did not return a list of coordinate pairs for the path:"print test_case_str(path,fix) + '\n'
correct = Falsecontinueassert( good_pairs )
# check whether to check against true or pseudo answers
answer = Noneif abs(newpath[1][0] - t_answer[1][0]) <= eps:
answer = t_answer
elif abs(newpath[1][0] - p_answer[1][0]) <= eps:
answer = p_answer
else:
print'smooth returned an incorrect answer for the path:'print test_case_str(path,fix) + '\n'continueassert( answer isnotNone )
entries_match = Truefor p,q in zip(newpath,answer):
for pi,qi in zip(p,q):
if abs(pi - qi) > eps:
entries_match = Falsebreakifnot entries_match: breakifnot entries_match:
print'smooth returned an incorrect answer for the path:'print test_case_str(path,fix) + '\n'continueassert( entries_match )
if answer == t_answer:
print'smooth returned the correct answer for the path:'print test_case_str(path,fix) + '\n'elif answer == p_answer:
print'smooth returned a correct* answer for the path:'print test_case_str(path,fix)
print'''*However, your answer uses the "nonsimultaneous" update method, which
is not technically correct. You should modify your code so that newpath[i][j] is only
updated once per iteration, or else the intermediate updates made to newpath[i][j]
will affect the final answer.\n'''
solution_check(smooth)
PD Control
# -----------# User Instructions## Implement a PD controller by running 100 iterations# of robot motion. The steering angle should be set# by the parameter tau_p and tau_d so that:## steering = -tau_p * CTE - tau_d * diff_CTE# where differential crosstrack error (diff_CTE)# is given by CTE(t) - CTE(t-1)### Only modify code at the bottom! Look for the TODO# ------------import random
import numpy as np
import matplotlib.pyplot as plt
# ------------------------------------------------# # this is the Robot class#classRobot(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.0defset(self, x, y, orientation):"""
Sets a robot coordinate.
"""
self.x = x
self.y = y
self.orientation = orientation % (2.0 * np.pi)
defset_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
defset_steering_drift(self, drift):"""
Sets the systematical steering drift parameter
"""
self.steering_drift = drift
defmove(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# 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 controllerdefrun_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)
defrun(robot, tau_p, tau_d, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
# TODO: your code here
pre_cte = robot.y
for i in range(n):
cte = robot.y
steer = -tau_p*cte - tau_d*(cte-pre_cte)
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
pre_cte = cte
return x_trajectory, y_trajectory
x_trajectory, y_trajectory = run(robot, 0.2, 3.0)
n = len(x_trajectory)
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='PD controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')
PID control
# -----------# User Instructions## Implement a P 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 - tau_i * int_CTE## where the integrated crosstrack error (int_CTE) is# the sum of all the previous crosstrack errors.# This term works to cancel out steering drift.## Only modify code at the bottom! Look for the TODO.# ------------import random
import numpy as np
import matplotlib.pyplot as plt
# ------------------------------------------------# # this is the Robot class#classRobot(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.0defset(self, x, y, orientation):"""
Sets a robot coordinate.
"""
self.x = x
self.y = y
self.orientation = orientation % (2.0 * np.pi)
defset_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
defset_steering_drift(self, drift):"""
Sets the systematical steering drift parameter
"""
self.steering_drift = drift
defmove(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# 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
robot = Robot()
robot.set(0, 1, 0)
defrun(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
# TODO: your code here
pre_cte = robot.y
sum_cte = 0for i in range(n):
cte = robot.y
steer = -tau_p*cte - tau_d*(cte-pre_cte) - tau_i*(sum_cte)
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
pre_cte = cte
sum_cte += cte
return x_trajectory, y_trajectory
x_trajectory, y_trajectory = run(robot, 0.2, 3.0, 0.004)
n = len(x_trajectory)
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8,8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='PID controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')
Twiddle
# ----------------# User Instructions## Implement twiddle as shown in the previous two videos.# Your accumulated error should be very small!## You don't have to use the exact values as shown in the video# play around with different values! This quiz isn't graded just see# how low of an error you can get.## Try to get your error below 1.0e-10 with as few iterations# as possible (too many iterations will cause a timeout).## No cheating!# ------------import random
import numpy as np
import matplotlib.pyplot as plt
# ------------------------------------------------# # this is the Robot class#classRobot(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.0defset(self, x, y, orientation):"""
Sets a robot coordinate.
"""
self.x = x
self.y = y
self.orientation = orientation % (2.0 * np.pi)
defset_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
defset_steering_drift(self, drift):"""
Sets the systematical steering drift parameter
"""
self.steering_drift = drift
defmove(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# 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 rundefmake_robot():"""
Resets the robot back to the initial position and drift.
You'll want to call this after you call `run`.
"""
robot = Robot()
robot.set(0.0, 1.0, 0.0)
robot.set_steering_drift(10.0 / 180.0 * np.pi)
return robot
# NOTE: We use params instead of tau_p, tau_d, tau_idefrun(robot, params, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
err = 0
prev_cte = robot.y
int_cte = 0for i in range(2 * n):
cte = robot.y
diff_cte = cte - prev_cte
int_cte += cte
prev_cte = cte
steer = -params[0] * cte - params[1] * diff_cte - params[2] * int_cte
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
if i >= n:
err += cte ** 2return x_trajectory, y_trajectory, err / n
# Make this tolerance bigger if you are timing out!deftwiddle(tol=0.2):# Don't forget to call `make_robot` before every call of `run`!
p = [0.0, 0.0, 0.0]
dp = [1.0, 1.0, 1.0]
robot = make_robot()
x_trajectory, y_trajectory, best_err = run(robot, p)
# TODO: twiddle loop herewhile(sum(dp)>tol):
for i in range(len(p)):
p[i] += dp[i]
robot = make_robot()
x_trajectory, y_trajectory, err = run(robot, p)
if err < best_err:
best_err = err
dp[i] *= 1.1continue
p[i] -= 2.0*dp[i]
robot = make_robot()
x_trajectory, y_trajectory, err = run(robot, p)
if err < best_err:
best_err = err
dp[i] *= 1.1continue
p[i] += dp[i]
dp[i] *= 0.9# # ------------- Reference code --------------------# n = 0# while(sum(dp)>tol):# for i in range(len(p)):# p[i] += dp[i]# robot = make_robot()# x_trajectory, y_trajectory, err = run(robot, p)# if err < best_err:# best_err = err# dp[i] *= 1.1# else:# p[i] -= 2.0*dp[i]# robot = make_robot()# x_trajectory, y_trajectory, err = run(robot, p)# if err < best_err:# best_err = err# dp[i] *= 1.1# else:# p[i] += dp[i]# dp[i] *= 0.9# n += 1# print 'Twiddle #', n, p, '->', best_errreturn p, best_err
params, err = twiddle()
print("Final twiddle error = {}".format(err))
robot = make_robot()
x_trajectory, y_trajectory, err = run(robot, params)
n = len(x_trajectory)
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='Twiddle PID controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')
Racetrack Control
# --------------# User Instructions# # Define a function cte in the robot class that will# compute the crosstrack error for a robot on a# racetrack with a shape as described in the video.## You will need to base your error calculation on# the robot's location on the track. Remember that # the robot will be traveling to the right on the# upper straight segment and to the left on the lower# straight segment.## --------------# Grading Notes## We will be testing your cte function directly by# calling it with different robot locations and making# sure that it returns the correct crosstrack error. from math import *
import random
# ------------------------------------------------# # this is the robot class#classrobot:# --------# init: # creates robot and initializes location/orientation to 0, 0, 0#def__init__(self, length = 20.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# --------# set: # sets a robot coordinate#defset(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------# set_noise: # sets the noise parameters#defset_noise(self, new_s_noise, new_d_noise):# makes it possible to change the noise parameters# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------# set_steering_drift: # sets the systematical steering drift parameter#defset_steering_drift(self, drift):
self.steering_drift = drift
# --------# move: # steering = front wheel steering angle, limited by max_steering_angle# distance = total distance driven, most be non-negativedefmove(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):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 = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
def__repr__(self):return'[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
############## ONLY ADD / MODIFY CODE BELOW THIS LINE ####################defcte(self, radius):# ## Add code here## if self.x <= radius:
cte = sqrt(pow(self.x-radius, 2) + pow(self.y-radius,2)) - radius
elif self.x <= 3*radius:
if self.orientation > pi/2.0and self.orientation < 3.0*pi/2:
cte = -self.y
else:
cte = self.y - 2*radius
else:
cte = sqrt(pow(self.x-3*radius, 2) + pow(self.y-radius,2)) - radius
return cte
############## ONLY ADD / MODIFY CODE ABOVE THIS LINE ##################### ------------------------------------------------------------------------## run - does a single control run.defrun(params, radius, printflag = False):
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0# motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200
crosstrack_error = myrobot.cte(radius) # You need to define the cte function!for i in range(N*2):
diff_crosstrack_error = - crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
- params[2] * int_crosstrack_error
myrobot = myrobot.move(steer, speed)
if i >= N:
err += crosstrack_error ** 2if printflag:
print myrobot
return err / float(N)
radius = 25.0
params = [10.0, 15.0, 0]
err = run(params, radius, True)
print'\nFinal parameters: ', params, '\n ->', err