Мне нужно завершить проект по математическому моделированию

Я уже начал его

Тема проекта:

«Робот-пылесос: обход всего помещения при наличии двух-трёх препятствий простой геометрической формы (прямоугольники); дополнительно можно попытаться минимизировать перемещение по уже пройденной территории.»

Основные требования:

Движение робота осуществляется на основе модели с дифференциальным приводом. Каждое новое положение рассчитывается по заданным формулам (все необходимые формулы и теоретические материалы предоставлю в виде файлов и лекционных презентаций после согласования заказа).

Текущий прогресс:

На данный момент реализовано движение робота по «змейке» в пустой комнате на Python. Я начал добавлять препятствия, но столкнулся с трудностями на этом этапе.

Могу скинуть имеющийся у меня код