New Subsumption Works on TeleOp!

Subsumption changes to no history, works with manual and avoidance. Need
to test on Loki.