Рассчитаем матрицу инерции в операционном пространстве для ближайшей точки
С помощью Якобиана для интересующей точки теперь можно вычислить матрицу инерции в операционном пространстве. Процедура уже работала раньше, она будет показана в полном коде ниже.
Полная реализация
Можно запустить пример всего этого кода, управляющего рукой UR5, чтобы обхода препятствий в VREP.
Единственная вещь в этом коде, что я не проговорил – место, где rho<.01, там устанавливается u=u_psp вместо добавления u_psp к u. Сделано для отказоустойчивости управления роботом, «если нужно сбить препятствие, то забудь про всё остальное и, всем уйти с дороги!».
Результаты
Мне действительно нравится, что получилось, когда это работает. Алгоритм реально эффективный. Давайте посмотрим на некоторые примеры его действий.
1. В 2D-среде, где действительно легко перемещаться вокруг препятствия, видно, как меняется реакция на новое положение препятствия.
Красный круг – цель, а синий круг – препятствие:
https://studywolf.files.wordpress.com/2016/11/avoid2d.gif
2. В 3D в VREP – пример кода, размещённый на github, реализующий это.
Пример его работы без кода обхода препятствий
https://studywolf.files.wordpress.com/2016/11/noavoid3d.gif
Пример с обходом препятствий.
https://studywolf.files.wordpress.com/2016/11/avoid3d.gif
Довольно трудно заметить, но в первом примере робот движется сквозь дальнюю сторону препятствия (золотая сфера) по пути к цели (красная сфера):
Последний пример: рука укорачивается от движущегося препятствия по пути к цели.
|
|
|
https://studywolf.files.wordpress.com/2016/11/movingavoid3d.gif
Реализация – это особое удовольствие. Этот изящный алгоритм работает довольно хорошо, и я не нашёл в статьях альтернатив, которые бы не затрагивали планирование путей (если знаете такие, можете поделиться в комментариях, было бы здорово!).
Эта статья была путешествием, и надеюсь, оказалась полезной.
Я продолжаю удивляться, находя разные интересные функции, подобные этой, и однажды, так получается, что Вы создали рабочий каркас системы управления в операционном пространстве.
Дата добавления: 2019-11-16; просмотров: 70; Мы поможем в написании вашей работы! |
Мы поможем в написании ваших работ!