Closed mposa closed 4 years ago
I with Yuming that we might want to add additional checks such as joint/torque limits in future commits. This looks good for now. We can add a check for N consecutive threshold violations if the velocity shutdown is too sensitive during testing.
It's easy enough to add code for N
consecutive failures, and just set N=1
for now. I'll make this change and then merge if it looks good to you.
Another idea--I think we can also have a flag to set an input bound manually. There are some cases where we know only little torque is needed (for example when we test the standing controller) We can manually cap the input to be less than 100 to protect the robot. Not sure how valuable this is.
Good idea. Will add.
@yminchen @yangwill I added a input saturation mechanism and the N-consecutive failures. Please re-review.
Looks good to me. There might be a point in the future where we want to output if the input supervisor is in shutdown mode, but we can add that later.
Added a new input supervisor. Right now, this serves to shut down our controller whenever the velocity of the robot exceeds a threshold. Copied from the class header below. I've tested this briefly against the Gazebo simulation.
/// The InputSupervisor is a simple Drake System that acts as an intermediary /// between commands from controllers and the actual robot. It's envisioned role /// is to (1) sanity check any inputs, (2) shut down in case of errors or /// instability, and (3) to mediate between multiple controllers. Of these three /// potential purposes, only (2) is currently implemented as a check on the /// current velocity of the robot. If the velocity passes a specified threshold, /// in absolute value, then an error flag (discrete state) is set and the output /// commands are set to zero. Note, in the presence of noise, this might be /// a brittle check. If this is the case, we will have to low-pass filter this /// check. For instance, we could only set the flag if the threshold is reached /// N times in a row.