Too many changes - have forgotten. Basically close to final version of code for IROS.