Ported cartesian controller over from PR2