Updated to work with modified pr2 robot model