Reject tiny/large errors in integral control #15
Description
Do not integrate errors if they are less than 1e-3 or greater than 0.3. Even if the setpoint for one joint is out of the range, reject Ki for that setpoint. Basically, the integral of error must be reset to 0. The idea is that the PD controller sends the joints to roughly the right place and then integral control kicks in, improving stability.