WikiSandBox

  1 
  2 
  3 
  4 
  5 
  6 
  7 
  8 
  9 
 10 
 11 
 12 
 13 
 14 
 15 
 16 
 17 
 18 
 19 
 20 
 21 
 22 
 23 
 24 
 25 
 26 
 27 
 28 
 29 
 30 
 31 
 32 
 33 
 34 
 35 
 36 
 37 
 38 
 39 
 40 
 41 
 42 
 43 
 44 
 45 
 46 
 47 
 48 
 49 
 50 
 51 
 52 
 53 
 54 
 55 
 56 
 57 
 58 
 59 
 60 
 61 
 62 
 63 
 64 
 65 
 66 
 67 
 68 
 69 
 70 
 71 
 72 
 73 
 74 
 75 
 76 
 77 
 78 
 79 
 80 
 81 
 82 
 83 
 84 
 85 
 86 
 87 
 88 
 89 
 90 
 91 
 92 
 93 
 94 
 95 
 96 
 97 
 98 
 99 
100 
101 
102 
103 
104 
105 
106 
107 
108 
109 
110 
111 
112 
113 
114 
115 
116 
117 

from pyrobot.brain import Brain
from pyrobot.brain import psom
import random
import time

def printList(ls, file):
    for i in range(len(ls)):
        file.write(str(ls[i]) + " ")
    file.write("\n")

class BothSoms(Brain):
    """  
    Written by Cassandra Telenko and Lisa Meeden  
     
    This class is designed to work with the Pioneer simulated robot.  
    The goal of this class is to dynamically create a som of sonar data  
    from a robot doing simple navigation with obstacle avoidance.  
      
    It operates in two modes: CollectData and Run.  The robot begins  
    in CollectData mode, and saves samples of its front sonar values  
    to a file called ir.dat while it is navigating.  After a  
    designated number of steps (given by maxCollectSteps), the sample  
    data file is closed and used to initialize a som.  Then the robot  
    goes into Run mode and uses its sonar values to train the som for  
    a designated number of steps (given by maxRunSteps).  
         
    With Doug and Deepak's avoidObstacles. 
 
    Modified By Cassandra Telenko: 
    It does the Above with Motor Values as well. The samples are saved to            motor.dat. 
    """

    mode='CollectData'
    maxSteps = 50
    currStep = 0
    minDistance = .9
    sensorDat = open("ir.dat", "w")
    sensorDat.write(str(8)+"\n")
    motorDat = open("motor.dat","w")
    motorDat.write(str(2)+"\n")
    myMotorSom=1
    mySensorSom=1

    def step(self):
        if self.mode=='CollectData':
            if self.currStep<self.maxSteps:
                instance=self.scout()
                sensorinstance=instance[0]
                motorinstance=instance[1]
                printList(sensorinstance, self.sensorDat)
                printList(motorinstance, self.motorDat)
                self.currStep = self.currStep + 1
            else:
                self.sensorDat.close()
                self.motorDat.close()
                print "done collecting sample data"
                self.stop() # stop the robot
                #Initializes the Sensor Som 
                mySensorDat = psom.dataset(file="ir.dat")
                self.mySensorSom = psom.psom(5,7,data=mySensorDat)
                self.mySensorSom.init_training(0.02,3.0,self.maxSteps)
                #Initializes the Motor Som 
                myMotorDat = psom.dataset(file="motor.dat")
                self.myMotorSom = psom.psom(5,7,data=myMotorDat)
                self.myMotorSom.init_training(0.02,3.0,self.maxSteps)
                self.mode='RunSom'
                self.currStep=0
                print "done initializing"
        elif self.mode=='RunSom':
            instance=self.scout()
            model1 = self.mySensorSom.train(psom.vector(instance[0]))
            model2 = self.myMotorSom.train(psom.vector(instance[1]))
            print "Sensor Input:\n" + str(instance[0]) + "\nSensor Model: "
            model1.display()
            print "Motor Input:\n" + str(instance[1]) + "\nMotor Model: "
            model2.display()
            self.currStep=self.currStep+1
            if self.currStep==self.maxSteps:
                self.mode='Done'
                print "done training"
        else:
            self.stop() # stop the robot

    def scout(self):
        motorInstance=self.avoidObstacles()
        time.sleep(0.25)
        sensorInstance = range(0,8)
        for i in range(0,8):
            sensorInstance[i] = self.robot.sonar[0][i].value
        return sensorInstance,motorInstance


    def avoidObstacles(self):
        minDist = self.minDistance
        happy=random.random()
        if min([s.value for s in self.robot.range["front"]]) < minDist:
            if (random.random() < 0.5):
                self.move(0, -happy)
                return [0,-happy]
            else:
                self.move(0, happy)
                return [0,happy]

        elif min([s.value for s in self.robot.range["front-left"]]) < minDist:
            self.move(0, -happy)
            return [0,-happy]
        elif min([s.value for s in self.robot.range["front-right"]]) < minDist:
            self.move(0,happy)
            return [0,happy]
        else:
            self.move(.2,0)
            return [.2,0]


def INIT(engine):
   return BothSoms('BothSoms', engine)