(sbpl_interface) pose goals now working