Corrected some of umi-bridge encoder to state conversions. (wrist still wrong).
Started implementing control with controll manager. Compiles but does not link.