plan

Планируйте путь, используя RRT для манипуляторов

Описание

пример

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

пример

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

[path,solnInfo] = plan(___) также возвращает информацию о решении результатов из планировщика RRT с помощью предыдущих входных параметров.

Примеры

свернуть все

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

Загрузите модель робота в рабочую область. Используйте руку манипулятора KUKA LBR 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})

Создайте планировщик 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 seed для повторяемости.

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

Визуализируйте путь. Чтобы добавить больше промежуточных состояний, интерполируйте путь. По умолчанию в interpolate объект использует значение ValidationDistance свойство для определения количества промежуточных состояний. The 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

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

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

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

Figure contains an axes. The axes 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. The axes 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 seed для обеспечения повторяемых результатов.

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. The axes 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.

Настройте положение End-Effector

Заметьте, что рука робота приближается к рабочей области снизу. Чтобы развернуть ориентацию конечного положения, добавьте 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. The axes 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 объект.

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

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

свернуть все

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

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

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

  • IsPathFound - Логический параметр, указывающий, был ли найден путь

  • ExitFlag - целое число, указывающее, почему плановик завершил работу:

    • 1 - Достигнуто строение цели

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

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

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

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

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