Permalink
Browse files

twiddle

  • Loading branch information...
huanghuang
huanghuang committed Mar 25, 2012
1 parent 8e741ef commit ce2ec7aeb9f4dd03016489d67e73b031224bc868
Showing with 378 additions and 2 deletions.
  1. +2 −2 5-11.py
  2. +171 −0 5-14.py
  3. +205 −0 twiddle.py
View
@@ -138,7 +138,7 @@ def __repr__(self):
def run(param1, param2):
myrobot = robot()
myrobot.set(0.0, 1.0, 0.0)
- #myrobot.set_steering_drift(10.0 / 180.8 * pi)
+ myrobot.set_steering_drift(10.0 / 180.8 * pi)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
N = 100
cte = myrobot.y
@@ -154,7 +154,7 @@ def run(param1, param2):
#
# Call your function with parameters of 0.2 and 3.0 and print results
-run(0.2, 3.0)
+run(0.2, 4.0)
View
171 5-14.py
@@ -0,0 +1,171 @@
+# -----------
+# 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)
+ N = 100
+ 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!
+ cte = myrobot.y
+ I = 0
+ for i in range(N):
+ cte_now = myrobot.y
+ p = cte
+ d = cte_now - cte
+ I += cte
+ steer = -param1 * p - param2 * d - param3 * I
+ myrobot = myrobot.move(steer, speed)
+ cte = cte_now
+ print myrobot, steer
+
+
+# Call your function with parameters of (0.2, 3.0, and 0.004)
+run(0.2, 3.0, 0.004)
+
+
+
+
+
View
@@ -0,0 +1,205 @@
+# ----------------
+# 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.0000000001): #Make this tolerance bigger if you are timing out!
+############## ADD CODE BELOW ####################
+
+
+
+ # -------------
+ # Add code here
+ # -------------
+
+ params = [0,0,0]
+ dp = [1,1,1]
+ best_error = run(params)
+ while sum(dp) > tol:
+ for i in range(len(params)):
+ params[i] += dp[i]
+ error = run(params)
+ if error < best_error:
+ best_error = error
+ dp[i] *= 1.1
+ else:
+ params[i] -= 2*dp[i]
+ error = run(params)
+ if error < best_error:
+ best_error = error
+ dp[i] *= 1.1
+ else:
+ params[i] += dp[i]
+ dp[i] *= 0.9
+ print params
+ print dp
+ return run(params)
+
+
+twiddle()
+
+
+

0 comments on commit ce2ec7a

Please sign in to comment.