Robot Find Light

Creating a dataset for finding the light.

from jyro.simulator import (Robot, Pioneer, Pioneer16Sonars, PioneerFrontLightSensors,
                            Camera, Simulator, VSimulator)
import numpy as np
from math import pi
from random import random

def make_world(physics):
    physics.addBox(0, 0, 4, 4, fill="backgroundgreen", wallcolor="gray")
    physics.addBox(1.75, 2.9, 2.25, 3.0, fill="blue", wallcolor="blue")
    physics.addLight(2, 3.5, 1.0)

def make_robot():
    robot = Pioneer("Pioneer", 2, 1, 0) #paremeters are x, y, heading (in radians)
    robot.addDevice(Camera())
    robot.addDevice(Pioneer16Sonars())
    light_sensors = PioneerFrontLightSensors(3) #parameter defines max range
    #light_sensors.lightMode = 'ambient'
    robot.addDevice(light_sensors)
    return robot

def get_senses(robot):
    light = robot["light"].getData()
    #print("light", light)
    sonar = robot["sonar"].getData()
    #print("sonar", sonar)
    return [light, sonar]

def random_start(robot):
    robot.setPose(0.5 + random()*2.5, 0.5 + random()*2, random()*2*pi)

def determine_move(senses):
    """Returns tuple of (translation, rotation) movement"""
    lights = senses[0]
    left_light = lights[0]
    right_light = lights[1]
    light_diff = abs(left_light-right_light)
    sonars = senses[1]
    # if found light, then stop
    if sum(lights) > 1.8:
        return (0, 0)
    # if getting close to an obstacle in front, turn to avoid it
    elif min(sonars[2:6]) < 0.5:
        # if closer on left, turn right
        if min(sonars[1:4]) < min(sonars[4:7]):
            return (0, -0.3)
        # otherwise, turn left
        else:
            return (0, 0.3)
    # if diff in light readings is high enough or total of light readings is
    # low ennough, then turn towards the light
    elif light_diff > 0.1 or sum(lights) < 0.1:
        # if brighter on left side, turn slightly left
        if  left_light > right_light:
            return (0.1, 0.3)
        else:
            return (0.1, -0.3)
    # default is to go straight
    else:
        return (0.3, 0)

def find_light_brain(robot):
    senses = get_senses(robot)
    translate, rotate = determine_move(senses)
    robot.move(translate, rotate)

robot = make_robot()
vsim = VSimulator(robot, make_world) #create a visual simulator to watch robot's behavior
random_start(robot)
vsim.update_gui()
robot.brain = find_light_brain

robot["camera"].getImage().resize((240, 160))

robot["camera"].lights

[(2.661347914428072, 1.9886887713326211)]

robot["camera"].scan

[(151, 9, 3.1468658102018714, 0.6853134189798128),
 (151, 9, 3.205917041685684, 0.6794082958314316),
 (151, 9, 3.271427583532677, 0.6728572416467322),
 (151, 9, 3.117340854250534, 0.6882659145749466),
 (151, 9, 2.955939301808889, 0.7044060698191111),
 (151, 10, 2.813799179070223, 0.7186200820929777),
 (151, 10, 2.6879332300963967, 0.7312066769903603),
 (151, 11, 2.575952233737128, 0.7424047766262871),
 (151, 11, 2.475923400873351, 0.752407659912665),
 (151, 11, 2.3862669170965702, 0.761373308290343),
 (151, 11, 2.3056804602620136, 0.7694319539737986),
 (151, 12, 2.233080789922769, 0.7766919210077231),
 (151, 12, 2.1675613146576613, 0.7832438685342339),
 (151, 12, 2.1083576826492845, 0.7891642317350716),
 (151, 12, 2.054822509355691, 0.7945177490644308),
 (151, 12, 2.006404088175722, 0.7993595911824277),
 (151, 12, 1.9626309549651584, 0.8037369045034841),
 (151, 13, 1.9230983499660799, 0.807690165003392),
 (151, 13, 1.8874580699053687, 0.8112541930094631),
 (151, 13, 1.855410109638303, 0.8144589890361698),
 (151, 13, 1.8266954680809961, 0.8173304531919005),
 (151, 13, 1.8010907999569872, 0.8198909200043012),
 (151, 13, 1.7784038489031955, 0.8221596151096804),
 (151, 13, 1.7584693819658332, 0.8241530618034167),
 (151, 13, 1.7411464026794672, 0.8258853597320532),
 (151, 13, 1.726315305661425, 0.8273684694338576),
 (151, 13, 1.7138759757197426, 0.8286124024280257),
 (151, 13, 1.70374598830988, 0.8296254011690121),
 (151, 13, 1.6958592459161284, 0.8304140754083871),
 (151, 13, 1.6901649600661617, 0.8309835039933839),
 (151, 13, 1.6866268159290632, 0.8313373184070937),
 (151, 13, 1.6852223823983883, 0.8314777617601612),
 (151, 13, 1.6859427943949996, 0.8314057205605),
 (151, 13, 1.6887925940390223, 0.8311207405960979),
 (151, 13, 1.6937898161250025, 0.8306210183874997),
 (151, 13, 1.700966267038597, 0.8299033732961403),
 (151, 13, 1.7103680115403526, 0.8289631988459647),
 (151, 13, 1.722056088186009, 0.8277943911813992),
 (151, 13, 1.7361075622433133, 0.8263892437756686),
 (151, 13, 1.7526166634867562, 0.8247383336513243),
 (151, 13, 1.7716965367527164, 0.8228303463247283),
 (151, 13, 1.7934810363489488, 0.8206518963651052),
 (151, 13, 1.8181272471987486, 0.8181872752801251),
 (151, 13, 1.8458184304946816, 0.8154181569505319),
 (151, 13, 1.876767429202236, 0.8123232570797765),
 (151, 13, 1.9112211316817869, 0.8088778868318214),
 (151, 12, 1.949465798554368, 0.8050534201445633),
 (151, 12, 1.9918332863340396, 0.8008166713665961),
 (151, 12, 2.0387089690575677, 0.7961291030942432),
 (151, 12, 2.0054235864692225, 0.7994576413530778),
 (151, 13, 1.9140926223573742, 0.8085907377642625),
 (151, 13, 1.832927119961525, 0.8167072880038475),
 (151, 13, 1.7604929736783075, 0.8239507026321693),
 (151, 13, 1.6956213995870806, 0.8304378600412919),
 (151, 13, 1.637350561557506, 0.8362649438442494),
 (151, 14, 1.5848822220366188, 0.8415117777963381),
 (151, 14, 1.5375492910943342, 0.8462450708905666),
 (151, 14, 1.4947903656360915, 0.8505209634363908),
 (151, 14, 1.4561303674579862, 0.8543869632542014),
 (151, 14, 1.4211653031102505, 0.857883469688975)]

get_senses(robot)

[[0.1461228864206821, 0.12172244605589383],
 [2.6372377111130882,
  2.854330863239106,
  2.0527609659953323,
  1.5421885828814874,
  1.4881493361730065,
  1.8251022517793707,
  1.1900404307087387,
  1.0197245152096421,
  1.0079192040998226,
  1.1047337454336814,
  1.5508853769772477,
  2.205636881674792,
  2.130937951766538,
  1.2567712693888453,
  3.0386776706954968,
  2.6490431220491546]]

def generate_data(robot, make_world, trials, filename):
    sim = Simulator(robot, make_world)
    fp = open(filename, "w")
    for i in range(trials):
        #print("Trial %d" % i)
        random_start(robot)
        while True:
            senses = get_senses(robot)
            translate, rotate = determine_move(senses)
            if translate == 0 and rotate == 0:
                break # found light, so end trial
            robot.move(translate, rotate)
            sim.step()
            lights = senses[0]
            sonars = [min(v/3.0, 1.0) for v in senses[1]] #normalize sonar values
            for value in lights:
                fp.write("%.3f " % value)
            for value in sonars[1:7]:
                fp.write("%.3f " % value)
            fp.write("%.1f %.1f\n" % (translate, rotate))
    fp.close()

generate_data(robot, make_world, 5, "testing_data.txt")