Updated servo controller and fixed dumb bug with Pose