fix bug for non free floating robots