Optimization works! Going from control to state checks outgit status Prismatic joint bounds are a little funky