def run_robot(self, currentState):
'''
Runs the robot with currentState parameters and returns the
distance walked. If currentState is a function, calls that
function instead of passing to a motion model.
'''
if hasattr(currentState, '__call__'):
# is a function
motionModel = currentState
else:
# is a parameter vector
model = SineModel5()
motionModel = lambda time: model.model(time,
parameters = currentState)
# Reset before measuring
self.robot.readyPosition()
wiiTrack = WiiTrackClient("localhost", 8080)
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 continue.')
print 'Retrying last run'
return self.run_robot(currentState)
if not self.robot.shimmy():
self.manual_reset('Shimmy failed. Fix and restart.')
return self.run_robot(currentState)
try:
self.robot.run(motionModel, runSeconds = 9, resetFirst = False,
interpBegin = 1, interpEnd = 2)
except RobotFailure as ee:
print ee
override = self.manual_reset('Robot run failure. Fix something and push enter to continue, or enter a fitness to manually enter it.')
# print 'Retrying last run'
# return self.run_robot(currentState)
try:
manualDist = float(override)
return manualDist
except ValueError:
print 'Retrying last run'
return self.run_robot(currentState)
endPos = wiiTrack.getPosition()
if endPos is None:
# Robot walked out of sensor view
override = self.manual_reset('Robot has walked outisde sensor view. Please place back in center\nand push enter to retry or enter manual fitness override to skip.')
try:
manualDist = float(override)
return manualDist
except ValueError:
print 'Retrying last run'
return self.run_robot(currentState)
distance_walked = self.calculate_distance(beginPos, endPos)
#print ' walked %.2f' % distance_walked
if not self.robot.shimmy():
ret = distance_walked / 2.0
print 'Shimmy failed, returning %.2f instead of %.2f' % (ret, distance_walked)
self.manual_reset('Shimmy failed at end, reset robot if necessary and push enter to continue.')
return ret
else:
return distance_walked
def doRun():
# Parameters are: amp, tau, scaleInOut, flipFB, flipLR
android = Robot(commandRate = 40, loud = False)
# TODO: motion = Motion()
# TODO: Move ranges below
#ranges = [(0, 400),
# (.5, 8),
# (-2, 2),
# (False, True),
# (False, True)]
ranges = [(0, 400),
(.5, 8),
(-2, 2),
(-1, 1),
(-1, 1)]
if len(sys.argv) > 1:
currentState = [eval(xx) for xx in sys.argv[1].split()]
else:
currentState = initialState(ranges)
statesSoFar = set() # Keeps track of the states tested so far
bestDistance = -1e100
stateSoFar = set()
logFile = open('log.txt', 'a')
logFile.write('\nOptimize (gauss_op) started at %s\n' % datetime.now().ctime())
logFile.close()
for ii in range(10000):
print
print 'Iteration %2d params' % ii, prettyVec(currentState)
beginDistance = WiiTrackClient.getPosition()
# Make sure this state is new, skip otherwise
if tuple(currentState) in statesSoFar:
print '*** Skipping duplicate iteration!'
continue
stateSoFar.add(tuple(currentState))
motionModel = lambda time: sineModel(time,
parameters = currentState)
android.run(motionModel, runSeconds = 10, resetFirst = False,
interpBegin = 3, interpEnd = 3)
endDistance = WiiTrackClient.getPosition()
currentDistance = RunManager.calculateDistance(beginDistance,
endDistance)
if currentDistance >= bestDistance: # Is this a new best?
bestState = copy(currentState) # Save new neighbor to best found
bestDistance = copy(currentDistance)
print ' best so far', prettyVec(bestState), bestDistance # Prints best state and distance so far
# Writes to log file that keeps track of tests so far
stats = ' '.join([repr(xx) for xx in currentState])
logFile = open('log.txt', 'a')
logFile.write(stats + ", " + str(currentDistance) + "\n")
logFile.close()
currentState = neighbor(ranges, bestState)
return bestState # Return the best solution found (a list of params)
def run_robot(self, currentState):
'''
Runs the robot with currentState parameters and returns the
distance walked. If currentState is a function, calls that
function instead of passing to a motion model.
'''
if hasattr(currentState, '__call__'):
# is a function
motionModel = currentState
else:
# is a parameter vector
model = SineModel5()
motionModel = lambda time: model.model(time,
parameters=currentState)
# Reset before measuring
self.robot.readyPosition()
wiiTrack = WiiTrackClient("localhost", 8080)
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 continue.'
)
print 'Retrying last run'
return self.run_robot(currentState)
if not self.robot.shimmy():
self.manual_reset('Shimmy failed. Fix and restart.')
return self.run_robot(currentState)
try:
self.robot.run(motionModel,
runSeconds=9,
resetFirst=False,
interpBegin=1,
interpEnd=2)
except RobotFailure as ee:
print ee
override = self.manual_reset(
'Robot run failure. Fix something and push enter to continue, or enter a fitness to manually enter it.'
)
# print 'Retrying last run'
# return self.run_robot(currentState)
try:
manualDist = float(override)
return manualDist
except ValueError:
print 'Retrying last run'
return self.run_robot(currentState)
endPos = wiiTrack.getPosition()
if endPos is None:
# Robot walked out of sensor view
override = self.manual_reset(
'Robot has walked outisde sensor view. Please place back in center\nand push enter to retry or enter manual fitness override to skip.'
)
try:
manualDist = float(override)
return manualDist
except ValueError:
print 'Retrying last run'
return self.run_robot(currentState)
distance_walked = self.calculate_distance(beginPos, endPos)
#print ' walked %.2f' % distance_walked
if not self.robot.shimmy():
ret = distance_walked / 2.0
print 'Shimmy failed, returning %.2f instead of %.2f' % (
ret, distance_walked)
self.manual_reset(
'Shimmy failed at end, reset robot if necessary and push enter to continue.'
)
return ret
else:
return distance_walked
def doRun():
# Parameters are: amp, tau, scaleInOut, flipFB, flipLR
android = Robot(commandRate=40, loud=False)
# TODO: motion = Motion()
# TODO: Move ranges below
#ranges = [(0, 400),
# (.5, 8),
# (-2, 2),
# (False, True),
# (False, True)]
ranges = [(0, 400), (.5, 8), (-2, 2), (-1, 1), (-1, 1)]
if len(sys.argv) > 1:
currentState = [eval(xx) for xx in sys.argv[1].split()]
else:
currentState = initialState(ranges)
statesSoFar = set() # Keeps track of the states tested so far
bestDistance = -1e100
stateSoFar = set()
logFile = open('log.txt', 'a')
logFile.write('\nOptimize (gauss_op) started at %s\n' %
datetime.now().ctime())
logFile.close()
for ii in range(10000):
print
print 'Iteration %2d params' % ii, prettyVec(currentState)
beginDistance = WiiTrackClient.getPosition()
# Make sure this state is new, skip otherwise
if tuple(currentState) in statesSoFar:
print '*** Skipping duplicate iteration!'
continue
stateSoFar.add(tuple(currentState))
motionModel = lambda time: sineModel(time, parameters=currentState)
android.run(motionModel,
runSeconds=10,
resetFirst=False,
interpBegin=3,
interpEnd=3)
endDistance = WiiTrackClient.getPosition()
currentDistance = RunManager.calculateDistance(beginDistance,
endDistance)
if currentDistance >= bestDistance: # Is this a new best?
bestState = copy(currentState) # Save new neighbor to best found
bestDistance = copy(currentDistance)
print ' best so far', prettyVec(
bestState), bestDistance # Prints best state and distance so far
# Writes to log file that keeps track of tests so far
stats = ' '.join([repr(xx) for xx in currentState])
logFile = open('log.txt', 'a')
logFile.write(stats + ", " + str(currentDistance) + "\n")
logFile.close()
currentState = neighbor(ranges, bestState)
return bestState # Return the best solution found (a list of params)
