Added intial robohow pick and place plan, plus some utility functions