Планирование пути с использованием RRT для манипуляторов
path = plan(rrt,startConfig,goalConfig)rrt.
path = plan(rrt,startConfig,goalRegion)workspaceGoalRegion объект
Используйте 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 затравка для повторяемости.
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
Укажите область цели в рабочей области и запланируйте путь в пределах этих границ. workspaceGoalRegion объект определяет границы XYZ-положения и ориентации Euler ZYX концевого эффектора робота. manipulatorRRT объект планирует путь на основе этой области цели и выполняет выборку случайных поз в пределах границ.
Загрузка существующей модели робота в качестве rigidBodyTree объект.
robot = loadrobot("kinovaGen3", "DataFormat", "row"); ax = show(robot);
Создание планировщика путей
Создайте быстро исследуемый планировщик пути случайного дерева (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);
План Путь к Цели Регион
Запланируйте путь к области цели из домашней конфигурации робота. Из-за случайности в алгоритме 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
Настройка положения концевого эффектора
Обратите внимание, что рука робота подходит к рабочему пространству снизу. Чтобы развернуть ориентацию конечного положения, добавьте 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
