Motion Planning

Motion Planning
Given: Map, starting location, goal location, cost
Goal: Find minimum cost

Each Actions: move, turn

```grid = [[0, 0, 1, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 1, 1, 0],
[0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0],
[0, -1],
[1, 0],
[0, 1]]

delta_name = ['^','<','v','>']

def search(grid, init, goal,cost):
closed = [[0 for row in range(len(grid[0]))] fro col in range(len(grid))]

x = init[0]
y = init[1]
g = 0

open = [[g, x, y]]
return path
```

Circular motion

```from math import *
import random

landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0],[100.0, 100.0]]
world_size = 100.0
max_steering_angle = pi/4

class robot:

def __init__(self, length = 100.0):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.length = length
self.bearing_noise = 0.0
self.steering_noise = 0.0
self.distance_noise = 0.0

def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))

def set(self, new_x, new_y, new_orientation):

if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)

def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)

def move(self, motion, tolerance = 0.001):

steering = motion[0]
distance = motion[1]

if abs(steering) > max_steering_angle:
raise ValueError, 'Exceeding max steering angle'

if distance < 0.0:
raise ValueError, 'Moving backwards is not valid'
return result
```

Resampling

p1, w1 = 0.6
p2, w2 = 1.2
p3, w3 = 2.4
p4, w4 = 0.6
p5, w5 = 1.2
P(p1)

```p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
```
```myrobot = robot()

p = []
for i in range(N):
r = robot()
r.set_noise(0.05, 0.05, 5.0)
p.append(r)

for t in range(T):
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
```
```length = 20.
steering_noise = 0.0
distance_noise = 0.0

myrobot = robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)

motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]

T = len(motions)
print 'Robot: '.myrobot
```

Particle Filter

– easiest to prgram
– most flexible

continuous, multi-modal, approximate

```from math import *
import random

landmarks = [[20.0, 20.0],
[80.0, 80.0],
[20.0, 80.0],
[80.0, 20.0]]
world_size = 100.0

class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noize = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;

def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new x)

def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);

def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z

def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'

orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi

dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size
y %= world_size

res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res

def Gaussian(self, mu, sigma, x):
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))

def measurement_prob(self, measurement):

prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0])**2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob

def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))

def eval(r, p):
sum = 0.0;
for i in range(len(p)):
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
myrobot = robot()
myrobot.set(10.0, 10.0, 0.0)
print myrobot
myrobot - myrobot.move(0.0, 10.0)
print myrobot
myrobot = myrobot.move(pi/2, 10.0)
print myrobot
print myrobot.sense()
```
```N = 1000
p = []
for i in range(N):
x = robot()
p.append(x)
print len(p)
```

Kalman Matrices

```from math import *

class matrix:

def __init__(self, value):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0

def zero(self, dimx, dimy):
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0 for row in range(dimy)] for col in range(dimx)]

def identity(self, dim):
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1

def show(self):
for i in range(self.dimx):
print self.value[i]
print ''

if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimensions to add"
else:
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res

def __sub__(self, other):
if self.dimx != otehr.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimensions to subtract"
else:
res	= matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res

def __mul__(self, other):
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
res = matrix([[]])
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res

def transpose(self):
res = matrix([[]])
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res

def Cholesky(self, ztol=1.0e-5):
res = matrix([[]])
res.zero(self.dimx, self.dimx)

for i in range(self.dimx):
S = sum([(res.value[i][i])** 2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S  sum([res.value[k][i] * res.value[k][j] for k in rande(self.dimx)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res

def CholeskyInverse(self):
res = matrix([[]])
res.zero(self.dimx, self.dimx)

for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/tjj**2 - S/tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
return res

def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res

def __repr__(self):
return repr(self.value)

def kalman_filter(x, P):
for n in range(len(measurements)):
return x, P
```

Kalman Filters

KALMAN FILTER : continuous uni-modal
MONTE CARLO Localization : discrete

f(x) = 1/√2πΩ2 exp -1/2(x-μ2)/Θ2

Gaussians smallest is preferred.

maximize gaussian

```from math import *
def f(mu, sigma2, x):
return 1/sqrt(2.*pi*sigma2) * exp(-.5 * (x-mu)**2 / sigma2)

print f(10., 4., 8.)
```

new mean

```def update(mean1, var1, mean2, var2):
new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2)
new_var = 1 / (1/var1 + 1 / var2)
return [new_mean, new_var]

print update(10.,8.,13.,2.)
```

prediction

```def update(mean1, var1, mean2, var2):
new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2)
new_var = 1 / (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]

print update(10.,8.,13.,2.)
print predict(10., 4., 12., 4.)
```
```def update(mean1, var1, mean2, var2):
new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2)
new_var = 1 / (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 = 10000.

for n in range(len(measurements)):
[mu, sig] = update(mu, sig, measurements[n], measurement_sig)
print 'update: ', [mu, sig]
[mu, sig] = predict(mu, sig, motion[n], motion_sig)
print 'predict: ', [mu, sig]

print update(10.,8.,13.,2.)
print predict(10., 4., 12., 4.)
```

Limit Distribution

0.8p(x2)+ 0.1p(x1)+ 0.1p(x3) = p(x4)

Gain information, loses information
Entropy -Σp(xi)log p(x)

BELiEF = PROBABility
sense = product followed by normalization
formal definition
0 <= p(x) <= 1 p(x1) = 0.2 p(x2) = 0.8 bayes rule x = grid cell, z = measurement p(x|z) = p(z|x)p(x)/ p(z) motion - total probability p(xit) = Σj P(xg t-1)・p(xi|xj) p(x)=0.2 p(¬x)=0.8 p(x)=0.2 p(y)=0.2 p(x,y) = 0.04 p(x)=0.2 p(y|x)=0.6 p(y|¬x)=0.6 p(y)=0.6 [python] colors = [['green', 'green', 'green'], ['green', 'red', 'green'], ['green', 'green', 'green']] measurements = ['red', 'red'] motions = [[0, 0], [0, 1]] sensor_right = 1.0 p_move = 1.0 [/python]

```def localize(colors, measurements, motions, sensor_right, p_move):
print = 1.0 / float(len(colors)) / float(len(colors[0]))
p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]

return p

sensor_wrong = 1.0 – sensor_right
p_stay = 1.0 – p_move

def show(p):
rows = [‘[‘ + ‘,’.join(map(lambda x: ‘{0:.5f’.format(x), r))+’]’ for r in p]
print ‘[‘ + ‘,\n ‘.join(rows) + ‘]’

colors = [[‘R’,’G’,’G’,’R’,’R’],
[‘R’,’R’,’G’,’R’,’R’],
[‘R’,’R’,’G’,’G’,’R’],
[‘R’,’R’,’R’,’R’,’R’]]
measurements = [‘G’,’G’,’G’,’G’,’G’]
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)
show(p)
```

Sense Function

```p=[0.2,0.2,0.2,0.2]
world=['green','red','red','green','green']
pHit = 0.6
pMiss = 0.2

def sense(p, Z)
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
return q

print sense(p, Z)
```
```p=[0.2,0.2,0.2,0.2]
world=['green','red','red','green','green']
pHit = 0.6
pMiss = 0.2

def sense(p, Z)
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
sum i in range(len(p)):
q[i] = q[i] / s
return q

print sense(p, Z)
```
```p=[0.2,0.2,0.2,0.2]
world=['green','red','red','green','green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

def sense(p, Z)
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
sum i in range(len(p)):
q[i] = q[i] / s
return q

for k in range(len(measurements)):
p = sense(p, measurements[k])

print sense(p, Z)
```
```def move(p, U):
q = []
for i in range(len(p)):
s = pExact * p[(i-U) % len(p)]
s = s + pOvershoot * p[(i-U-1)%len(p)]
s = s + pUndershoot * p[(i-U+1)%len(p)]
q.append(s)
return q

```

Artificial Intelligence for Robotics

Localization
Kalman Filters
Particle Filters
Search
PID Control
SLAM(simultaneous localization and mapping)

Localization
satellite pass signals with GPS(global position system)
localization has a lot of Math

easy solution of 5 vectors

```p=[0.2, 0.2, 0.2, 0.2, 0.2]
print p
```
```p=[]
n = 5

for i in range(n):
p.append(1./n)
print p
```

Segment

code here

```def splits(characters, longest=12):
"All ways to split characters into a first word and remainder."
return [(characters[:i], characters[i:])
for i in range(1, 1+min(longest, len(characters)))]

def Pwords(words):
"probability of a sequence of words."
return product(words, key=Pw)

@memo
def segment(text):
"Best segmentation of text into words, by probability."
if text == "": return []
candidates = [[first]+segment(rest) for first,rest in splits(text)]
return max(candidates, key=Pwords)
```

spelling correction
c* = argmaxc P(c|w)
= argmaxc P(w|c) p(c)