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

Создайте быстро исследуемый планировщик пути случайного дерева (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