🛸 Гибридное планирование в MoveIt

Тема этого поста - гибридное планирование, для движения в динамически меняющейся обстановке. Предыдущие посты:
• Обзор архитектуры
• Кинематика
• Планирование движения

Стандартный подход к планированию движений робота в MoveIt, известный как "Sense-Plan-Act" (Восприятие-Планирование-Действие), работает надежно в статических, хорошо изученных средах. Робот сначала "осматривается", затем полностью просчитывает всю траекторию от начала до конца и, наконец, выполняет её без изменений.

Однако в реальном мире такой подход часто не работает. Задачи вроде подачи стакана воды движущемуся человеку или письма мелом на неровной доске требуют постоянной адаптации. Окружение может меняться, могут возникать непредвиденные препятствия. Для решения этой задачи MoveIt предлагае гибридное планирование.

Что такое гибридное планирование?

Идея гибридного планирования, уже хорошо зарекомендовавшая себя в навигационных системах, заключается в комбинации двух разнородных планировщиков, работающих параллельно: глобального и локального.
• Глобальный планировщик: его задача — решить общую задачу движения, построив полный маршрут от старта до цели. Он не ограничен по времени вычислений и использует сложные, полные алгоритмы (например, OMPL), чтобы найти оптимальный путь в общей картине мира. Он не обязан работать в реальном времени и предоставляет "генеральный план" движения.
• Локальный планировщик: этот планировщик работает непрерывно в цикле с высокой частотой в реальном времени. Его задача — следовать по общему маршруту, но при этом реагировать на изменения "на лету". Он похож на очень умный контроллер, способный решать локальные задачи:
- динамически уклоняться от внезапно появившихся препятствий,
- адаптировать траекторию к локальным условиям (например, поддерживать постоянное усилие прижима инструмента к неровной поверхности, используя данные с силовых датчиков),
- оптимизировать небольшой участок траектории, что вычислительно гораздо быстрее, чем пересчитывать весь путь.
способным работать в реальном времени.

Архитектура гибридного планирования

Система состоит из трех ключевых компонентов, которые общаются друг с другом через стандартные интерфейсы ROS 2:
• Hybrid Planning Manager: принимает основную задачу на выполнение, запускает глобальный планировщик для получения общего маршрута, а затем координирует работу локального планировщика, управляя процессом через систему событий. Например, получив от локального планировщика сигнал о препятствии, он может отдать команду глобальному на перепланирование.
• Global Planner Component: предоставляет интерфейс для запуска глобального планирования. Внутри него обычно работает стандартный конвейер планирования MoveIt. Он вычисляет траекторию и публикует ее для локального планировщика.
• Local Planner Component: Этот компонент принимает глобальную траекторию как ориентир. Внутри него работает быстрый решатель, который постоянно генерирует команды для контроллеров робота, следуя пути и одновременно адаптируясь к сенсорным данным и локальным ограничениям.

Как это работает на практике?

Представим, что робот начал движение по рассчитанному пути, но внезапно на его пути появляется препятствие:
1. локальный планировщик, постоянно проверяющий окружение, обнаруживает неминуемое столкновение;
2. он немедленно останавливает робота или замедляет его и посылает событие "обнаружено столкновение" в Hybrid Planning Manager;
3. Manager, согласно заложенной в него логике, реагирует на это событие, отправляя команду глобальному планировщику на перепланирование маршрута с учетом нового препятствия;
4. пока глобальный планировщик работает, робот безопасно ждет;
5. как только новый, свободный от столкновений маршрут готов, он отправляется локальному планировщику;
6. локальный планировщик плавно "встраивает" новый участок траектории в текущее движение и продолжает выполнение задачи.

Благодаря такой архитектуре робот становится способным эффективно работать в динамичных и непредсказуемых условиях реального мира. Пример из набора tutorial MoveIt.

#ros2 #moveit

⚛️ Ядро MoveIt - узел move_group

Предыдущие посты:
• Обзор архитектуры
• Кинематика
• Планирование движения
• Гибридное планирование

Рассмотрим центральный узел MoveIt - move_group. Его можно представить как интегратор, который собирает воедино все остальные части системы и предоставляеи стандартный интерфейс ROS (actions, services) для управления движениями робота.

Конфигурация

Для работы move_group необходима информация о роботе (URDF, SRDF), параметры планировщика и прочих модулей, хранящиеся в папке config.

Взаимодействие с роботом

• Состояние суставов: move_group подписывается на топик /joint_states, чтобы знать текущую конфигурацию робота в любой момент времени.

• Выполнение движения: move_group использует интерфейс FollowJointTrajectoryAction, где он выступает в роли клиента, передающего вычисленную траекторию в ros2_control.

• Данные с сенсоров: move_group получает данные с сенсоров робота, таких как 3D-камеры или лидары, что позволяет учитывать окружающую обстановку.

#ros2 #moveit

🗺 Сцена Планирования в MoveIt

Предыдущие посты:
• Обзор архитектуры
• Кинематика
• Планирование движения
• Гибридное планирование
• Центральный узел move_group

Чтобы робот мог безопасно двигаться, не сталкиваясь с препятствиями, ему необходимо иметь актуальное представление об окружающем мире. В MoveIt за это отвечает Сцена Планирования (Planning Scene).

Что такое Сцена Планирования?

Сцена Планирования - это объект в памяти, который хранит полную картину мира с точки зрения робота. Она включает в себя две основные части:
• Текущее состояние самого робота (положения всех его суставов).
• Геометрия окружающего мира, включая статические объекты и любые динамические препятствия.

Монитор Сцены Планирования (Planning Scene Monitor - PSM)

Поскольку к этой сцене могут обращаться несколько компонентов одновременно, для её безопасного обновления используется специальный менеджер - Монитор Сцены Планирования (PSM). Он обеспечивает потокобезопасный доступ для чтения и записи данных.

Ключевые узлы, такие как move_group и плагин визуализации в RViz, имеют свои собственные экземпляры PSM. move_group слушает топик /planning_scene для получения обновлений (например, добавления объектов пользователем) и публикует свою итоговую, собранную сцену в топик /monitored_planning_scene, на который, в свою очередь, подписан RViz для отображения.

Построение картины мира

Картина мира (геометрия окружения) строится с помощью Монитора Геометрии Мира (World Geometry Monitor). Он использует два источника данных:
• Данные с сенсоров робота (3D-камер, лидаров).
• Объекты, добавленные пользователем вручную (пример из моего пакета panda_gz_moveit).

Восприятие 3D-мира и Octomap

Обработка 3D-данных с сенсоров делегируется Монитору Карты Занятости (Occupancy Map Monitor). Этот компонент имеет плагинную архитектуру, что позволяет добавлять поддержку новых типов сенсоров. По умолчанию, в MoveIt встроены два плагина:

• PointCloudOccupancyMapUpdater: для обработки облаков точек (point clouds).
• DepthImageOccupancyMapUpdater: для обработки изображений глубины.

В основе Монитора Карты Занятости лежит Octomap. Это специальная древовидная структура данных для эффективного представления трехмерного пространства. Она делит мир на воксели (трехмерные пиксели) и для каждого хранит информацию о том, занят он, свободен или неизвестен. Octomap напрямую передается в библиотеку FCL (Flexible Collision Library), которую MoveIt использует для проверки столкновений.

Важной особенностью является самофильтрация. Например, плагин для обработки изображений глубины умеет "вычитать" из кадра видимые части самого робота, чтобы он случайно не принял свою руку за внешнее препятствие.

#ros2 #moveit

🏗 MoveIt Task Constructor: структурирование сложных задач

Завершаем погружение в архитектуру MoveIt. Предыдущие посты:
• Обзор архитектуры
• Кинематика
• Планирование движения
• Гибридное планирование
• Центральный узел move_group
• Сцена планирования

Для решения комплексных задач, таких как "взять объект со стола и положить его в коробку", простого планирования движения из точки А в точку Б недостаточно. Требуется разбить задачу на последовательность шагов: подойти, открыть захват, опуститься, схватить, поднять, переместиться к коробке, разжать захват. Именно для такого структурированного подхода и используется MoveIt Task Constructor (MTC). Он раскладывает сложную задачу на набор более простых, взаимосвязанных подзадач, которые называются "стадиями" (Stages). Эти стадии выстраиваются в иерархическую структуру, образуя полный план решения.

Стадии

Каждая стадия представляет собой определенный шаг в конвейере планирования. По тому, как они обрабатывают и передают информацию (решения), стадии делятся на три основных типа:

• Генераторы (Generators): создают исходные состояния и не зависят от соседних стадий. Они являются отправными точками для планирования.
Пример: cамая важная стадия-генератор CurrentState, которая просто получает текущее положение суставов робота. Другой пример - Generate Grasp Pose: генерация множества возможных поз для захвата объекта.

• Распространители (Propagators): получают решение от одной соседней стадии, выполняют свою подзадачу и распространяют результат дальше, на другую сторону. Они могут работать как "вперед" от начального состояния, так и "назад" от целевого.
Пример: cтадия относительного движения, такая как Move Relative, которая вычисляет траекторию подхода к объекту, начиная с уже известной позы робота.

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

Контейнеры: организация стадий

Чтобы управлять сложными иерархиями, стадии помещаются в контейнеры. Контейнеры сами являются стадиями и определяют логику их совместного выполнения.

• Последовательный контейнер (Serial Container): Организует стадии в линейную цепочку. Каждая следующая стадия начинается из состояния, в котором закончилась предыдущая. Весь процесс "взятия и переноса" объекта является классическим примером последовательного контейнера. По умолчанию, вся задача MTC — это один большой последовательный контейнер.

• Параллельный контейнер (Parallel Container): Позволяет рассматривать альтернативные решения или выполнять действия одновременно.
Пример 1 (Альтернативы): поместить в параллельный контейнер две подзадачи: "взять объект левой рукой" и "взять объект правой рукой". MTC найдет решение для обеих (если это возможно) и выберет лучшее (например, по длине траектории).
Пример 2 (Одновременность): одновременно планировать движение руки к цели и открытие захвата.

• Обертка (Wrapper): тип контейнера, который инкапсулирует одну дочернюю стадию, чтобы изменить или отфильтровать ее результаты.
Пример: стадия генерации поз захвата производит набор декартовых поз (положение + ориентация). Чтобы робот мог их достичь, нужны конкретные углы суставов. IK Wrapper (решатель обратной задачи кинематики) оборачивает стадию генерации поз и преобразует ее декартовы решения в углы суставов.

Оценка Стоимости Решений: CostTerm

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

Интерфейс CostTerm позволяет определять, как именно будет рассчитываться эта стоимость. Доступны различные реализации:
• Constant: присваивает каждому решению фиксированную, постоянную стоимость.
• PathLength: стоимость зависит от длины траектории в пространстве суставов. Чем длиннее путь, тем выше стоимость.
• TrajectoryDuration: стоимость основана на времени выполнения траектории.
• LinkMotion: стоимость зависит от длины пути, который проходит определенное звено робота.
• DistanceToReference: стоимость определяется тем, насколько конфигурация суставов далека от некой эталонной конфигурации.
• Clearance: стоимость обратно пропорциональна минимальному расстоянию до столкновения. Чем ближе робот к препятствию, тем выше стоимость.
• LambdaCostTerm: позволяет задать собственную функцию расчета стоимости с помощью лямбда-выражения.

Порядок решения задачи

1. Инициализация: cоздается объект Task, который является корневым контейнером.
2. Построение: в Task добавляются стадии и контейнеры, формируя полную логику задачи.
3. Настройка: для стадий задаются решатели (OMPL, CartesianPath) и, при необходимости, калькуляторы стоимости (CostTerm).
4. Планирование: запускается метод plan(). MTC проходит по всей иерархии стадий, генерируя и соединяя решения, и ищет сквозные пути с наименьшей итоговой стоимостью.
5. Выполнение: после нахождения успешного решения, оно передается контроллеру (ros2_control) для физического выполнения на роботе или симуляции.

Поиграться с конструктором задач можно в tutorial. Я, в свою очередь, также планирую сделать о нем ролик )

#ros2 #moveit

🧸 Друг, с которым можно поболтать обо всем

Взять мягкую игрушку, прикрутить к ней ChatGPT и немного заработать на ИИ для детей - что может пойти не так? Да, буквально, все 🤣

Беседа с мишкой или кактусом очень легко может перейти в плоскость вредных советов: где найти спички и ножи и как их пустить в дело, чтобы скоротать время в ожидании родителей. Или потолковать о БДСМ - тоже неплохой сценарий.

Двигаться, пить и курить как в фильме Ted подобные игрушки (пока) не могут, но по части поддержать разговор - то, что 13 лет назад выглядело как абсолютная фантастика, сегодня уже вполне доступно.

Понятно, что удержать детей в стороне от ИИ - тупиковый путь. Остается только разбираться самому и учить их не доверяться его советам, какими бы умными они не казались.