How to properly reset the ContinuousState in a class derived from LeafSystem? - drake

Question

I want to write a continuous time system derived from the LeafSystem that can have its continuous state reset to other values if some conditions are met. However, the system does not work as what I expected. To find out the reason, I implement a simple multi-step integrator system as below:
class MultiStepIntegrator(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.state_index = self.DeclareContinuousState(1)
self.DeclareStateOutputPort("x", self.state_index)
self.flag_1 = True
self.flag_2 = True
def reset_state(self, context, value):
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(value)
def DoCalcTimeDerivatives(self, context, derivatives):
t = context.get_time()
if t < 2.0:
V = [1]
elif t < 4.0:
if self.flag_1:
self.reset_state(context, [0])
print("Have done the first reset")
self.flag_1 = False
V = [1]
else:
if self.flag_2:
self.reset_state(context, [0])
print("Have done the second reset")
self.flag_2 = False
V = [-1]
derivatives.get_mutable_vector().SetFromVector(V)
What I expect from this system is that it will give me a piecewise and discontinuous trajectory. Given that I set the state initially to be 0, firstly the state will go from 0 to 2 for $t \in [0,2]$, then agian from 0 to 2 for $t \in [2,4]$ and then from 0 to -2 for $t \in [4,6]$.
Then I simulate this system, and plot the logging with
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-4)
plant.Finalize()
integrator = builder.AddSystem(MultiStepIntegrator())
state_logger = LogVectorOutput(integrator.get_output_port(), builder, 1e-2)
diagram = builder.Build()
simulator = Simulator(diagram)
log_state = state_logger.FindLog(context)
fig = plt.figure()
t = log_state.sample_times()
plt.plot(t, log_state.data()[0, :])
fig.set_size_inches(10, 6)
plt.tight_layout()
It seems that the resets never happen. However I do see the two logs indicating that the resets are done:
Have done the first reset
Have done the second reset
What happened here? Are there some checkings done behind the scene that the ContinuousState cannot jump (as the name indicates)? How can I reset the state value given that some conditions are met?
Thank you very much for your help!

Answer 1

In DoCalcTimeDerivatives, the context is a const (input-only) argument. It cannot be modified. The only thing DoCalcTimeDerivatives can do is output the derivative to enable the integrator to integrate the continuous state.
Not all integrators used fixed-size time steps. Some might need to evaluate the gradients multiple times before deciding what step size(s) to use. Therefore, it's not reasonable for a dx/dt calculation to have any side-effects. It must be a pure function, where its only consequence is to report a dx/dt.
To change a continuous state value other than through pure integration, the System needs to use an "unrestricted update" event. That event can mutate any and all elements of the State (including continuous state).
If the timing of the discontinuities is periodic (even if some events make no change to the state), you can use DeclarePeriodicUnrestrictedUpdateEvent to declare the update calculation.
If the discontinuities happen per a witness function, see bouncing_ball or rimless_wheel or compass_gait for an example.
If you need a generalized (bespoke) triggering schedule for the discontinuity events, you'll need to override DoCalcNextUpdateTime to manually inject the next event timing, something like the LcmSubscriberSystem. We don't have many good examples of this to my knowledge.