def __init__(self):
"""
"""
super(GaitSequencer, self).__init__()
self._state = 'idle'
self._step = 0
self._speed = 0.
self._direction = 90.
self._length = 0.
self._angle = 0.
self._updateWalkSignal = Signal()
self.setDaemon(True)
def __init__(self):
""" GaitSequencer implementation
"""
#super(GaitSequencer, self).__init__(name="GaitSequencer")
FSM.__init__(self)
threading.Thread.__init__(self, name="GaitSequencer")
self._state = 'idle'
self._step = 0
self._speed = 0.
self._direction = 0.
self._length = 0.
self._angle = 0.
self._updateWalkSignal = Signal()
self._lock = threading.RLock()
self.setDaemon(True)
class GaitSequencer(FSM, threading.Thread):
"""
@todo: define states for registered changes?
@todo: add emergency mode
"""
def __init__(self):
"""
"""
super(GaitSequencer, self).__init__()
self._state = 'idle'
self._step = 0
self._speed = 0.
self._direction = 90.
self._length = 0.
self._angle = 0.
self._updateWalkSignal = Signal()
self.setDaemon(True)
@property
def state(self):
return self._state
@property
def step(self):
return self._step
@property
def updateWalkSignal(self):
return self._updateWalkSignal
@property
def speed(self):
return self._speed
@speed.setter
def speed(self, speed):
self._speed = speed
@property
def direction(self):
return self._direction
@direction.setter
def direction(self, direction):
self._direction = direction
@property
def length(self):
return self._length
@length.setter
def length(self, length):
self._length = length
@property
def angle(self):
return self._angle
@angle.setter
def angle(self, angle):
self._angle = angle
def _createTransitions(self):
self._addTransition(event='to_idle', src='stop', dst='idle', onAfter=self._onIdle)
self._addTransition(event='to_idle', src='static_walk', dst='idle', onAfter=self._onIdle)
self._addTransition(event='to_start', src='idle', dst='start', onAfter=self._onStart)
self._addTransition(event='to_walk', src='start', dst='walk', onAfter=self._onWalk)
self._addTransition(event='to_pause', src='start', dst='pause', async=True, onBefore=self._onRegisterEvent, onAfter=self._onPause)
self._addTransition(event='to_pause', src=('walk', 'step'), dst='pause', onAfter=self._onPause)
self._addTransition(event='to_step', src='pause', dst='step', onAfter=self._onStep)
self._addTransition(event='to_static_walk', src='idle', dst='static_walk', onAfter=self._onCycle)
self._addTransition(event='to_resume', src='pause', dst='walk', onAfter=self._onResume)
self._addTransition(event='to_stop', src=('start', 'walk'), dst='stop', async=True, onBefore=self._onRegisterEvent, onAfter=self._onStop)
self._addTransition(event='to_stop', src=('pause', 'step'), dst='walk_then_stop', onAfter=self._onStop)
def _onRegisterEvent(self, event):
"""
"""
Logger().info("Register %s..." % event['name'])
def _onIdle(self, event):
"""
"""
Logger().info("Enter idle")
def _onStart(self, event):
"""
"""
Logger().info("Enter start sequence")
#self._step = 0
def _onWalk(self, event):
"""
"""
Logger().info("Enter walk sequence")
#self._step = 0
def _onPause(self, event):
"""
"""
self._pauseAt = time.time()
Logger().info("Enter pause")
def _onStep(self, event):
"""
"""
Logger().info("Execute step")
def _onCycle(self, event):
"""
"""
Logger().info("Execute static walk")
#self._step = 0
def _onResume(self, event):
"""
"""
Logger().info("Resume walk")
def _onStop(self, event):
"""
"""
Logger().info("Enter stop sequence")
#self._step = 0
def walk(self, speed, direction, length, angle, force=False):
"""
Called by a Controller object.
@todo: manage sign direction changes -> recompute new step (reversed)
"""
Logger().debug("GaitSequencer.walk(): speed=%.1f, direction=%d, length=%.1f, angle=%d" % (speed, direction, length, angle))
if round(speed, 1) != 0. or round(length, 1) != 0.:
self._speed = speed
self._direction = direction
self._length = length
self._angle = angle
if self._state == 'idle':
self.trigger('to_start')
elif self._state == 'pause':
self.trigger('to_resume')
else:
self.trigger('to_pause')
def run(self):
self._running = True
while self._running:
Logger().debug("GaitSequencer.run(): state=%s, step=%.1f" % (self.state, self._step))
try:
if self._state == 'start': # TODO: handle error -> stay on current step, and switch to ???
#Logger().debug("GaitSequencer.run(): start sequence, step=%.1f" % self._step)
try:
feetTarget, duration = GaitManager().gait.startSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
except StopIteration:
self._step = 0
try:
self.trigger('to_walk')
except Py4botFsmError:
self.transition() # execute pending stop/pause transition
elif self._state == 'walk':
#Logger().debug("GaitSequencer.run(): walk sequence, step=%.1f" % self._step)
feetTarget, duration = GaitManager().gait.walkSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
# In case a stop condition is pending
if self._step == 0 and self.transitionPending():
self.transition()
elif self._state == 'walk_then_stop':
#Logger().debug("GaitSequencer.run(): walk sequence, step=%.1f" % self._step)
# Check if ready to switch to stop sequence
if self._step == 0:
self.trigger('to_stop')
feetTarget, duration = GaitManager().gait.walkSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
elif self._state == 'pause':
#Logger().debug("GaitSequencer.run(): pause sequence, step=%.1f" % self._step)
# Handle auto return to stop delay
if config.WALK_PAUSE_TO_STOP_DELAY is not None and time.time() - self._pauseAt > config.WALK_PAUSE_TO_STOP_DELAY:
self.trigger('to_stop')
elif self._state == 'static_walk':
#Logger().debug("GaitSequencer.run(): static walk sequence, step=%.1f" % self._step)
feetTarget, duration = GaitManager().gait.walkSequence(self._step, speed=0.25, direction=0, length=0, angle=0)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
if self._step == 0:
self.trigger('to_idle')
elif self._state == 'step':
#Logger().debug("GaitSequencer.run(): step sequence, step=%.1f" % self._step)
feetTarget, duration = GaitManager().gait.walkSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
# Back to pause
self.trigger('to_pause')
elif self._state == 'stop':
#Logger().debug("GaitSequencer.run(): stop sequence, step=%.1f" % self._step)
try:
feetTarget, duration = GaitManager().gait.stopSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
except StopIteration:
self._step = 0
self.trigger('to_idle')
time.sleep(0.25)
except Py4botError:
Logger().exception("GaitSequencer.run()") #, debug=True)
except:
Logger().exception("GaitSequencer.run()")
Logger().critical("GaitSequencer crashed")
raise SystemExit
Logger().info("GaitSequencer stopped")
def stop(self):
""" stop sequencer
"""
Logger().trace("GaitSequencer.stop()")
self._running = False
class GaitSequencer(FSM, threading.Thread):
""" Gait sequencer
This sequencer is a singleton running in a thread, which generates feet positions according to inputs (from gamepad or so).
This class implements a finite-states machine to switch between the different phase of the gait.
It manages the changes of speed/direction, gait, which can only occure in certain phases/states.
@ivar _gait: current gait
@type _gait: Gait
@ivar _step: current step (sub-steps are allowed; use decimals - ???!!!???)
@type _step: float
@ivar _state: current state of the finite-state machine
'idle': do nothing
'start': start sequence
'pause': pause sequence
'walk': walking sequence
'static_walk': static walking sequence
'stop': stop sequence
@type _state: str
@ivar updateWalkSignal: Signal sent when walk position changed
@type updateWalkSignal: Signal
"""
__metaclass__ = Singleton
def __init__(self):
""" GaitSequencer implementation
"""
#super(GaitSequencer, self).__init__(name="GaitSequencer")
FSM.__init__(self)
threading.Thread.__init__(self, name="GaitSequencer")
self._state = 'idle'
self._step = 0
self._speed = 0.
self._direction = 0.
self._length = 0.
self._angle = 0.
self._updateWalkSignal = Signal()
self._lock = threading.RLock()
self.setDaemon(True)
@property
def state(self):
return self._state
@property
def step(self):
return self._step
@property
def updateWalkSignal(self):
return self._updateWalkSignal
@property
def speed(self):
return self._speed
@speed.setter
def speed(self, speed):
self._speed = speed
@property
def direction(self):
return self._direction
@direction.setter
def direction(self, direction):
self._direction = direction
@property
def length(self):
return self._length
@length.setter
def length(self, length):
self._length = length
@property
def angle(self):
return self._angle
@angle.setter
def angle(self, angle):
self._angle = angle
def _createTransitions(self):
self._addTransition(event='to_idle', src='stop', dst='idle', onAfter=self._onIdle)
self._addTransition(event='to_idle', src='static_walk', dst='idle', onAfter=self._onIdle)
self._addTransition(event='to_start', src='idle', dst='start', onAfter=self._onStart)
self._addTransition(event='to_walk', src='start', dst='walk', onAfter=self._onWalk)
self._addTransition(event='to_pause', src='start', dst='pause', async=True, onBefore=self._onRegisterPause, onAfter=self._onPause)
self._addTransition(event='to_pause', src=('walk', 'step'), dst='pause', onAfter=self._onPause)
self._addTransition(event='to_step', src='pause', dst='step', onAfter=self._onStep)
self._addTransition(event='to_static_walk', src='idle', dst='static_walk', onAfter=self._onStaticWalk)
self._addTransition(event='to_resume', src='pause', dst='walk', onAfter=self._onResume)
self._addTransition(event='to_stop', src=('start', 'walk'), dst='stop', async=True, onBefore=self._onRegisterStop, onAfter=self._onStop)
self._addTransition(event='to_stop', src=('pause', 'step'), dst='walk_then_stop', onAfter=self._onResume)
self._addTransition(event='to_stop', src='walk_then_stop', dst='stop', onAfter=self._onStop)
def _onRegisterPause(self, event):
"""
"""
Logger().info("Pause registered")
def _onRegisterStop(self, event):
"""
"""
Logger().info("Stop registered")
def _onIdle(self, event):
"""
"""
Logger().info("Enter idle")
def _onStart(self, event):
"""
"""
Logger().info("Start sequence")
self._step = 0
def _onWalk(self, event):
"""
"""
Logger().info("Walk sequence")
self._step = 0
def _onPause(self, event):
"""
"""
self._pauseAt = time.time()
Logger().info("Paused")
def _onStep(self, event):
"""
"""
Logger().info("Step")
def _onStaticWalk(self, event):
"""
"""
Logger().info("Static walk sequence")
self._step = 0
def _onResume(self, event):
"""
"""
Logger().info("Resume walk")
def _onStop(self, event):
"""
"""
Logger().info("Stop sequence")
self._step = 0
def selectPrevGait(self):
""" Select previous gait
The gait switch can only occurs at idle state.
"""
Logger().trace("GaitSequencer.setectPrevGait()")
if self._state == 'idle':
GaitManager().selectPrev()
Logger().info("New gait is '%s'" % GaitManager().gait.name)
else:
Logger().warning("Can't switch gait while walking")
def selectNextGait(self):
""" Select next gait
The gait switch can only occurs at idle state.
"""
Logger().trace("GaitSequencer.setectNextGait()")
if self._state == 'idle':
GaitManager().selectNext()
Logger().info("New gait is '%s'" % GaitManager().gait.name)
else:
Logger().warning("Can't switch gait while walking")
def reset(self):
""" Reset sequencer
"""
self._state = 'idle'
self._step = 0
self._speed = 0.
self._direction = 0.
self._length = 0.
self._angle = 0.
def walkStart(self):
""" Ask the sequencer to start walking
"""
self._lock.acquire()
try:
Logger().info("Start triggered")
self.trigger('to_start')
finally:
self._lock.release()
def walkPause(self):
""" Ask the sequencer to pause walking
"""
self._lock.acquire()
try:
Logger().info("Pause triggered")
self._pauseAt = time.time()
self.trigger('to_pause')
finally:
self._lock.release()
def walkStep(self):
""" Ask the sequencer to walk a step
"""
self._lock.acquire()
try:
Logger().info("Walk triggered")
self.trigger('to_step')
finally:
self._lock.release()
def walkResume(self):
""" Ask the sequencer to resume walking
"""
self._lock.acquire()
try:
Logger().info("Resume triggered")
self.trigger('to_resume')
finally:
self._lock.release()
def walkStop(self):
""" Ask the sequencer to stop walking
"""
self._lock.acquire()
try:
Logger().info("Stop triggered")
self.trigger('to_stop')
finally:
self._lock.release()
def walkStatic(self):
""" Ask the sequencer to static walk
"""
self._lock.acquire()
try:
Logger().info("Static walk triggered")
self.trigger('to_static_walk')
finally:
self._lock.release()
def walk(self, speed, direction, length, angle):
"""
Called by a Controller object.
@todo: manage sign direction changes -> recompute new step (reversed)
"""
Logger().debug("GaitSequencer.walk(): speed=%.1f, direction=%d, length=%.1f, angle=%.1f" % (speed, direction, length, angle))
if round(speed, 1) != 0. or round(length, 1) != 0.:
self._speed = speed
self._direction = direction
self._length = length
self._angle = angle
if self._state == 'idle':
self.walkStart()
elif self._state == 'pause':
self.walkResume()
else:
if self._state != 'idle':
self.walkPause()
def run(self):
""" GaitSequencer main loop
State machine.
"""
Logger().trace("GaitSequencer.run()")
Logger().info("Starting GaitSequencer loop...")
# Main loop
self._running = True
while self._running:
#Logger().debug("GaitSequencer.run(): state=%s, step=%.1f" % (self.state, self._step))
try:
self._lock.acquire()
try:
if self._state == 'start': # TODO: handle error -> stay on current step, and switch to ???
#Logger().debug("GaitSequencer.run(): start sequence, step=%.1f" % self._step)
try:
feetTarget, duration = GaitManager().gait.startSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
except StopIteration:
self._step = 0
try:
self.trigger('to_walk')
except Py4botFsmError:
self.transition() # execute pending stop/pause transition
elif self._state == 'walk':
#Logger().debug("GaitSequencer.run(): walk sequence, step=%.1f" % self._step)
feetTarget, duration = GaitManager().gait.walkSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
# In case a stop condition is pending
if self._step == 0 and self.transitionPending():
self.transition()
elif self._state == 'walk_then_stop':
#Logger().debug("GaitSequencer.run(): walk sequence, step=%.1f" % self._step)
# Check if ready to switch to stop sequence
if self._step == 1:
self.trigger('to_stop')
continue
feetTarget, duration = GaitManager().gait.walkSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
elif self._state == 'pause':
#Logger().debug("GaitSequencer.run(): pause sequence, step=%.1f" % self._step)
# Handle auto return to stop delay
if config.WALK_PAUSE_TO_STOP_DELAY is not None and time.time() - self._pauseAt > config.WALK_PAUSE_TO_STOP_DELAY:
self.trigger('to_stop')
elif self._state == 'static_walk':
#Logger().debug("GaitSequencer.run(): static walk sequence, step=%.1f" % self._step)
feetTarget, duration = GaitManager().gait.staticWalkSequence(self._step, speed=0.25)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
if self._step == 0:
self.trigger('to_idle')
elif self._state == 'step':
#Logger().debug("GaitSequencer.run(): step sequence, step=%.1f" % self._step)
feetTarget, duration = GaitManager().gait.walkSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
self._step %= GaitManager().gait.nbSteps
# Back to pause
self.trigger('to_pause')
elif self._state == 'stop':
#Logger().debug("GaitSequencer.run(): stop sequence, step=%.1f" % self._step)
try:
feetTarget, duration = GaitManager().gait.stopSequence(self._step, self._speed, self._direction, self._length, self._angle)
self._updateWalkSignal.emit(feetTarget, duration)
self._step += 1
except StopIteration:
self._step = 0
self.trigger('to_idle')
finally:
self._lock.release()
time.sleep(0.001)
except Py4botError:
Logger().exception("GaitSequencer.run()") #, debug=True)
except:
Logger().exception("GaitSequencer.run()")
Logger().critical("GaitSequencer crashed")
raise SystemExit
Logger().info("GaitSequencer stopped")
def stop(self):
""" stop sequencer
"""
Logger().trace("GaitSequencer.stop()")
self._running = False
