如何基于LeafSystem编写FSM风格的离散规划器？

核心设计逻辑 #

用LeafSystem封装有限状态机（FSM）的状态管理、转移触发逻辑及控制器调度逻辑，完全契合Drake“全层级定义动力学”的理念，替代原有的时间驱动set_monitor切换，让状态转移完全由高阶状态估计条件（如位姿误差、力反馈阈值等）触发。

具体实现步骤 #

1. 定义FSM状态与转移规则

   • 枚举所有任务状态（如插钉任务的接近、对齐、插入），通过DeclareDiscreteState(1)在LeafSystem中创建离散状态端口，用整数编码当前状态；
   • 为每一组状态转移定义触发条件函数：基于输入的状态估计数据（末端位姿、力传感器读数、对准误差等），返回布尔值判断是否满足转移触发条件。

2. 封装控制器调度与输出路由

   • 为每个状态对应的控制器（阻抗设定点或TrajOpt轨迹）配置输入/输出端口：
     • 输入端口：接收MultiBodyPlant的状态输出、状态估计模块的结果；
     • 输出端口：根据当前FSM状态，将对应控制器的指令（阻抗设定点或轨迹跟踪信号）路由到底层控制器。
   • 在CalcOutput函数中，基于离散状态端口的当前值，选择对应控制器的输出作为系统最终输出。

3. 实现条件驱动的状态转移

   • 通过DeclarePeriodicDiscreteUpdateEvent声明离散更新事件，在更新函数中完成状态转移逻辑：
     • 读取当前状态与输入的状态估计数据，遍历所有转移条件，若条件满足则更新离散状态端口的数值；
     • 彻底替代时间触发的set_monitor，实现纯状态条件驱动的切换。

4. 与现有系统集成

   • 将FSMLeafSystem与已实现的MultiBodyPlant、底层控制器做端口连接：
     • 把MultiBodyPlant的状态输出、状态估计结果接入FSM的输入端口；
     • 把FSM的控制器指令输出连接到底层阻抗控制器的设定点输入。

关键优化点 #

• 转移条件鲁棒性：对状态估计数据加入滤波或滞后判定（如对齐状态需持续满足误差阈值超过0.1s才触发插入转移），避免噪声导致状态频繁跳转；
• 控制器平滑切换：状态转移时，对前后控制器的输出做一阶滤波过渡，避免控制指令突变引发机器人抖动；
• 调试可视化：添加输出端口实时输出当前FSM状态，结合Drake的可视化工具监控状态流转过程。

参考资料 #

• Drake官方文档中LeafSystem的离散状态与事件处理章节；
• Drake官方示例（如manipulation_station、pick_and_place）中的状态机实现逻辑。

内容的提问来源于stack exchange，提问作者kamiradi