robocup particle filter

gistfile1.py

from Tkinter import *
from math import *
import random

GAME_FIELD_PADDING = 0.7
GAME_FIELD_WIDTH = 6.0
GAME_FIELD_HEIGHT = 4.0
PENALTY_AREA_WIDTH = 0.6
PENALTY_AREA_HEIGHT = 2.2
PENALTY_MARK_SIZE = 0.1
PENALTY_MARK_DISTANCE = 1.8
GOAL_HEIGHT = 1.4
GOAL_POST_HEIGHT = 0.8
GOAL_POST_WIDTH = 0.1
CENTER_CIRCLE_RADIUS = 0.6

LINE_WIDTH = 0.05

GOAL_POST_OPPONENT_TOP_COORD = (GAME_FIELD_WIDTH/2, GOAL_HEIGHT/2)
GOAL_POST_OPPONENT_BOTTOM_COORD = (GAME_FIELD_WIDTH/2, -GOAL_HEIGHT/2)
GOAL_POST_OWN_TOP_COORD = (-GAME_FIELD_WIDTH/2, GOAL_HEIGHT/2)
GOAL_POST_OWN_BOTTOM_COORD = (-GAME_FIELD_WIDTH/2, -GOAL_HEIGHT/2)
CENTER_CIRCLE_COORD = (0.0, 0.0)

CAMERA_H_VIEWING_ANGLE = 0.96
CAMERA_V_VIEWING_ANGLE = 0.74

METER_PIXEL_RATIO = 100.0
FIELD_COLOR = "darkgreen"
LINE_COLOR = "white"
GOAL_COLOR = "yellow"
GOAL_WIDTH = 0.2
PARTICLE_ORIENTATION_LINE_LENGTH = 0.15
PARTICLE_ORIENTATION_LINE_WIDTH = 0.01
PARTICLE_RADIUS = 0.05
PARTICLE_ORIENTATION_COLOR = "red"
PARTICLE_COLOR = "black"
PARTICLE_VIEWING_ANGLE_LINE_LENGTH = 4
PARTICLE_VIEWING_ANGLE_LINE_WIDTH = 0.03

BEARING_NOISE = 0.1
MOVE_ANGLE_NOISE = 0.1
MOVE_DISTANCE_NOISE = 0.1

landmarks = [GOAL_POST_OPPONENT_TOP_COORD, GOAL_POST_OPPONENT_BOTTOM_COORD, GOAL_POST_OWN_TOP_COORD, GOAL_POST_OWN_BOTTOM_COORD]

class Particle:
    def __init__(self):
        self.x = (random.random() - 0.5) * GAME_FIELD_WIDTH
        self.y = (random.random() - 0.5) * GAME_FIELD_HEIGHT
        self.orientation = random.random() * 2.0 * pi
        
    def set(self, new_x, new_y, new_orientation):
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)
        
    def sense(self, noise_enabled=1): #do not change the name of this function
        Z = []
        
        for i in range(len(landmarks)):
            bearing = atan2(landmarks[i][1] - self.y, landmarks[i][0] - self.x) - self.orientation
            if noise_enabled == 1:
                if abs((abs(bearing) + pi) % (2.0 * pi) - pi) > CAMERA_H_VIEWING_ANGLE/2.0:
                    continue
                bearing += random.gauss(0.0, BEARING_NOISE)
            bearing %= 2.0 * pi
            Z.append(bearing)

        return Z
    
    def move(self, motion, noise_enabled=1):
        alpha = float(motion[0])
        dist = float(motion[1])
        if noise_enabled:
            alpha += random.gauss(0.0, MOVE_ANGLE_NOISE)
            dist += random.gauss(0.0, MOVE_DISTANCE_NOISE)
        x = self.x + dist * cos(self.orientation)
        y = self.y + dist * sin(self.orientation)
        orientation = (self.orientation + alpha) % (2.0*pi)
        result = Particle()
        result.set(x, y, orientation)
        return result
    
    def measurement_prob(self, measurements):
        predicted_measurements = self.sense(0)
        random.shuffle(predicted_measurements)
        error = 1.0
        for i in range(len(measurements)):
            best_match_index = -1;
            best_match_error_bearing = 0
            for j in range(len(predicted_measurements)):
                error_bearing = abs(measurements[i] - predicted_measurements[j])
                error_bearing = (error_bearing + pi) % (2.0 * pi) - pi
                
                if best_match_index == -1:
                    best_match_index = j
                    best_match_error_bearing = error_bearing
                elif abs(error_bearing) < abs(best_match_error_bearing):
                    best_match_index = j
                    best_match_error_bearing = error_bearing
                    
            if best_match_index != -1:
                predicted_measurements.pop(best_match_index)
                error *= (exp(- (best_match_error_bearing ** 2) / (BEARING_NOISE ** 2) / 2.0) / sqrt(2.0 * pi * (BEARING_NOISE ** 2)))
        return error
    
class ParticleFilterLocalization:
    def __init__(self, N=500):
        self.particles = []
        for i in range(N):
            self.particles.append(Particle())
    
    def localize(self, motion, measurements): # I know it's tempting, but don't change N!
        p = self.particles
        
        # motion update (prediction)
        p2 = []
        for i in range(len(p)):
            p2.append(p[i].move(motion))
        p = p2

        # measurement update
        w = []
        for i in range(len(p)):
            w.append(p[i].measurement_prob(measurements))

        # resampling
        p3 = []
        index = int(random.random() * len(p))
        beta = 0.0
        mw = max(w)
        for i in range(len(p)):
            beta += random.random() * 2.0 * mw
            while beta > w[index]:
                beta -= w[index]
                index = (index + 1) % len(p)
            p3.append(p[index])
        p = p3
        
        self.particles = p

class FieldCanvas(Canvas):
    def __init__(self, master):
        Canvas.__init__(self, master,
                        bg=FIELD_COLOR,
                        width=((GAME_FIELD_WIDTH + 2*GAME_FIELD_PADDING) * METER_PIXEL_RATIO),
                        height=((GAME_FIELD_HEIGHT + 2*GAME_FIELD_PADDING) * METER_PIXEL_RATIO))
        
        self.draw_field()
        
        self.draw_line((1, 1), (1 + 0.15*cos(pi/4.0), 1 + 0.15 * sin(pi/4.0)), 0.01, "red")
        self.draw_circle((1, 1), 0.05, 0.01, "black")
        
    def convertCoordinate(self, x, y):
        return ((x + GAME_FIELD_WIDTH/2.0 + GAME_FIELD_PADDING) * METER_PIXEL_RATIO,
                (GAME_FIELD_HEIGHT/2.0 + GAME_FIELD_PADDING - y) * METER_PIXEL_RATIO)
        
    def draw_line(self, (x1, y1), (x2, y2), width, color):
        FieldCanvas.create_line(self, self.convertCoordinate(x1, y1), self.convertCoordinate(x2, y2), fill=color, width=width*METER_PIXEL_RATIO)
        
    def draw_circle(self, (x, y), r, width, color, fillColor=None):
        FieldCanvas.create_oval(self, self.convertCoordinate(x - r, y - r), self.convertCoordinate(x + r, y + r), outline=color, width=width*METER_PIXEL_RATIO, fill=fillColor)
        
    def draw_field(self):
        self.delete("all")
        # borders
        self.draw_line((-GAME_FIELD_WIDTH/2,   GAME_FIELD_HEIGHT/2), ( GAME_FIELD_WIDTH/2,   GAME_FIELD_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line((-GAME_FIELD_WIDTH/2,  -GAME_FIELD_HEIGHT/2), ( GAME_FIELD_WIDTH/2,  -GAME_FIELD_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line((-GAME_FIELD_WIDTH/2,   GAME_FIELD_HEIGHT/2), (-GAME_FIELD_WIDTH/2,  -GAME_FIELD_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2,   GAME_FIELD_HEIGHT/2), ( GAME_FIELD_WIDTH/2,  -GAME_FIELD_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        # center line and mark
        self.draw_line((                  0,   GAME_FIELD_HEIGHT/2), (                  0,  -GAME_FIELD_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line((        -LINE_WIDTH,                     0), (         LINE_WIDTH,                     0), LINE_WIDTH, LINE_COLOR)
        # penalty areas
        self.draw_line((-GAME_FIELD_WIDTH/2,  PENALTY_AREA_HEIGHT/2), (-GAME_FIELD_WIDTH/2 + PENALTY_AREA_WIDTH,  PENALTY_AREA_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line((-GAME_FIELD_WIDTH/2, -PENALTY_AREA_HEIGHT/2), (-GAME_FIELD_WIDTH/2 + PENALTY_AREA_WIDTH, -PENALTY_AREA_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2,  PENALTY_AREA_HEIGHT/2), ( GAME_FIELD_WIDTH/2 - PENALTY_AREA_WIDTH,  PENALTY_AREA_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2, -PENALTY_AREA_HEIGHT/2), ( GAME_FIELD_WIDTH/2 - PENALTY_AREA_WIDTH, -PENALTY_AREA_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line((-GAME_FIELD_WIDTH/2 + PENALTY_AREA_WIDTH,  PENALTY_AREA_HEIGHT/2), (-GAME_FIELD_WIDTH/2 + PENALTY_AREA_WIDTH,  -PENALTY_AREA_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2 - PENALTY_AREA_WIDTH,  PENALTY_AREA_HEIGHT/2), ( GAME_FIELD_WIDTH/2 - PENALTY_AREA_WIDTH,  -PENALTY_AREA_HEIGHT/2), LINE_WIDTH, LINE_COLOR)
        # goal posts
        self.draw_line((-GAME_FIELD_WIDTH/2 - GOAL_WIDTH,  GOAL_HEIGHT/2), (-GAME_FIELD_WIDTH/2,  GOAL_HEIGHT/2), GOAL_POST_WIDTH, GOAL_COLOR)
        self.draw_line((-GAME_FIELD_WIDTH/2 - GOAL_WIDTH, -GOAL_HEIGHT/2), (-GAME_FIELD_WIDTH/2, -GOAL_HEIGHT/2), GOAL_POST_WIDTH, GOAL_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2 + GOAL_WIDTH,  GOAL_HEIGHT/2), ( GAME_FIELD_WIDTH/2,  GOAL_HEIGHT/2), GOAL_POST_WIDTH, GOAL_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2 + GOAL_WIDTH, -GOAL_HEIGHT/2), ( GAME_FIELD_WIDTH/2, -GOAL_HEIGHT/2), GOAL_POST_WIDTH, GOAL_COLOR)
        self.draw_line((-GAME_FIELD_WIDTH/2 - GOAL_WIDTH,  GOAL_HEIGHT/2), (-GAME_FIELD_WIDTH/2 - GOAL_WIDTH, -GOAL_HEIGHT/2), LINE_WIDTH, GOAL_COLOR)
        self.draw_line(( GAME_FIELD_WIDTH/2 + GOAL_WIDTH,  GOAL_HEIGHT/2), ( GAME_FIELD_WIDTH/2 + GOAL_WIDTH, -GOAL_HEIGHT/2), LINE_WIDTH, GOAL_COLOR)
        # center circle
        self.draw_circle((0, 0), CENTER_CIRCLE_RADIUS, LINE_WIDTH, LINE_COLOR)
        
    def draw_particle(self, (x, y, orientation)):
        self.draw_line((x, y), (x + PARTICLE_ORIENTATION_LINE_LENGTH*cos(orientation), y + PARTICLE_ORIENTATION_LINE_LENGTH*sin(orientation)), PARTICLE_ORIENTATION_LINE_WIDTH, PARTICLE_ORIENTATION_COLOR)
        self.draw_circle((x, y), PARTICLE_RADIUS, 0.001, PARTICLE_COLOR)
            
    def draw_ground_truth_particle(self, (x, y, orientation)):
        self.draw_line((x, y), (x + PARTICLE_ORIENTATION_LINE_LENGTH*cos(orientation), y + PARTICLE_ORIENTATION_LINE_LENGTH*sin(orientation)), PARTICLE_ORIENTATION_LINE_WIDTH, PARTICLE_ORIENTATION_COLOR)
        self.draw_circle((x, y), PARTICLE_RADIUS, 0.001, PARTICLE_COLOR, PARTICLE_COLOR)
        self.draw_line((x, y), (x + PARTICLE_VIEWING_ANGLE_LINE_LENGTH*cos(orientation + CAMERA_H_VIEWING_ANGLE/2.0), y + PARTICLE_VIEWING_ANGLE_LINE_LENGTH*sin(orientation + CAMERA_H_VIEWING_ANGLE/2.0)), PARTICLE_ORIENTATION_LINE_WIDTH, PARTICLE_ORIENTATION_COLOR)
        self.draw_line((x, y), (x + PARTICLE_VIEWING_ANGLE_LINE_LENGTH*cos(orientation - CAMERA_H_VIEWING_ANGLE/2.0), y + PARTICLE_VIEWING_ANGLE_LINE_LENGTH*sin(orientation - CAMERA_H_VIEWING_ANGLE/2.0)), PARTICLE_ORIENTATION_LINE_WIDTH, PARTICLE_ORIENTATION_COLOR)
        
class App:
    def __init__(self, master):
        frame = Frame(master)
        frame.pack()
        
        Button(frame, text="Reset", command=self.reset).grid(row=0, column=0, padx=(10, 10), pady=(10, 0))
        Button(frame, text="Rotate", command=self.rotate).grid(row=0, column=1, padx=(10, 10), pady=(10, 0))
        Button(frame, text="Forward", command=self.forward).grid(row=0, column=2, padx=(10, 10), pady=(10, 0))
        
        self.field_canvas =FieldCanvas(frame)
        self.field_canvas.grid(row=1, columnspan=3, padx=(10, 10), pady=(10, 10))
        
        self.reset()
        
    def reset(self):
        self.particle = Particle()
        self.particle.set(0, 0, 0)
        self.particle_filter = ParticleFilterLocalization()
        self.field_canvas.draw_field()
        self.draw_particles(self.particle_filter.particles)
        
    def move(self, (angle, distance)):
        self.particle = self.particle.move((angle, distance), 0);
        self.particle_filter.localize((angle, distance), self.particle.sense())
        self.field_canvas.draw_field()
        self.draw_particles(self.particle_filter.particles)
        
    def rotate(self):
        self.move((pi/4, 0));
        
    def forward(self):
        self.move((0, 0.1));
        
    def draw_particles(self, particles):
        for i in range(len(particles)):
            self.field_canvas.draw_particle((particles[i].x, particles[i].y, particles[i].orientation))
        self.field_canvas.draw_ground_truth_particle((self.particle.x, self.particle.y, self.particle.orientation))
        
if __name__ == '__main__':
    root = Tk()
    root.title("Robocup")
    app = App(root)
    root.mainloop()