🦾 Устройство MoveIt - Кинематика

Продолжаем разбираться с устройством фреймвока MoveIt, все посты серии:
• Обзор архитектуры

В этом посте речь пойдет про основу всего - расчет кинематики.

Что такое кинематика?

Если коротко, то кинематика описывает движение робота без учета сил, которые это движение вызывают. Она делится на два типа:
• Прямая кинематика (Forward Kinematics, FK): Позволяет определить положение и ориентацию конечного звена манипулятора (например, завата) в пространстве, зная углы поворота всех его суставов. Это относительно простая вычислительная задача.
• Обратная кинематика (Inverse Kinematics, IK): Это обратная задача — найти необходимые углы в суставах, чтобы конечное звено достигло нужного положения и ориентации. Эта задача намного сложнее, так как может иметь как одно, так и несколько решений или вообще ни одного.

Архитектура плагинов в MoveIt

MoveIt использует модульную архитектуру, которая позволяет легко подключать и использовать различные плагины с алгоритмами для решения задачи задач кинематики. Это дает возможность выбрать наиболее подходящий модуль (solver) под конкретного робота и задачу.

Настройка кинематики

По умолчанию в MoveIt применяется KDL Kinematics Plugin, использующий численные методы для решения обратной кинематической задачи (IK).

Конфигурация модуля кинематики производится в файле kinematics.yaml, который можно сгенирировать с помощью MoveIt Setup Assistant или сделать вручную. В этом файле указывается, какой решатель использовать для каждой группы планирования, а также задаются его параметры: таймаут, количество попыток и разрешение поиска - пример из моего последнего gazebo tutorial.

Проверка столкновений (Collision Checking)

В MoveIt проверка столкновений настраивается в объекте Planning Scene, для пользователя этот процесс в основном автоматизирован. Под капотом MoveIt использует библиотеку FCL (Flexible Collision Library) для выполнения вычислений.

MoveIt умеет работать со следующими типами объектов:
• Meshes: Сложные 3D-модели в форматах .stl или .dae, которые описывают звенья робота или объекты в его окружении.
• Primitive Shapes: Простые геометрические объекты, такие как кубы, цилиндры, сферы и плоскости.
• OctoMap: 3D-карта окружения, построенная с помощью сенсоров (например, 3D-камеры), которая может напрямую использоваться для обнаружения препятствий.

Матрица разрешенных столкновений (Allowed Collision Matrix, ACM)

Проверка столкновений — очень ресурсоемкая операция, занимающая до 90% времени при планировании движения. Чтобы оптимизировать этот процесс, используется ACM.

ACM — это, по сути, таблица, которая указывает, для каких пар объектов нужно, а для каких не нужно выполнять проверку на столкновение. Если для пары объектов проверка отключена (значение true), MoveIt будет их игнорировать. Это полезно в двух случаях:
• Объекты никогда не столкнутся: Например, основание робота и удаленная стена.
• Объекты находятся в контакте по умолчанию: Например, захват робота и зафиксированный на нем датчик (камера).

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

SRDF: Семантическое описание робота

Для настройки ACM используется файл SRDF с семантическим описанием робота (Semantic Robot Description Format) - пример для руки Panda. Его также можно сгенерировать с помощью MoveIt Setup Assistant. Если URDF (Unified Robot Description Format) описывает физику робота (его звенья, суставы, размеры), то SRDF описывает его семантику — то есть, как с этим роботом работать.

Файл SRDF дополняет URDF и содержит следующую информацию:
• Группы планирования: Например, "рука"или "захват".
• Именованные позы робота: Заранее определенные конфигурации, такие как home или ready.
• Описание исполнительных звеньев (End-Effectors): Определяет, какое звено является рабочим инструментом.
• ACM: С помощью тегов <disable_collisions> указываются пары звеньев, для которых проверка столкновений должна быть отключена.

Иначе говоря, URDF - это "скелет" робота, а SRDF - "инструкция по его применению".

#ros2 #moveit

🛣 Устройство MoveIt: планирование движения

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

OMPL: Абстрактный решатель

Весь фреймворк MoveIt построен по модульному принципу, поэтому для планирования движения могут использоваться различные плагины. Планировщиком по умолчанию является OMPL (Open Motion Planning Library) - это библиотека с открытым исходным кодом, которая содержит в себе множество алгоритмов для планирования движений, в основном, основанных на сэмплировании (рандомизированные планировщики).

Ключевая особенность OMPL - абстрактность. Сама по себе библиотека ничего не знает о роботах, их форме, суставах или о том, что такое коллизия. Она оперирует математическими концепциями: пространствами состояний, валидаторами состояний и целями.

Именно MoveIt выступает в роли связующего звена. Он "объясняет" OMPL, с чем та работает:
• Предоставляет модель робота, его кинематику и текущее состояние.
• Описывает окружающую среду и определяет, какие конфигурации робота приводят к столкновению.
• Формулирует задачу (целевую точку) на понятном для OMPL языке.

Таким образом, MoveIt конфигурирует OMPL и предоставляет ей всю необходимую информацию для решения конкретной задачи.

Запрос на движение (Motion Plan Request)

Все начинается с того, что мы отправляем в MoveIt запрос, который описывает, какое движение мы хотим получить от робота. Обычно это одна из двух задач:
• переместить суставы манипулятора в определенные положения (заданные в углах или радианах) (FK),
• переместить рабочий орган (например, захват) в конкретную точку пространства с нужной ориентацией (IK).

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

По умолчанию система всегда проверяет возможные столкновения — как с объектами окружения, так и с самим собой.

Результат: Путь и Траектория

В ответ на запрос move_group (ключевой узел MoveIt) генерирует траекторию, являющуюся не просто путем движения.

Путь (Path) - это всего лишь последовательность точек в пространстве, через которые должен пройти робот. Это чисто геометрическое понятие, как маршрут на карте.

Траектория (Trajectory) - это путь, дополненный временными параметрами. Она состоит из набора точек, где для каждой указаны не только положения суставов, но и скорости, ускорения и, самое главное, момент времени, к которому эта точка должна быть достигнута. Таким образом, траектория — это полноценный план движения, который учитывает динамические возможности робота.

Процесс превращения пути в траекторию называется временной параметризацией. MoveIt выполняет его с помощью специальных компонентов — адаптеров планирования.

Адаптеры планирования (Planning Request Adapters)

Прежде чем выполнить запрос, и после того как план сгенерирован, MoveIt обрабатывает данные с помощью адаптеров:
• AddTimeParameterization: как раз тот адаптер, что превращает "кинематический путь" от планировщика в полноценную траекторию, добавляя временные метки и рассчитывая скорости/ускорения.
• CheckStartStateBounds: если робот по какой-то причине начинает движение из положения, слегка выходящего за пределы лимитов сустава, этот адаптер "подвинет" начальную точку на границу допустимого диапазона.
• CheckStartStateCollision: если начальная позиция робота оказывается в состоянии столкновения, адаптер пытается найти рядом свободную от коллизий конфигурацию, немного "подергав" суставы.

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

#ros2 #moveit

🛸 Гибридное планирование в 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 лет назад выглядело как абсолютная фантастика, сегодня уже вполне доступно.

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