Browse files

cs373 finished

  • Loading branch information...
1 parent 57cb1a7 commit 326eaefbb0a50d398a20a014338faa26d50f13c2 @jaz303 committed Apr 10, 2012
View
183 udacity/cs373/final/planning.py
@@ -0,0 +1,183 @@
+# -------------------
+# Background Information
+#
+# In this problem, you will build a planner that makes a robot
+# car's lane decisions. On a highway, the left lane (in the US)
+# generally has a higher traffic speed than the right line.
+#
+# In this problem, a 2 lane highway of length 5 could be
+# represented as:
+#
+# road = [[80, 80, 80, 80, 80],
+# [60, 60, 60, 60, 60]]
+#
+# In this case, the left lane has an average speed of 80 km/h and
+# the right lane has a speed of 60 km/h. We can use a 0 to indicate
+# an obstacle in the road.
+#
+# To get to a location as quickly as possible, we usually
+# want to be in the left lane. But there is a cost associated
+# with changing lanes. This means that for short trips, it is
+# sometimes optimal to stay in the right lane.
+#
+# -------------------
+# User Instructions
+#
+# Design a planner (any kind you like, so long as it works).
+# This planner should be a function named plan() that takes
+# as input four parameters: road, lane_change_cost, init, and
+# goal. See parameter info below.
+#
+# Your function should RETURN the final cost to reach the
+# goal from the start point (which should match with our answer).
+# You may include print statements to show the optimum policy,
+# though this is not necessary for grading.
+#
+# Your solution must work for a variety of roads and lane
+# change costs.
+#
+# Add your code at line 92.
+#
+# --------------------
+# Parameter Info
+#
+# road - A grid of values. Each value represents the speed associated
+# with that cell. A value of 0 means the cell in non-navigable.
+# The cost for traveling in a cell must be (1.0 / speed).
+#
+# lane_change_cost - The cost associated with changing lanes.
+#
+# init - The starting point for your car. This will always be somewhere
+# in the right (bottom) lane to simulate a highway on-ramp.
+#
+# goal - The destination. This will always be in the right lane to
+# simulate a highway exit-ramp.
+#
+# --------------------
+# Testing
+#
+# You may use our test function below, solution_check
+# to test your code for a variety of input parameters.
+#
+# You may also use the build_road function to build
+# your own roads to test your function with.
+
+import random
+
+# ------------------------------------------
+# build_road - Makes a road according to your specified length and
+# lane_speeds. lane_speeds is a list of speeds for the lanes (listed
+# from left lane to right). You can also include random obstacles.
+#
+def build_road(length, lane_speeds, print_flag = False, obstacles = False, obstacle_prob = 0.05):
+ num_lanes = len(lane_speeds)
+ road = [[lane_speeds[i] for dist in range(length)] for i in range(len(lane_speeds))]
+ if obstacles:
+ for x in range(len(road)):
+ for y in range(len(road[0])):
+ if random.random() < obstacle_prob:
+ road[x][y] = 0
+ if print_flag:
+ for lane in road:
+ print '[' + ', '.join('%5.3f' % speed for speed in lane) + ']'
+ return road
+
+def plan_r(road, costs, lcc, cost, x, y):
+ if costs[x][y] == -1 or cost < costs[x][y]:
+ costs[x][y] = cost
+
+ for d in [[0,1],[-1,1],[1,1]]:
+ next_x = x + d[0]
+ next_y = y + d[1]
+
+ if next_x >= 0 and next_x < len(road) and next_y >= 0 and next_y < len(road[0]):
+ if road[next_x][next_y] > 0:
+ move_cost = 1.0 / road[x][y]
+ if next_x != x:
+ move_cost += lcc
+ plan_r(road, costs, lcc, cost + move_cost, next_x, next_y)
+
+# ------------------------------------------
+# plan - Returns cost to get from init to goal on road given a
+# lane_change_cost.
+#
+def plan(road, lane_change_cost, init, goal): # Don't change the name of this function!
+ costs = [[-1.0 for i in range(len(road[0]))] for j in range(len(road))]
+ plan_r(road, costs, lane_change_cost, 0.0, init[0], init[1])
+ print costs
+ return costs[goal[0]][goal[1]]
+
+################# TESTING ##################
+
+# ------------------------------------------
+# solution check - Checks your path function using
+# data from list called test[]. Uncomment the call
+# to solution_check at the bottom to test your code.
+#
+def solution_check(test, epsilon = 0.00001):
+ answer_list = []
+ for i in range(len(test[0])):
+ user_cost = plan(test[0][i], test[1][i], test[2][i], test[3][i])
+ true_cost = test[4][i]
+ if abs(user_cost - true_cost) < epsilon:
+ answer_list.append(1)
+ else:
+ answer_list.append(0)
+ correct_answers = 0
+ print
+ for i in range(len(answer_list)):
+ if answer_list[i] == 1:
+ print 'Test case', i+1, 'passed!'
+ correct_answers += 1
+ else:
+ print 'Test case', i+1, 'failed.'
+ if correct_answers == len(answer_list):
+ print "\nYou passed all test cases!"
+ return True
+ else:
+ print "\nYou passed", correct_answers, "of", len(answer_list), "test cases. Try to get them all!"
+ return False
+
+# Test Case 1 (FAST left lane)
+test_road1 = build_road(8, [100, 10, 1])
+lane_change_cost1 = 1.0 / 1000.0
+test_init1 = [len(test_road1) - 1, 0]
+test_goal1 = [len(test_road1) - 1, len(test_road1[0]) - 1]
+true_cost1 = 1.244
+
+# Test Case 2 (more realistic road)
+test_road2 = build_road(14, [80, 60, 40, 20])
+lane_change_cost2 = 1.0 / 100.0
+test_init2 = [len(test_road2) - 1, 0]
+test_goal2 = [len(test_road2) - 1, len(test_road2[0]) - 1]
+true_cost2 = 0.293333333333
+
+# Test Case 3 (Obstacles included)
+test_road3 = [[50, 50, 50, 50, 50, 40, 0, 40, 50, 50, 50, 50, 50, 50, 50], # left lane: 50 km/h
+ [40, 40, 40, 40, 40, 30, 20, 30, 40, 40, 40, 40, 40, 40, 40],
+ [30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]] # right lane: 30 km/h
+lane_change_cost3 = 1.0 / 500.0
+test_init3 = [len(test_road3) - 1, 0]
+test_goal3 = [len(test_road3) - 1, len(test_road3[0]) - 1]
+true_cost3 = 0.355333333333
+
+# Test Case 4 (Slalom)
+test_road4 = [[50, 50, 50, 50, 50, 40, 0, 40, 50, 50, 0, 50, 50, 50, 50], # left lane: 50 km/h
+ [40, 40, 40, 40, 0, 30, 20, 30, 0, 40, 40, 40, 40, 40, 40],
+ [30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]] # right lane: 30 km/h
+lane_change_cost4 = 1.0 / 65.0
+test_init4 = [len(test_road4) - 1, 0]
+test_goal4 = [len(test_road4) - 1, len(test_road4[0]) - 1]
+true_cost4 = 0.450641025641
+
+
+testing_suite = [[test_road1, test_road2, test_road3, test_road4],
+ [lane_change_cost1, lane_change_cost2, lane_change_cost3, lane_change_cost4],
+ [test_init1, test_init2, test_init3, test_init4],
+ [test_goal1, test_goal2, test_goal3, test_goal4],
+ [true_cost1, true_cost2, true_cost3, true_cost4]]
+
+solution_check(testing_suite) #UNCOMMENT THIS LINE TO TEST YOUR CODE
+
+
+
View
18 udacity/cs373/final/q14.rb
@@ -0,0 +1,18 @@
+def go(state, remain, bins)
+ if remain == 0
+ state[:count] += 1
+ state[:matching] += 1 if bins.all? { |b| !b.empty? }
+ else
+ 4.times do |i|
+ bins[i].push true
+ go(state, remain - 1, bins)
+ bins[i].pop
+ end
+ end
+end
+
+tally = {:count => 0, :matching => 0}
+
+go(tally, 12, [[], [], [], []])
+
+p tally
View
15 udacity/cs373/unit-2/coconuts.py
@@ -0,0 +1,15 @@
+def f(n):
+ return 4 * (n - 1) / 5.0
+
+def f6(n):
+ for i in range(0, 6):
+ n = f(n)
+ return n
+
+def is_int(n):
+ return abs(n - int(n)) < 0.0010001
+
+for i in range(0, 100000):
+ if is_int(f6(i)):
+ print i
+ break
View
6 udacity/cs373/unit-2/gaussian.py
@@ -0,0 +1,6 @@
+from math import *
+
+def f(mu, sigma2, x):
+ return 1/sqrt(2.0*pi*sigma2) * exp(-0.5 * (x - mu)**2 / sigma2)
+
+print f(10.0, 4.0, 10.0)
View
29 udacity/cs373/unit-2/kalman-1d.py
@@ -0,0 +1,29 @@
+def update(mean1, var1, mean2, var2):
+ new_mean = (var2*mean1 + var1*mean2) / (var1 + var2)
+ new_var = 1.0 / (1 / var1 + 1 / var2)
+ return [new_mean, new_var]
+
+def predict(mean1, var1, mean2, var2):
+ new_mean = mean1 + mean2
+ new_var = var1 + var2
+ return [new_mean, new_var]
+
+measurements = [5., 6., 7., 9., 10.]
+motion = [1., 1., 2., 1., 1.]
+measurement_sig = 4.
+motion_sig = 2.
+mu = 0
+sig = 0.0000000001
+
+for i in range(0, len(measurements)):
+ [mu, sig] = update(mu, sig, measurements[i], measurement_sig)
+ print "update: ", [mu, sig]
+ [mu, sig] = predict(mu, sig, motion[i], motion_sig)
+ print "predict: ", [mu, sig]
+
+#Please print out ONLY the final values of the mean
+#and the variance in a list [mu, sig].
+
+# Insert code here
+
+print [mu, sig]
View
1 udacity/cs373/unit-2/kalman-matrices.py
@@ -0,0 +1 @@
+a = matrix([])
View
6 udacity/cs373/unit-2/update-kalman.py
@@ -0,0 +1,6 @@
+def update(mean1, var1, mean2, var2):
+ new_mean = (var2*mean1 + var1*mean2) / (var1 + var2)
+ new_var = 1.0 / (1 / var1 + 1 / var2)
+ return [new_mean, new_var]
+
+print update(1.0, 1.0, 5.0, 4.0)
View
233 udacity/cs373/unit-5/cyclic-smoother.py
@@ -0,0 +1,233 @@
+# -------------
+# User Instructions
+#
+# Here you will be implementing a cyclic smoothing
+# algorithm. This algorithm should not fix the end
+# points (as you did in the unit quizzes). You
+# should use the gradient descent equations that
+# you used previously.
+#
+# 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.
+#
+# --------------
+# Testing Instructions
+#
+# To test your code, call the solution_check function with
+# two arguments. The first argument should be the result of your
+# smooth function. The second should be the corresponding answer.
+# For example, calling
+#
+# solution_check(smooth(testpath1), answer1)
+#
+# should return True if your answer is correct 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 THIS LINE ##########
+
+# ------------------------------------------------
+# smooth coordinates
+# If your code is timing out, make the tolerance parameter
+# larger to decrease run time.
+#
+
+def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001):
+
+ # Make a deep copy of path into newpath
+ newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))]
+ for i in range(len(path)):
+ for j in range(len(path[0])):
+ newpath[i][j] = path[i][j]
+
+ #### ENTER CODE BELOW THIS LINE ###
+ change = tolerance
+ while change >= tolerance:
+ change = 0.0
+ for i in range(len(path)):
+ for j in range(2):
+ aux = newpath[i][j]
+ newpath[i][j] += weight_data * (path[i][j] - newpath[i][j])
+ newpath[i][j] += weight_smooth * (newpath[(i+1)%len(path)][j] + newpath[(i-1)%len(path)][j] - (2 * newpath[i][j]))
+ change += abs(aux - newpath[i][j])
+
+ return newpath # Leave this line for the grader!
+
+# thank you - EnTerr - for posting this on our 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 'close enough,'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 our reference 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 of your entries is not correct.'
+ return False
+ print "Test case correct!"
+ return True
+
+# --------------
+# Testing Instructions
+#
+# To test your code, call the solution_check function with
+# two arguments. The first argument should be the result of your
+# smooth function. The second should be the corresponding answer.
+# For example, calling
+#
+# solution_check(smooth(testpath1), answer1)
+#
+# should return True if your answer is correct 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.5449300156668018, 0.47485226780102946],
+ [1.2230705677535505, 0.2046277687200752],
+ [2.079668890615267, 0.09810778721159963],
+ [3.0000020176660755, 0.07007646364781912],
+ [3.9203348821839112, 0.09810853832382399],
+ [4.7769324511170455, 0.20462917195702085],
+ [5.455071854686622, 0.4748541381544533],
+ [5.697264197153936, 1.1249625336275617],
+ [5.697263485026567, 1.8750401628534337],
+ [5.455069810373743, 2.5251482916876378],
+ [4.776929339068159, 2.795372759575895],
+ [3.92033110541304, 2.9018927284871063],
+ [2.999998066091118, 2.929924058932193],
+ [2.0796652780381826, 2.90189200881968],
+ [1.2230677654766597, 2.7953714133566603],
+ [0.544928391271399, 2.5251464933327794],
+ [0.3027360471605494, 1.875038145804603],
+ [0.302736726373967, 1.1249605602741133]]
+
+## [0.000, 0.000] -> [0.545, 0.475]
+## [1.000, 0.000] -> [1.223, 0.205]
+## [2.000, 0.000] -> [2.080, 0.098]
+## [3.000, 0.000] -> [3.000, 0.070]
+## [4.000, 0.000] -> [3.920, 0.098]
+## [5.000, 0.000] -> [4.777, 0.205]
+## [6.000, 0.000] -> [5.455, 0.475]
+## [6.000, 1.000] -> [5.697, 1.125]
+## [6.000, 2.000] -> [5.697, 1.875]
+## [6.000, 3.000] -> [5.455, 2.525]
+## [5.000, 3.000] -> [4.777, 2.795]
+## [4.000, 3.000] -> [3.920, 2.902]
+## [3.000, 3.000] -> [3.000, 2.930]
+## [2.000, 3.000] -> [2.080, 2.902]
+## [1.000, 3.000] -> [1.223, 2.795]
+## [0.000, 3.000] -> [0.545, 2.525]
+## [0.000, 2.000] -> [0.303, 1.875]
+## [0.000, 1.000] -> [0.303, 1.125]
+
+
+testpath2 = [[1, 0], # Move in the shape of 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.239080543767428, 0.5047204351187283],
+ [1.7609243903912781, 0.5047216452560908],
+ [2.0915039821562416, 0.9085017167753027],
+ [2.495281862032503, 1.2390825203587184],
+ [2.4952805300504783, 1.7609262468826048],
+ [2.0915003641706296, 2.0915058211575475],
+ [1.7609195135622062, 2.4952837841027695],
+ [1.2390757942466555, 2.4952826072236918],
+ [0.9084962737918979, 2.091502621431358],
+ [0.5047183914625598, 1.7609219230352355],
+ [0.504719649257698, 1.2390782835562297],
+ [0.9084996902674257, 0.9084987462432871]]
+
+## [1.000, 0.000] -> [1.239, 0.505]
+## [2.000, 0.000] -> [1.761, 0.505]
+## [2.000, 1.000] -> [2.092, 0.909]
+## [3.000, 1.000] -> [2.495, 1.239]
+## [3.000, 2.000] -> [2.495, 1.761]
+## [2.000, 2.000] -> [2.092, 2.092]
+## [2.000, 3.000] -> [1.761, 2.495]
+## [1.000, 3.000] -> [1.239, 2.495]
+## [1.000, 2.000] -> [0.908, 2.092]
+## [0.000, 2.000] -> [0.505, 1.761]
+## [0.000, 1.000] -> [0.505, 1.239]
+## [1.000, 1.000] -> [0.908, 0.908]
+
+solution_check(smooth(testpath2), answer2)
+
+
+
View
154 udacity/cs373/unit-5/p-controller.py
@@ -0,0 +1,154 @@
+# -----------
+# 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 * crosstrack_error
+#
+# Note that tau is called "param" in the function
+# run to be completed below.
+#
+# Your code should print output that looks like
+# the output shown in the video. That is, at each step:
+# print myrobot, steering
+#
+# Only modify code at the bottom!
+# ------------
+
+from math import *
+import random
+
+
+# ------------------------------------------------
+#
+# this is the robot class
+#
+
+class robot:
+
+ # --------
+ # 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
+ #
+
+ 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 drift parameter
+ #
+
+ def set_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-negative
+
+ def move(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)
+
+############## ADD / MODIFY CODE BELOW ####################
+
+# ------------------------------------------------------------------------
+#
+# run - does a single control run
+
+
+def run(param):
+ myrobot = robot()
+ myrobot.set(0.0, 1.0, 0.0)
+ speed = 1.0 # motion distance is equal to speed (we assume time = 1)
+ N = 100
+
+ for i in range(100):
+ myrobot = myrobot.move(param * -myrobot.y, speed)
+ print myrobot
+
+ #
+ # Add Code Here
+ #
+
+run(0.1) # call function with parameter tau of 0.1 and print results
View
67 udacity/cs373/unit-5/path-smoothing.py
@@ -0,0 +1,67 @@
+# -----------
+# User Instructions
+#
+# Define a function smooth that takes a path as its input
+# (with optional parameters for weight_data, weight_smooth)
+# and returns a smooth path.
+#
+# 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 previous video:
+#
+# If your function isn't submitting it is possible that the
+# runtime is too long. Try sacrificing accuracy for speed.
+# -----------
+
+
+from math import *
+
+# 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]]
+
+# ------------------------------------------------
+# smooth coordinates
+#
+
+def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001):
+
+ # Make a deep copy of path into newpath
+ newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))]
+ for i in range(len(path)):
+ for j in range(len(path[0])):
+ newpath[i][j] = path[i][j]
+
+
+ #### ENTER CODE BELOW THIS LINE ###
+ change = tolerance
+ while change >= tolerance:
+ change = 0.0
+ for i in range(1, len(path)-1):
+ for j in range(2):
+ aux = newpath[i][j]
+ newpath[i][j] += weight_data * (path[i][j] - newpath[i][j])
+ newpath[i][j] += weight_smooth * (newpath[i+1][j] + newpath[i-1][j] - (2 * newpath[i][j]))
+ change += abs(aux - newpath[i][j])
+
+ return newpath # Leave this line for the grader!
+
+# feel free to leave this and the following lines if you want to print.
+newpath = smooth(path, 0, 0.9)
+
+# thank you - EnTerr - for posting this on our discussion forum
+for i in range(len(path)):
+ print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']'
+
+
+
+
+
View
161 udacity/cs373/unit-5/pd-controller.py
@@ -0,0 +1,161 @@
+# -----------
+# 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!
+# ------------
+
+from math import *
+import random
+
+
+# ------------------------------------------------
+#
+# this is the robot class
+#
+
+class robot:
+
+ # --------
+ # 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
+ #
+
+ 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 drift parameter
+ #
+
+ def set_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-negative
+
+ def move(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)
+
+############## ADD / MODIFY CODE BELOW ####################
+
+# ------------------------------------------------------------------------
+#
+# run - does a single control run.
+
+
+def run(param1, param2):
+ myrobot = robot()
+ myrobot.set(0.0, 1.0, 0.0)
+ speed = 1.0 # motion distance is equal to speed (we assume time = 1)
+ N = 1000
+ myrobot.set_steering_drift(10.0 / 180.0 * pi)
+
+ crosstrack_error = myrobot.y
+ for i in range(N):
+ diff_crosstrack_error = myrobot.y - crosstrack_error
+ crosstrack_error = myrobot.y
+ steering = - (param1 * crosstrack_error) - (param2 * diff_crosstrack_error)
+ myrobot = myrobot.move(steering, speed)
+ print [myrobot, steering]
+
+ #
+ # Enter code here
+ #
+
+# Call your function with parameters of 0.2 and 3.0 and print results
+run(0.2, 3.0)
+
+
+
View
170 udacity/cs373/unit-5/pid-controller.py
@@ -0,0 +1,170 @@
+# -----------
+# 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.
+#
+# Your code should print a list that looks just like
+# the list shown in the video.
+#
+# Only modify code at the bottom!
+# ------------
+
+from math import *
+import random
+
+
+# ------------------------------------------------
+#
+# this is the robot class
+#
+
+class robot:
+
+ # --------
+ # 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
+ #
+
+ 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 drift parameter
+ #
+
+ def set_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-negative
+
+ def move(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)
+
+
+
+
+############## ADD / MODIFY CODE BELOW ####################
+
+# ------------------------------------------------------------------------
+#
+# run - does a single control run.
+
+
+def run(param1, param2, param3):
+ myrobot = robot()
+ myrobot.set(0.0, 1.0, 0.0)
+ speed = 1.0 # motion distance is equal to speed (we assume time = 1)
+ myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias, this will be added in by the move function, you do not need to add it below!
+
+ prev_crosstrack_error = myrobot.y
+ total_crosstrack_error = 0
+
+ for i in range(1000):
+ crosstrack_error = myrobot.y
+ d_crosstrack_error = crosstrack_error - prev_crosstrack_error
+ total_crosstrack_error += crosstrack_error
+ steering = -param1 * crosstrack_error - param2 * d_crosstrack_error - param3 * total_crosstrack_error
+ myrobot = myrobot.move(steering, speed)
+ prev_crosstrack_error = crosstrack_error
+ print myrobot, steering
+
+
+# Call your function with parameters of (0.2, 3.0, and 0.004)
+run(0.2, 3.0, 0.004)
+
+
+
+
View
196 udacity/cs373/unit-5/twiddle.py
@@ -0,0 +1,196 @@
+# ----------------
+# User Instructions
+#
+# Implement twiddle as shown in the previous two videos.
+# Your accumulated error should be very small!
+#
+# Your twiddle function should RETURN the accumulated
+# error. Try adjusting the parameters p and dp to make
+# this error as small as possible.
+#
+# Try to get your error below 1.0e-10 with as few iterations
+# as possible (too many iterations will cause a timeout).
+# No cheating!
+# ------------
+
+from math import *
+import random
+
+
+# ------------------------------------------------
+#
+# this is the robot class
+#
+
+class robot:
+
+ # --------
+ # 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
+ #
+
+ 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 drift parameter
+ #
+
+ def set_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-negative
+
+ def move(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)
+
+
+# ------------------------------------------------------------------------
+#
+# run - does a single control run.
+
+
+def run(params, printflag = False):
+ myrobot = robot()
+ myrobot.set(0.0, 1.0, 0.0)
+ speed = 1.0
+ err = 0.0
+ int_crosstrack_error = 0.0
+ N = 100
+ # myrobot.set_noise(0.1, 0.0)
+ myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree steering error
+
+
+ crosstrack_error = myrobot.y
+
+
+ for i in range(N * 2):
+
+ diff_crosstrack_error = myrobot.y - crosstrack_error
+ crosstrack_error = myrobot.y
+ int_crosstrack_error += crosstrack_error
+
+ steer = - params[0] * crosstrack_error \
+ - params[1] * diff_crosstrack_error \
+ - int_crosstrack_error * params[2]
+ myrobot = myrobot.move(steer, speed)
+ if i >= N:
+ err += (crosstrack_error ** 2)
+ if printflag:
+ print myrobot, steer
+ return err / float(N)
+
+
+def twiddle(tol = 0.2): #Make this tolerance bigger if you are timing out!
+############## ADD CODE BELOW ####################
+
+ params = [0.0, 0.0, 0.0]
+ deltas = [1.0, 1.0, 1.0]
+
+ best_err = run(params)
+ while sum(deltas) > tol:
+ for i in range(len(params)):
+ params[i] += deltas[i]
+ err = run(params)
+ if err < best_err:
+ best_err = err
+ deltas[i] *= 1.1
+ else:
+ params[i] -= 2 * deltas[i]
+ err = run(params)
+ if err < best_err:
+ best_err = err
+ deltas[i] *= 1.1
+ else:
+ params[i] += deltas[i]
+ deltas[i] *= 0.9
+
+
+ return run(params)
+
+print twiddle()

0 comments on commit 326eaef

Please sign in to comment.