课程 16：Problem Set 5
课程 16：Problem Set 5
1. 练习：Missing Parameters
这个练习向我们展示了缺失PID参数时的可能效果。
a. 优化的PID参数，理论上会让轨迹向期望的轨迹逐渐收敛
b. 如果缺失P参数，则CTE表现最大，并逐渐增大。
c. 如果缺失D参数，CTE表现没有b那样，但是也会逐渐增大。
d. 如果缺失I参数，CTE也会变小，轨迹在期望轨迹附近，呈一定的误差。
2. 练习：Cyclic Smoothing
在课程15中，做平滑的时候，起点和终点，都没有动。
这回的代码说明中只要求不动终点，起点可以动。结果演示代码答案时，循环到了所有的元素，，所以循环边界变了一下。
说明，如果是环线的平滑的时候，要包括起点和终点。
代码中还有个方法solution_check(smooth(testpath1),answer1)用来检查平滑方案的优劣，说让用这个方法测试answer1是不是正确，但是没有提到这个answer1是怎么来的。
代码：
# -*- coding: utf-8 -*-
# -------------
# User Instructions
#
# Here you will be implementing a cyclicsmoothing
# algorithm. This algorithm should not fixthe end
# points (as you did in the unit quizzes).You
# should use the gradient descentequations that
# you used previously.
#
# Your function should return the newpaththat it
# calculates.
#
# Feel free to use the providedsolution_check function
# to test your code. You can find it atthe bottom.
#
# --------------
# Testing Instructions
#
# To test your code, call thesolution_check function with
# two arguments. The first argument shouldbe the result of your
# smooth function. The second should bethe corresponding answer.
# For example, calling
#
# solution_check(smooth(testpath1),answer1)
#
# should return True if your answer iscorrect and False if
# it is not.
from math import *
# Do not modify path inside your function.
path=[[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]]
############# ONLY ENTER CODE BELOW THISLINE ##########
#------------------------------------------------
# smooth coordinates
# If your code is timing out, make thetolerance parameter
# larger to decrease run time.
#
def smooth(path, weight_data = 0.1,weight_smooth = 0.1, tolerance = 0.00001):
#
# Enter code here
#
# deepcopy path
newpath = []
for i in range(len(path)):
p1 = []
for j in range(len(path[0])):
p1.append(path[i][j])
newpath.append(p1)
changeFactor = tolerance
while changeFactor >= tolerance:
changeFactor = 0
for i in range(len(path)):
for j in range(len(path[0])):
aux = 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))%len(path)][j] - 2.0 *newpath[i][j])
changeFactor += abs(aux -newpath[i][j])
return newpath
# thank you - EnTerr - for posting this onour discussion forum
#newpath = smooth(path)
#for i in range(len(path)):
# print('['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ','.join('%.3f'%x for x in newpath[i]) +']')
##### TESTING ######
#--------------------------------------------------
# check if two numbers are 'closeenough,'used in
# solution_check function.
#
def close_enough(user_answer, true_answer,epsilon = 0.001):
if abs(user_answer - true_answer) > epsilon:
return False
return True
#--------------------------------------------------
# check your solution against ourreference solution for
# a variety of test cases (given below)
#
def solution_check(newpath, answer):
if type(newpath) != type(answer):
print("Error. You do not return a list.")
return False
if len(newpath) != len(answer):
print('Error. Your newpath is not the correct length.')
return False
if len(newpath[0]) != len(answer[0]):
print('Error. Your entries do not contain an (x, y) coordinate pair.')
return False
for i in range(len(newpath)):
for j in range(len(newpath[0])):
if not close_enough(newpath[i][j], answer[i][j]):
print('Error, at least one ofyour entries is not correct.')
return False
print("Test case correct!")
return True
# --------------
# Testing Instructions
#
# To test your code, call thesolution_check function with
# two arguments. The first argument shouldbe the result of your
# smooth function. The second should bethe corresponding answer.
# For example, calling
#
# solution_check(smooth(testpath1),answer1)
#
# should return True if your answer iscorrect and False if
# it is not.
testpath1 = [[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]]
answer1 = [[0.4705860385182691,0.4235279620576893],
[1.1764695730296597, 0.16470408411716733],
[2.058823799247812, 0.07058633859438503],
[3.000001503542886, 0.04705708651959327],
[3.9411790099468273, 0.07058689299792453],
[4.8235326678889345, 0.16470511854183797],
[5.529415336860586, 0.4235293374365447],
[5.76470933698621, 1.1058829941330384],
[5.764708805535902, 1.8941189433780983],
[5.5294138118186265, 2.5764724018811056],
[4.823530348360371, 2.835296251305122],
[3.941176199414957, 2.929413985845729],
[2.9999985709076413, 2.952943245204772],
[2.0588211310939526, 2.9294134622132018],
[1.1764675231284938, 2.8352952720424938],
[0.4705848811030855, 2.5764710948028178],
[0.23529088056307781, 1.8941174802285707],
[0.23529138316655338, 1.1058815684272394]]
testpath2 = [[1, 0], # Move in the shapeof a plus sign
[2, 0],
[2, 1],
[3, 1],
[3, 2],
[2, 2],
[2, 3],
[1, 3],
[1, 2],
[0, 2],
[0, 1],
[1, 1]]
answer2 = [[1.2222234770374059,0.4444422843711052],
[1.7777807251383388, 0.4444432993123497],
[2.111114925633848, 0.8888894279539462],
[2.5555592020540376, 1.2222246475393077],
[2.5555580686154244, 1.7777817817879298],
[2.111111849558437, 2.1111159707965514],
[1.7777765871460525, 2.55556033483712],
[1.2222194640861452, 2.5555593592828543],
[0.8888853322565222, 2.111113321684573],
[0.44444105139827167, 1.777778212019149],
[0.44444210978390364, 1.2222211690821811],
[0.8888882042812255, 0.8888870211766268]]
# solution_check(smooth(testpath1),answer1)
# solution_check(smooth(testpath2),answer2)
3. 练习：Constrained Smoothing
约束性/限制性 平滑
就是说平滑过程中，要求某些点固定不动。
这个视频下方的编程练习的说明又说视频里面的计算方式有点小问题。
还是说这个newpath[i][j] 在计算中间过程的时候不能被改变，只能用最初值计算，建议用一个公式就完成每次对于单点newpath[i][j]的调整计算。
对非固定点的单次计算公式，考虑到了前后点对 weight_smooth 部分的计算。
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])例子中对于固定的点，给了一个fix列表，值为1，则表明是固定点，不能改变位置。
例子代码如下：
# -*- coding: utf-8 -*-
"""
Created on Tue May 22 18:41:08 2018
@author: Administrator
"""
# -------------
# User Instructions
#
# Now you will be incorporating fixedpoints into
# your smoother.
#
# You will need to use the equations fromgradient
# descent AND the new equations presentedin the
# previous lecture to implement smoothingwith
# fixed points.
#
# Your function should return the newpaththat it
# calculates.
#
# Feel free to use the providedsolution_check function
# to test your code. You can find it atthe bottom.
#
######################## ENTER CODE BELOWHERE #########################
def smooth(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 = [[path[row][col] for col in range(len(path[0]))] for row inrange(len(path))]
changeFactor = tolerance
while changeFactor >= tolerance:
changeFactor = 0
for i in range(len(path)):
if fix[i] == 0:
for j in range(len(path[0])):
aux = newpath[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])
changeFactor += abs(aux -newpath[i][j])
return newpath
# --------------
# Testing Instructions
#
# To test your code, call thesolution_check function with the argument smooth:
# solution_check(smooth)
#
def solution_check(smooth, eps = 0.0001):
def test_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 = True
for 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 = False
continue
if len(newpath) != len(path):
print("Error: smooth did not return a list of the correct lengthfor the path:")
print(test_case_str(path,fix) + '\n')
correct = False
continue
good_pairs = True
for newpt,pt in zip(newpath,path):
if len(newpt) != len(pt):
good_pairs = False
break
if not good_pairs:
print("Error: smooth did not return a list of coordinate pairs forthe path:")
print(test_case_str(path,fix) + '\n')
correct = False
continue
assert( good_pairs )
# check whether to check against true or pseudo answers
answer = None
if 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')
continue
assert( answer is not None )
entries_match = True
for p,q in zip(newpath,answer):
for pi,qi in zip(p,q):
if abs(pi - qi) > eps:
entries_match = False
break
if not entries_match: break
if not entries_match:
print('smooth returned an incorrect answer for the path:')
print(test_case_str(path,fix) + '\n')
continue
assert( 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 shouldmodify your code so that newpath[i][j] is only
updated once per iteration, or else theintermediate updates made to newpath[i][j]
will affect the final answer.\n''')
solution_check(smooth)
4. 练习：Racetrack Control
赛马场控制，有个要注意的点是，视频下方提示了：
Make sure CTE is negative inside the track and positive outside the track.
我之前没看到，在计算跟x轴重合的那条路线时，计算得到的cte值的符号跟其他地方相反。导致运行结果 err 比较大，然后改正后，err值收敛，符合要求了。
但是为何有这个要求呢？？看了后面一节课的Help，我想这主要是因为，类比等高线图来说，环形线就是一个等高线，进入环内和远离环内，意味着沿着梯度的上升或者下降，所以得方向相反，而在环外的是同一个方向。跟坐标系无关。另外一方面，这样就使前进方向的同一侧的cte值是同一种符号。
代码：
# -*- coding: utf-8 -*-
# --------------
# User Instructions
#
# Define a function cte in the robot class thatwill
# compute the crosstrack error for a robot on a
# racetrack with a shape as described in thevideo.
#
# You will need to base your error calculationon
# the robot's location on the track. Rememberthat
# the robot will be traveling to the right onthe
# upper straight segment and to the left on thelower
# straight segment.
#
# --------------
# Grading Notes
#
# We will be testing your cte function directlyby
# calling it with different robot locations andmaking
# sure that it returns the correct crosstrackerror.
from math import *
import random
#------------------------------------------------
#
# this is the robot class
#
class robot:
#--------
#init:
# creates robot and initializeslocation/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
#
def set(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
#
def set_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 driftparameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift
#--------
#move:
# steering = front wheelsteering angle, limited by max_steering_angle
# distance = total distancedriven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
ifsteering > 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 BELOWTHIS LINE ####################
def cte(self, radius):
#
#
#Add code here
#
#
#find out the two circle center point.
CxLeft = radius
CyLeft = radius
CxRight = radius + 2 * radius
CyRight = radius
cte = float('Inf')
if self.x < radius:#robot.x >= 0
cte = sqrt((self.x - CxLeft)**2 + (self.y- CyLeft)**2) - radius
elif self.x >= CxLeft and self.x < CxRight:
cte1 = self.y - 2 * radius
cte2 = self.y
if abs(cte1) < abs(cte2):
cte = cte1
else:
# 因为编程前视频下方说明：Make sure CTE is negative inside the track andpositive outside the track.
#cte = cte2
cte = -cte2
else:# self.x >= CxRight and self.x < CxRight + radius
cte = sqrt((self.x - CxRight)**2 + (self.y - CyRight)**2) - radius
return cte
############## ONLY ADD / MODIFY CODE ABOVETHIS LINE ####################
#------------------------------------------------------------------------
#
# run - does a single control run.
def run(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 ctefunction!
for iin 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 ** 2
if 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)