def main():
MIN_ARGS = 4
if len(sys.argv) >= MIN_ARGS:
if len(sys.argv) > 2 and sys.argv[1] == '-SineModel5':
sineModel5Params = [eval(xx) for xx in sys.argv[2].split()]
print 'Using SineModel5 with params: ', sineModel5Params
motionFunction = lambda time: SineModel5().model(
time, parameters=sineModel5Params)
elif len(sys.argv) > 2 and sys.argv[1] == '-neatFiltFile':
raise Exception('not yet')
filtFile = sys.argv[2]
currentState = None
else:
usage()
runName = sys.argv[3]
if len(sys.argv) > 4:
timeScale = float(sys.argv[4])
else:
timeScale = 1
else:
usage()
motionFunctionScaled = scaleTime(motionFunction, timeScale)
runman = RunManager()
print 'Run name is:', runName
for ii in range(1):
print
print 'Iteration', ii
runman.run_function_and_log(motionFunctionScaled,
runSeconds=10,
timeScale=1,
logFilename='log_%s.txt' % runName)
def main():
MIN_ARGS = 4
if len(sys.argv) >= MIN_ARGS:
if len(sys.argv) > 2 and sys.argv[1] == '-SineModel5':
sineModel5Params = [eval(xx) for xx in sys.argv[2].split()]
print 'Using SineModel5 with params: ', sineModel5Params
motionFunction = lambda time: SineModel5().model(time,
parameters = sineModel5Params)
elif len(sys.argv) > 2 and sys.argv[1] == '-neatFiltFile':
raise Exception('not yet')
filtFile = sys.argv[2]
currentState = None
else:
usage()
runName = sys.argv[3]
if len(sys.argv) > 4:
timeScale = float(sys.argv[4])
else:
timeScale = 1
else:
usage()
motionFunctionScaled = scaleTime(motionFunction, timeScale)
runman = RunManager()
print 'Run name is:', runName
for ii in range(1):
print
print 'Iteration', ii
runman.run_function_and_log(motionFunctionScaled, runSeconds = 10, timeScale = 1, logFilename = 'log_%s.txt' % runName)
def run(self, motionFunction, runSeconds = 10, resetFirst = True,
interpBegin = 0, interpEnd = 0, timeScale = 1, logFile = None,
extraLogInfoFn = None):
'''Run the robot with a given motion generating function.
Positional arguments:
motionFunction -- Function used to generate the desired motor
positions. This function must take a single
argument -- time, in seconds -- and must
return the desired length 9 vector of motor
positions. The current implementation
expects that this function will be
deterministic.
Keyword arguments:
runSeconds -- How many seconds to run for. This is in
addition to the time added for interpBegin and
interpEnd, if any. Default: 10
resetFirst -- Begin each run by resetting the robot to its
base position, currently implemented as a
transition from CURRENT -> POS_FLAT ->
POS_READY. Default: True
interpBegin -- Number of seconds over which to interpolate
from current position to commanded positions.
If this is not None, the robot will spend the
first interpBegin seconds interpolating from its
current position to that specified by
motionFunction. This should probably be used
for motion models which do not return POS_READY
at time 0. Affected by timeScale. Default: None
interpEnd -- Same as interpBegin, but at the end of motion.
If interpEnd is not None, interpolation is
performed from final commanded position to
POS_READY, over the given number of
seconds. Affected by timeScale. Default: None
timeScale -- Factor by which time should be scaled during this
run, higher is slower. Default: 1
logFile -- File to log time/positions to, should already be
opened. Default: None
extraLogInfoFn -- Function to call and append info to every
line the log file. Should return a
string. Default: None
'''
#net, actuators = initialize()
#def run(self, motionFunction, runSeconds = 10, resetFirst = True
# interpBegin = 0, interpEnd = 0):
if self.loud:
print 'Starting motion.'
failures = self.pingAll()
if failures:
self.initServos()
failures = self.pingAll()
if failures:
raise RobotFailure('Could not communicate with servos %s at beginning of run.' % repr(failures))
self.resetClock()
self.currentPos = self.readCurrentPosition()
if logFile:
#print >>logFile, '# time, servo goal positions (9), servo actual positions (9), robot location (x, y, age)'
print >>logFile, '# time, servo goal positions (9), robot location (x, y, age)'
# Reset the robot position, if desired
if resetFirst:
self.interpMove(self.readCurrentPosition(), POS_FLAT, 3)
self.interpMove(POS_FLAT, POS_READY, 3)
#self.interpMove(POS_READY, POS_HALFSTAND, 4)
self.currentPos = POS_READY
self.resetClock()
# Begin with a segment smoothly interpolated between the
# current position and the motion model.
if interpBegin is not None:
self.interpMove(self.currentPos,
scaleTime(motionFunction, timeScale),
interpBegin * timeScale,
logFile, extraLogInfoFn)
self.currentPos = motionFunction(self.time)
# Main motion segment
self.interpMove(scaleTime(motionFunction, timeScale),
scaleTime(motionFunction, timeScale),
runSeconds * timeScale,
logFile, extraLogInfoFn)
self.currentPos = motionFunction(self.time)
# End with a segment smoothly interpolated between the
# motion model and a ready position.
if interpEnd is not None:
self.interpMove(scaleTime(motionFunction, timeScale),
POS_READY,
interpEnd * timeScale,
logFile, extraLogInfoFn)
failures = self.pingAll()
if failures:
# give it a second chance
sleep(1)
failures = self.pingAll()
if failures:
raise RobotFailure('Servos %s may have died during run.' % repr(failures))
