optimal control looks like it works now that the state bounds have been added. some trajectory-tracking finessing is required to make this operable on the real robot, but that's not a real problem
