manipulatorRRT

Запланируйте движение дерево твердого тела использование двунаправленного RRT

расширьте все на странице

Описание

manipulatorRRT объект является планировщиком единого запроса для рук манипулятора, который использует двунаправленные быстро исследующие случайные деревья (RRT) алгоритм с дополнительной эвристикой подключения, чтобы потенциально увеличить скорость.

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

Для более жадного поиска, включая свойство EnableConnectHeuristic отключает предел на MaxConnectionDistance свойство при соединении между этими двумя деревьями.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is true, the connection steps are not limited by the max connection distance.

Установка EnableConnectHueristic свойство к false ограничивает дополнительное расстояние при соединении между этими двумя деревьями со значением MaxConnectionDistance свойство.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is false, the connection steps are limited by the max connection distance.

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

Чтобы запланировать путь между запуском и целевой настройкой, используйте plan объектная функция. После планирования можно интерполировать состояния вдоль пути с помощью interpolate объектная функция. Чтобы попытаться сократить путь путем обрезки ребер, используйте shorten объектная функция.

Чтобы задать область к демонстрационным положениям исполнительного элемента конца около целевой настройки, создайте workspaceGoalRegion возразите и задайте его как goalRegion введите к plan объектная функция. Чтобы изменить вероятность выборки дополнительных целевых настроек, задайте свойство WorkspaceGoalRegionBias.

Для получения дополнительной информации о вычислительной сложности, смотрите Сложность Планирования.

Создание

Синтаксис

rrt = manipulatorRRT(robotRBT,{})
rrt = manipulatorRRT(robotRBT,collisionObjects)

Описание

пример

rrt = manipulatorRRT(robotRBT,{}) создает двунаправленного планировщика RRT для заданного rigidBodyTree модель робота. Массив пустой ячейки указывает, что нет никаких препятствий в среде.

rrt = manipulatorRRT(robotRBT,collisionObjects) создает планировщика для модели робота с объектами столкновения, помещенными в среду. Планировщик проверяет на столкновения с этими объектами.

Свойства

развернуть все

Максимальная длина между запланированными настройками в виде положительной скалярной величины. Объект вычисляет продолжительность движения как Евклидово расстояние между двумя объединенными настройками. Во время дополнительного процесса это - максимальное расстояние, которое может изменить настройка.

Когда шарнирные соединения имеют бесконечные пределы, разности между двумя объединенными положениями вычислены с помощью angdiff функция.

Если EnableConnectheuristic свойство установлено в true, объект игнорирует это расстояние при соединении этих двух деревьев во время этапа подключения.

Типы данных: double

Разрешающая способность по дальности для проверки движения между настройками в виде положительной скалярной величины. Расстояние валидации определяет количество интерполированных узлов между двумя смежными узлами в дереве. Объект подтверждает каждый интерполированный узел путем проверки на столкновения с роботом и средой.

Типы данных: double

Максимальное количество случайных настроек сгенерировано в виде положительного целого числа.

Типы данных: double

Непосредственно соедините деревья во время фазы connect планировщика в виде логического 1 TRUE) или 0 ложь). Установка этого свойства к true вызывает объект проигнорировать MaxConnectionDistance свойство при попытке соединить эти два дерева вместе.

Типы данных: логический

Вероятность, чтобы произвести целевое состояние из области цели рабочей области в виде положительного значения в области значений [0,1). Смещение задает вероятность, чтобы добавить дополнительные целевые состояния в дерево от workspaceGoalRegion объект. Когда это значение обнуляется, workspaceGoalRegion возразите все еще производит одну цель для планировщика запланировать к.

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

Зависимость

Необходимо использовать goalRegion введите при вызове plan объектная функция.

Типы данных: double

Проигнорируйте сам столкновения во время планирования в виде логического. Если это свойство установлено в true, plan функционируйте проверка пропусков между телами для столкновений, и только сравнивает тела со средой. Не проверяя на самостолкновения, можно улучшить скорость стадии планирования.

Типы данных: логический

Функции объекта

planЗапланируйте путь с помощью RRT для манипуляторов
interpolateИнтерполируйте состояния вдоль пути от RRT
shortenРебра для обрезки, чтобы сократить путь от RRT

Примеры

свернуть все

Скрипт Open Live Script

Используйте manipulatorRRT возразите, чтобы запланировать путь модель робота дерева твердого тела в среде с препятствиями. Визуализируйте запланированный путь с интерполированными состояниями.

Загрузите модель робота в рабочую область. Используйте LBR KUKA рука манипулятора iiwa©.

robot = loadrobot("kukaIiwa14","DataFormat","row");

Сгенерируйте среду для робота. Создайте объекты столкновения и задайте их положения относительно основы робота. Визуализируйте среду.

env = {collisionBox(0.5, 0.5, 0.05) collisionSphere(0.3)};
env{1}.Pose(3, end) = -0.05;
env{2}.Pose(1:3, end) = [0.1 0.2 0.8];

show(robot);
hold on
show(env{1})
show(env{2})

Figure contains an axes object. The axes object contains 31 objects of type patch, line. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

Создайте планировщика RRT для модели робота.

rrt = manipulatorRRT(robot,env);

Задайте запуск и целевую настройку.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Запланируйте путь. Из-за случайности алгоритма RRT, набор rng отберите для воспроизводимости.

rng(0)
path = plan(rrt,startConfig,goalConfig);

Визуализируйте путь. Чтобы добавить больше промежуточных состояний, интерполируйте путь. По умолчанию, interpolate возразите, что функция использует значение ValidationDistance свойство определить количество промежуточных состояний. for цикл показывает каждый 20-й элемент интерполированного пути.

interpPath = interpolate(rrt,path);
clf
for i = 1:20:size(interpPath,1)
    show(robot,interpPath(i,:));
    hold on
end
show(env{1})
show(env{2})
hold off

Figure contains an axes object. The axes object contains 437 objects of type patch, line. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

Скрипт Open Live Script

Задайте целевую область в своей рабочей области и запланируйте путь в тех границах. workspaceGoalRegion объект задает границы на XYZ-позиционной и Эйлеровой ориентации ZYX исполнительного элемента конца робота. manipulatorRRT возразите планирует путь на основе той целевой области и выборок случайные положения в границах.

Загрузите существующую модель робота как rigidBodyTree объект.

robot = loadrobot("kinovaGen3", "DataFormat", "row");
ax = show(robot);

Figure contains an axes object. The axes object contains 25 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Создайте планировщика пути

Создайте планировщика пути к быстро исследующему случайному дереву (RRT) для робота. Этот пример использует пустую среду, но этот рабочий процесс также работает хорошо с нарушенными средами. Можно добавить, что столкновение возражает против среды как collisionBox или collisionMesh объект.

planner = manipulatorRRT(robot,{});

Задайте целевую область

Создайте область цели рабочей области с помощью имени корпуса исполнительного элемента конца робота.

Задайте целевые параметры области для своей рабочей области. Целевая область включает ссылочное положение, XYZ-позиционные границы и пределы ориентации на Углах Эйлера ZYX. Этот пример задает границы на XY-плоскости в метрах и позволяет вращение вокруг оси Z в радианах.

goalRegion = workspaceGoalRegion(robot.BodyNames{end}); 
goalRegion.ReferencePose = trvec2tform([0.5 0.5 0.2]);
goalRegion.Bounds(1, :) = [-0.2 0.2];    % X Bounds
goalRegion.Bounds(2, :) = [-0.2 0.2];    % Y Bounds
goalRegion.Bounds(4, :) = [-pi/2 pi/2];  % Rotation about the Z-axis

Можно также применить фиксированное смещение ко всем положениям, произведенным в области. Это смещение может составлять схватывание инструментов или изменений размерностей в вашей рабочей области. В данном примере примените фиксированное преобразование, которое помещает исполнительный элемент конца на 5 см выше рабочей области.

goalRegion.EndEffectorOffsetPose = trvec2tform([0 0 0.05]);
hold on
show(goalRegion);

Figure contains an axes object. The axes object contains 35 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Запланируйте путь к целевой области

Запланируйте путь к целевой области от домашней настройки робота. Из-за случайности в алгоритме RRT, этот пример устанавливает rng отберите, чтобы гарантировать повторяемые результаты.

rng(0)
path = plan(planner,homeConfiguration(robot),goalRegion);

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

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations)
    show(robot,interpConfigurations(i,:),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1],...
        'CameraViewAngle',5)
  
    drawnow
end
hold off

Figure contains an axes object. The axes object contains 35 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Настройте положение исполнительного элемента конца

Заметьте, что манипулятор приближается к рабочей области от нижней части. Чтобы инвертировать ориентацию конечного положения, добавьте pi вращение к Оси Y для ссылочного положения. 

goalRegion.EndEffectorOffsetPose = ... 
    goalRegion.EndEffectorOffsetPose*eul2tform([0 pi 0],"ZYX");

Повторно запланируйте путь и визуализируйте движение робота снова. Робот теперь приближается от верхней части.

hold on
show(goalRegion);
path = plan(planner,homeConfiguration(robot),goalRegion);

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations)
    show(robot, interpConfigurations(i, :),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1])
    drawnow;
end
hold off

Figure contains an axes object. The axes object contains 45 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Советы

Планирование сложности

  • При планировании движения между узлами в дереве набор настроек сгенерирован и подтвержден. Это время вычисления планировщика пропорционально количеству сгенерированных настроек. Количеством настроек между узлами управляет отношение свойств MaxConnectionDistance и ValidationDistance. Чтобы улучшить время планирования, рассмотрите увеличение расстояния валидации или уменьшение макс. расстояния связи.

  • Проверка каждой настройки имеет сложность O (mn+m2), где m является количеством тел столкновения в rigidBodyTree объект и n являются количеством объектов столкновения в worldObjects. Используя большие количества сеток, чтобы представлять вашего робота или среду увеличивает время для проверки каждой настройки.

Пределы соединения Бога

  • Если ваш rigidBodyTree модель робота имеет объединенные пределы, которые имеют бесконечную область значений (например, шарнирное соединение с пределами [-Inf Inf]), manipulatorRRT возразите использует пределы [-1e10 1e10] выполнять универсальную случайную выборку в объединенных пределах.

Ссылки

[1] Kuffner, J. J. и С. М. Лэвалл. “RRT-подключение: Эффективный Подход к Планированию пути Единого запроса”. В Продолжениях 2000 ICRA. Конференция тысячелетия. Международная конференция IEEE по вопросам Робототехники и Автоматизации. Продолжения симпозиумов (CAT. № 00CH37065), 2:995–1001. Сан-Франциско, CA, США: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Смотрите также

Объекты

Функции

Темы

Введенный в R2020b

Документация Robotics System Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте