Fix bug when robot gets stuck inside expanded obstacle