plan

Запланируйте путь с помощью RRT для манипуляторов

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

Синтаксис

path = plan(rrt,startConfig,goalConfig)
path = plan(rrt,startConfig,goalRegion)
[path,solnInfo] = plan(___)

Описание

пример

path = plan(rrt,startConfig,goalConfig) планирует путь между заданным запуском и целевыми настройками с помощью манипулятора, быстро исследуя случайные деревья (RRT) планировщик rrt.

пример

path = plan(rrt,startConfig,goalRegion) планирует путь между заданным запуском и целевой областью как workspaceGoalRegion объект

[path,solnInfo] = plan(___) также возвращает информацию о решении о результатах планировщика 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.

Входные параметры

свернуть все

Манипулятор планировщик RRT в виде manipulatorRRT объект. Этот планировщик для определенной модели робота дерева твердого тела, сохраненной как rigidBodyTree объект.

Начальная настройка робота в виде n - вектор элемента из объединенных положений для rigidBodyTree объект сохранил в планировщике RRT rrt. n является количеством нефиксированных соединений в модели робота.

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

Желаемая настройка робота в виде n - вектор элемента из объединенных положений для rigidBodyTree объект сохранил в планировщике RRT rrt. n является количеством нефиксированных соединений в модели робота.

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

Область цели рабочей области в виде workspaceGoalRegion объект.

workspaceGoalRegion объект задает границы на положении исполнительного элемента конца и sample объектная функция возвращает случайные положения, чтобы добавить к дереву RRT. Установите свойство WorkspaceGoalRegionBias изменить вероятность выборки состояния в целевой области.

Выходные аргументы

свернуть все

Запланированный путь на объединенном пробеле, возвращенном как r-by-n матрица объединенных настроек, где r является количеством настроек в пути и n, является количеством нефиксированных соединений в rigidBodyTree модель робота.

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

Информация о решении от планировщика, возвращенного как структура с этими полями:

  • IsPathFound — Логическое указание, если путь был найден

  • ExitFlag — Целое число, указывающее, почему отключенный планировщик:

    • 1 — Целевая настройка достигнута

    • 2 — Максимальное количество итераций достигнуто

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

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

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

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

Объекты

Функции

Темы

Введенный в R2020b

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

Поддержка

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