Finite State Machine Planner with specified transitions

[kamiradi]

Hi,

Building on top of this stackoverflow discussion, I'd like to implement a Finite State Machine that allows the user to specify higher-order states and corresponding transitions between the higher-order states. This is a standard way to program control-switching logic for robots to perform complex behaviour.

Taking inspiration from the "cluttered scened" example, I'd like to add a task-level planner that takes care of this Motion Plan (MP) switching between higher order states. The crucial aspect here would be to implement this using LeafSystem inheritance as this could then easily be added into subsequent diagrams. A simple pencil diagram of the same has been attached to this issue.

I would also like to take this opportunity to start contributing to your repository. As this is my first such issue, I was hoping to get some feedback on this from the maintainers.

Do let me know what you think. Looking forward to your feedback!

[xuchenhan-tri]

Assigned to @hongkai-dai for initial triage per platform reviewer checklist.

[jwnimmer-tri]

I interpret this request as providing a framework for writing a planner, not implementing an actual planning algorithm. As such, it should be the system framework component, not the planning component.

[kamiradi]

Hey, was wondering how the process for getting started on something like this works for your repo. Shall I wait for feedback from the assigned maintainer?

[jwnimmer-tri]

I think it's worth pointing out that a user can already write a LeafSystem that exhibits FSM behavior -- this is demonstrated in the "Bin Picking" demo linked above.

What's not yet clear to me in this issue is what exactly we'd like to add to Drake?

Is the goal only to make the "Drake with FSM" capability easier for users to discover and write their own FSMs?

If so, then we don't even necessarily need to add any new library code to Drake. It's possible that simply adding a new Tutorial (and some documentation cross- references) would be sufficient.

The more complicated alternative would be to offer some kind of reusable code (e.g., an abstract base class) so the user only needs to enumerate their states and transitions, and then Drake can automatically convert that information to the state and event declaration(s) required by the system framework.

One way to view that kind of code would be as "syntactic sugar" -- taking something already possible, and making it less burdensome to express as code.

Another possibility is that with careful attention to the API, the library code could also enable analysis tools to directly interrogate the FSM structure of the system, before it gets compiled down into events. This may lead to more a powerful programming system, compared to writing out FSMs manually -- essentially a new feature, not mere syntactic sugar.

It's important to decide which of the above we're aiming for, because they vary wildly in the amount of effort required. As per Fred Brooks in the first few pages of The Mythical Man-Month, going from a simple Program (a concrete FSM system written by hand in a tutorial) to a Programming Systems Product (the Drake library code for generically writing FSMs) will consume on the order of 9 times more effort to implement. Writing reusable modeling infrastructure is extremely difficult.

If we're talking about generic code, then the other thing that wasn't clear to me was which category FSM we're aiming for.

The "Bin Picking" demo is a Mealy machine (because of its "output the current pose" fallback when we're outside of a trajectory), but could easily see users wanting a Moore machine instead so that the system is not direct-feedthrough. Should we aim for both?

Also, in my experience anyone who tries to use an FSM in earnest eventually becomes dissatisfied with those basic models and either gives up (going back to writing raw event handlers manually) or switches to using a more powerful model like a Harel statechart. It's possible that if Drake offers an abstract base class for FSMs, it should be a full-on statechart so that users can directly express those more powerful abstractions.

If we're talking about generic code, then the other thing that wasn't clear to me how we map the input alphabet onto the systems framework. Is the state transition function just a black box callback (in which case analysis tools cannot penetrate it and we're in the realm of syntactic sugar, with no new features) or do we somehow try to map the input ports onto an alphabet? Similarly, the "Bin Picking" demo uses a simple, periodic clock. Is that all we should aim for? In some cases we might want to immediately make the transition (using a Witness triggered event, instead of a periodic event).

[jwnimmer-tri]

Oops, one other thing I intended to mention. There are already powerful and well-known tools out there like py_trees. Another path forward here would be to offer a Tutorial and/or library code for bolting py_trees onto a Drake LeafSystem.

[kamiradi]

Thank you @jwnimmer-tri for your very insightful reply. Indeed, I initially thought of a more generic base-class that could be utilised by users to construct FSM style planners. I used my own case as the motivation, where I have a set of DiffIK based controllers (with various constraints and objectives), and a set of States that would be composed using these controllers.

Your suggestion of offering a Harel-statechart as a Base class, and some intermediate specification that drake can compile into the state machine is what I had in mind. The implementation details were something that I wanted to discuss with the Drake maintainers and the broader community.

As a more short-term MVP approach, your last comment on utilising py_trees and demonstrating an FSM through tutorials could be good a milestone. Based on how the community responds we can think of a more structured API approach.

Your thoughts?

[jwnimmer-tri]

I don't think I have much new to add to the discussion. I posed a lot of open questions, and not many of them have been answered. Maybe trying to discuss them in prose is difficult.

One possible next step might be to write a code outline (i.e., pseudo-code) showing what you have in mind to add to Drake. Then we could discuss that specific outline, which might be easier.