How can one write an FSM style discrete planner using the LeafSystem?

For a project I am working on, I want to write a high-level finite state-machine planner with prescribed transitions between a set of pre-defined states. Each transition would be a different set-point control for an impedance controller (or some TrajOpt formulation). The idea is to trigger switches whenever some condition for higher-order state estimation is met. For example, robot inserting a peg into hole can be split into the approach state, align state, inserted state - writing up controllers for corresponding transitions. This is similar to a question posted here, however I am not sure how it actually works on drake.

I have the low level MultiBodyPlant working with all the controllers working. The way I switch states right now is by using the set_monitor function with some time-based logic. However, I'd like to utilise drake's "specify dynamics at all levels" philosophy and think about how to write a high-level planner using the LeafSystem. Any ideas or resources on how to go about this?

Thank you for your support

Solution

Adding methods to help writing finite-state machines and/or behavior trees in the systems framework has been a standing goal for me, but I'm afraid I haven't gotten to it yet.

You can find an example of writing an FSM manually in the "clutter clearing" example of my manipulation notes.

The basic idea is to declare an enum

class PlannerState(Enum):
  WAIT_FOR_OBJECTS_TO_SETTLE = 1
  PICKING_FROM_X_BIN = 2
  PICKING_FROM_Y_BIN = 3
  GO_HOME = 4

then to declare an abstract state for the mode

  self._mode_index = self.DeclareAbstractState(
    AbstractValue.Make(PlannerState.WAIT_FOR_OBJECTS_TO_SETTLE)
  )

then declare a periodic unrestricted update function that works (and potentially updates) the mode

def Update(self, context, state):
  mode = context.get_abstract_state(int(self._mode_index)).get_value()
  ...

  if mode == PlannerState.WAIT_FOR_OBJECTS_TO_SETTLE:
    if context.get_time() - times["initial"] > 1.0:
      self.Plan(context, state)
    return
  elif mode == PlannerState.GO_HOME:
  ...