Hi booster,
I took the torque command after the manual switch (first attached picture), and gave it in input to a subsystem I called bias generator (second attached picture) in which there is discrete-time integrator.
As you can see, I have enabled a reset connected to a constant block. This block is connected to a push-button outside the subsystem. I use it every time I need to reset the integral.
Then, I directly add the output of the block to the theta angle before it enters the controller subsystem.
I hope this helps.
