Robot Find LightΒΆ
Creating a dataset for finding the light.
In [1]:
from jyro.simulator import (Robot, Pioneer, Pioneer16Sonars, PioneerFrontLightSensors,
Camera, Simulator, VSimulator)
import numpy as np
from math import pi
from random import random
In [2]:
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)
In [3]:
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
In [4]:
def get_senses(robot):
light = robot["light"].getData()
#print("light", light)
sonar = robot["sonar"].getData()
#print("sonar", sonar)
return [light, sonar]
In [5]:
def random_start(robot):
robot.setPose(0.5 + random()*2.5, 0.5 + random()*2, random()*2*pi)
In [6]:
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)
In [7]:
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
In [8]:
robot["camera"].getImage().resize((240, 160))
Out[8]:
In [9]:
robot["camera"].lights
Out[9]:
[(2.661347914428072, 1.9886887713326211)]
In [10]:
robot["camera"].scan
Out[10]:
[(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)]
In [11]:
get_senses(robot)
Out[11]:
[[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]]
In [12]:
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()
In [13]:
generate_data(robot, make_world, 5, "testing_data.txt")