I want to write a finite state machine (FSM) like planner that has several different modes. I implement it as a system derived from the LeafSystem and want to use the witness function like in the rimless_wheel and the spring-loaded inverted pendulum example to define the transition between modes. (Follow the suggestion in the answer to my another question). To do so, I define a witness function inside my class:
self.touch_witness = self.MakeWitnessFunction('touch', WitnessFunctionDirection.kNegativeThenNonNegative, self.offset_vertical_force, UnrestrictedUpdateEvent(self.touch_event))
With the guard function self.offset_vertical_force defined as:
def offset_vertical_force(self, context):
F_ext = self.get_input_port(1).Eval(context)
return F_ext[1] - 1.0
Here the F_ext is the external force sensed and F_ext[1] is the force on the vertical direction. In my considered scenario, at first there is no external force so offset_vertical_force should return -1, then after collision happens, the offset_vertical_force would return a NonNegative Value. Then self.touch_event will be trigered. (The witness function is added with the DoGetWitnessFunctions method.)
However, when I run the code, I find that the self.touch_event is never triggered, despite the fact that self.offset_vertical_force returns negative value first and then returns positive value. Then when I change the guard function self.offset_vertical_force to use some continuous state of the FSM planner itself, I do see that the event can be triggered. So my first question is that is it true that the witness function (to be more precise, the guard function) can only be defined by the system's own internal state?
However, for my purpose I need the FSM planner's mode transition be triggered by the external information. My second question is that if the witness function can only be defined by the system's own internal state, are there some workaround ways?
Apart from above, I have also tried to change the state in the self.touch_event
def touch_event(self, context, event, state):
At first I tried to change the state using
context.get_mutable_continuous_state_vector().SetFromVector(value)
However, when I run the code I see no change in the state. Then I use
state.get_mutable_continuous_state().get_mutable_vector().SetFromVector(Value)
and when I run the code I can see the state changes. My understanding is that the context in the event function is const but the state in the event function is not. Am I right?
Thank you very much for your answer!
Chen.
To answer your second question, yes in an event handler like touch_event() the Context is const and the handler's update is written to the mutable state object. Later the Simulator transfers the new state into the Context.
For the first question, witness functions are triggered only during continuous evolution of the full System (or Diagram) state. The integrator evaluates all witness functions at the beginning of a step, advances the state tentatively, then re-evaluates the witness functions. If a sign change is seen, then the step is revised to isolate the time of occurrence, and then the handler is called.
So there is no problem with having the witness function depend on an input port, but the value at that input port must be dependent on continuous state variables. If the input port is discrete, then the integrator sees the same value at the beginning and end of step so doesn't trigger.
I'm not sure what workaround would be appropriate for your application (hopefully someone else can provide more ideas). My first thought would be to explore whether you can make the input port dependent on continuous state.