# Artificial Intelligence for Robotics - PID Control

## Path Smooth

```# -----------
# 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 forum
def printpaths(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]]

def smooth(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 = 1000
while(st > tolerance):
newpath_pre = deepcopy(newpath)
st = 0
for 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))```

## 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
#

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

# 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
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 = []
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
#

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

# 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
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)

def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
pre_cte = robot.y
sum_cte = 0
for 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
#

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

# 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
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

def make_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_i
def run(robot, params, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
err = 0
prev_cte = robot.y
int_cte = 0
for 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 ** 2
return x_trajectory, y_trajectory, err / n

# Make this tolerance bigger if you are timing out!
def twiddle(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 here
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
continue

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
continue

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_err

return 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')```