예제 #1
0
    def run_function_and_log(self,
                             motionFunction,
                             runSeconds,
                             timeScale=1,
                             logFilename=None):
        '''
        Runs the robot with the given motion function from 0 to
        runSeconds, logging time and position to logFileName
        '''

        # Reset before measuring
        self.robot.readyPosition()

        wiiTrack = WiiTrackFastClient("localhost", 8080)
        sleep(.5)
        beginPos = wiiTrack.getPosition()
        if beginPos is None:
            # Robot walked out of sensor view
            self.manual_reset(
                'Robot has walked outisde sensor view.  Please place back in center and push enter to retry.'
            )
            raise Exception

        if not self.robot.shimmy():
            print 'Shimmy failed :('
            #self.manual_reset('Shimmy failed.  Fix and push enter to retry.')
            #####HACK !!!!!!
            #raise Exception

        ff = open(logFilename, 'a')

        try:
            self.robot.run(motionFunction,
                           runSeconds=runSeconds,
                           resetFirst=False,
                           interpBegin=1,
                           interpEnd=2,
                           logFile=ff,
                           extraLogInfoFn=lambda: getLogPosString(wiiTrack))
        except RobotFailure as ee:
            ff.close()
            print ee
            override = self.manual_reset(
                'Robot run failure.  Fix something and push enter to retry.')
            raise Exception
        ff.close()

        endPos = wiiTrack.getPosition()
        if endPos is None:
            # Robot walked out of sensor view
            override = self.manual_reset(
                'Robot has ended outisde sensor view.  Please place back in center\nand push enter to retry.'
            )
            raise Exception

        distance_walked = self.calculate_distance(beginPos, endPos)
        print 'Total Distance: %.2f' % distance_walked
예제 #2
0
def main():
    if len(sys.argv) < 3:
        print 'Usage: %s input_gait_file output_position_file' % sys.argv[0]
        sys.exit(1)

    gaitFile = sys.argv[1]
    posFile  = sys.argv[2]


    strategy = TimedFileStrategy(posFile = gaitFile)

    motionFunction, logInfo = strategy.getNext()

    #runman = RunManager()
    #runman.do_many_runs(strategy, SineModel5.typicalRanges)

    #timeScale = .3
    #motionFunctionScaled = scaleTime(motionFunction, timeScale)
    wiiTrack = WiiTrackFastClient("localhost", 8080)
    time.sleep(.5)
    position,age = wiiTrack.getPosAge()
    if age is None:
        raise Exception('Could not get position from wiiTrack.')

    robot = Robot(loud = True)
    bucket = []

    def foo():
        savePosition(wiiTrack, bucket)

    robot.run(motionFunction, runSeconds = 10, resetFirst = True,
              interpBegin = 2, interpEnd = 2, extraLogInfoFn = foo)

    print 'Positions:'
    print len(bucket)

    relTimeBucket = []
    for ii, line in enumerate(bucket):
        delta = line[0] - bucket[0][0]
        relTime = delta.seconds + delta.microseconds/1e6
        relTimeBucket.append((relTime, line[1], line[2]))


    ff = open (posFile, 'w')
    ff.write('# time (junk junk)x9 pos.x pos.y 0 age\n')
    for ii, timePosAge in enumerate(relTimeBucket):
        timeOfPosition, position, age = timePosAge
        line = '%.3f' % timeOfPosition
        line += ' -1 -1' * 9
        line += ' %.1f %.1f %.1f' % (position[0], position[1], 0)
        line += ' %f' % age
        ff.write(line + '\n')
    ff.close()
    print 'Wrote position file:', posFile
예제 #3
0
    def run_function_and_log(self, motionFunction, runSeconds, timeScale = 1, logFilename = None):
        '''
        Runs the robot with the given motion function from 0 to
        runSeconds, logging time and position to logFileName
        '''

        # Reset before measuring
        self.robot.readyPosition()

        wiiTrack = WiiTrackFastClient("localhost", 8080)
        sleep(.5)
        beginPos = wiiTrack.getPosition()
        if beginPos is None:
            # Robot walked out of sensor view
            self.manual_reset('Robot has walked outisde sensor view.  Please place back in center and push enter to retry.')
            raise Exception

        if not self.robot.shimmy():
            print 'Shimmy failed :('
            #self.manual_reset('Shimmy failed.  Fix and push enter to retry.')
            #####HACK !!!!!!
            #raise Exception

        ff = open(logFilename, 'a')

        try:
            self.robot.run(motionFunction, runSeconds = runSeconds, resetFirst = False,
                           interpBegin = 1, interpEnd = 2, logFile = ff,
                           extraLogInfoFn = lambda: getLogPosString(wiiTrack))
        except RobotFailure as ee:
            ff.close()
            print ee
            override = self.manual_reset('Robot run failure.  Fix something and push enter to retry.')
            raise Exception
        ff.close()
        
        endPos = wiiTrack.getPosition()
        if endPos is None:
            # Robot walked out of sensor view
            override = self.manual_reset('Robot has ended outisde sensor view.  Please place back in center\nand push enter to retry.')
            raise Exception

        distance_walked = self.calculate_distance(beginPos, endPos)
        print 'Total Distance: %.2f' % distance_walked
예제 #4
0
def main22():
    if len(sys.argv) > 2 and sys.argv[1] == '-filt':
        filtFile = sys.argv[2]
        strategy = FileStrategy(filtFile = filtFile)
        motionFunction, logInfo = strategy.getNext()
    elif len(sys.argv) > 2 and sys.argv[1] == '-sine':
        sineModel5Params = [eval(xx) for xx in sys.argv[2].split()]
        print 'Using SineModel5 with params: ', sineModel5Params
        motionFunction = lambda time: SineModel5().model(time,
                                                         parameters = sineModel5Params)
    else:
        #filtFile = '../results/hyperneatTo20gens_101/neat_110115_175446_00014_008_filt'
        filtFile = '../results/hyperneatTo20gens_101/neat_110115_175446_00004_007_filt'
        strategy = FileStrategy(filtFile = filtFile)
        motionFunction, logInfo = strategy.getNext()
    	

    
    #runman = RunManager()
    #runman.do_many_runs(strategy, SineModel5.typicalRanges)

    #timeScale = .3
    #motionFunctionScaled = scaleTime(motionFunction, timeScale)
    wiiTrack = WiiTrackFastClient("localhost", 8080)
    time.sleep(.5)
    position,age = wiiTrack.getPosAge()
    if age is None:
        raise Exception('Could not get position from wiiTrack.')

    robot = Robot(loud = True)
    bucket = []

    def foo():
        savePosition(wiiTrack, bucket)

    robot.run(motionFunction, runSeconds = 8, resetFirst = False,
              interpBegin = 2, interpEnd = 2, extraLogInfoFn = foo)
    
    print bucket