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

	def __add__(self, other):
		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
move = convolution(=addition)
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