WitnessFunction 能否依赖于输入端口？

Question

我想编写一个有限状态机 (FSM)，类似于具有多种不同模式的规划器。我将其实现为一个派生自 LeafSystem 的系统，并希望像 rimless_wheel and the spring-loaded inverted pendulum example to define the transition between modes. (Follow the suggestion in the answer to my 中那样使用 witness 函数。为此，我在 class:

中定义了一个 witness 函数

self.touch_witness = self.MakeWitnessFunction('touch', WitnessFunctionDirection.kNegativeThenNonNegative, self.offset_vertical_force, UnrestrictedUpdateEvent(self.touch_event))

守卫函数self.offset_vertical_force定义为：

def offset_vertical_force(self, context):
      F_ext = self.get_input_port(1).Eval(context)
      return F_ext[1] - 1.0

这里F_ext是感应到的外力，F_ext[1]是垂直方向的力。在我考虑的场景中，起初没有外力，所以 offset_vertical_force 应该 return -1，然后在碰撞发生后， offset_vertical_force 将 return 一个非负值。然后 self.touch_event 将被触发。（witness函数是用DoGetWitnessFunctions方法添加的。）

然而，当我运行代码时，我发现 self.touch_event 从未被触发，尽管事实上 self.offset_vertical_force return 的负值先是returns 正值。然后，当我将保护函数 self.offset_vertical_force 更改为使用 FSM 规划器本身的某些连续状态时，我确实看到可以触发该事件。所以我的第一个问题是，witness function（更准确的说是guard function）是否只能由系统自身的内部状态来定义？

但是，出于我的目的，我需要 FSM 规划器的模式转换由外部信息触发。我的第二个问题是，如果witness function只能由系统自身的内部状态来定义，是否有一些解决方法？

除上述之外，我还尝试更改 self.touch_event

中的状态

def touch_event(self, context, event, state):

起初我尝试使用

改变状态

context.get_mutable_continuous_state_vector().SetFromVector(value)

但是，当我运行代码时，我看到状态没有变化。然后我用

state.get_mutable_continuous_state().get_mutable_vector().SetFromVector(Value)

当我运行代码时，我可以看到状态发生变化。我的理解是事件函数中的 context 是 const 但事件函数中的 state 不是。我说得对吗？

非常感谢您的回答！

Answer 1

陈.

要回答你的第二个问题，是的，在像 touch_event() 这样的事件处理程序中，上下文是 const 并且处理程序的更新被写入可变状态对象。稍后模拟器将新状态传输到上下文中。

对于第一个问题，witness functions仅在完整System（或Diagram）状态的持续演化过程中被触发。集成器在步骤开始时评估所有见证函数，暂时推进状态，然后 re-evaluates 见证函数。如果看到符号变化，则修改步骤以隔离发生时间，然后调用处理程序。

所以witness函数依赖于一个输入端口没有问题，但是那个输入端口的值必须依赖于连续的状态变量。如果输入端口是离散的，则积分器在步骤的开始和结束时看到相同的值，因此不会触发。

我不确定哪种解决方法适合您的应用程序（希望其他人可以提供更多想法）。我的第一个想法是探索是否可以使输入端口依赖于连续状态。

WitnessFunction 能否依赖于输入端口？

Can the WitnessFunction depend on the input port?

drake