Планируйте путь, используя 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 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);
Создайте Планировщик Пути
Создайте быстро исследуемый планировщик пути (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 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
Настройте положение 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
