fix for #8, not tested on real robot yet.