Использование манипуляторов для выбора и размещения объектов в среде может потребовать алгоритмов планирования путей, таких как быстро исследуемый планировщик случайных деревьев. Планировщик исследует пространство совместной конфигурации и ищет путь без столкновений между различными конфигурациями роботов. В этом примере показано, как использовать manipulatorRRT объект для настройки параметров планировщика и планирования пути между двумя конфигурациями соединения на основе rigidBodyTree модель робота Franka Emika™ Panda. После настройки параметров планировщика робот-манипулятор планирует траекторию перемещения банки из одного места в другое.
Загрузка робота и его среды с помощью exampleHelperLoadPickAndPlaceRRT функция. Функция выводит три переменные:
franka - Модель робота Franka Emika Panda в качестве rigidBodyTree объект. Модель была изменена, чтобы удалить некоторые смежные сети столкновений, которые всегда находятся в столкновении, и скорректировать пределы положения на основе осуществимости.
config - Начальная конфигурация положений соединения для робота.
env - набор объектов-коллизий в виде массива ячеек, представляющих среду робота. Планировщик путей проверяет наличие собственных коллизий и коллизий с этой средой.
[franka,config,env] = exampleHelperLoadPickAndPlaceRRT;
Визуализация сетей столкновения модели робота и объектов среды.
figure("Name","Pick and Place Using RRT",... "Units","normalized",... "OuterPosition",[0, 0, 1, 1],... "Visible","on"); show(franka,config,"Visuals","off","Collisions","on"); hold on for i = 1:length(env) show(env{i}); end
Создайте планировщик путей RRT и укажите модель робота и среду. Определите некоторые параметры, которые будут настроены позже, и укажите конфигурацию начала и цели для робота.
planner = manipulatorRRT(franka, env); planner.MaxConnectionDistance = 0.3; planner.ValidationDistance = 0.1; startConfig = config; goalConfig = [0.2371 -0.0200 0.0542 -2.2272 0.0013 2.2072 -0.9670 0.0400 0.0400];
Запланируйте путь между конфигурациями. Планировщик RRT должен создать быстро исследуемое дерево случайных конфигураций для исследования пространства и в конечном итоге вернуть путь без столкновений через среду. Перед планированием сбросьте генератор случайных чисел MATLAB для повторяющихся результатов.
rng('default');
path = plan(planner,startConfig,goalConfig);Чтобы визуализировать весь контур, выполните интерполяцию контура на небольшие шаги. По умолчанию interpolate генерирует все конфигурации, которые были проверены на выполнимость (проверка коллизий) на основе ValidationDistance собственность.
interpStates = interpolate(planner, path); for i = 1:2:size(interpStates,1) show(franka, interpStates(i,:),... "PreservePlot", false,... "Visuals","off",... "Collisions","on"); title("Plan 1: MaxConnectionDistance = 0.3") drawnow; end
Настройте планировщик пути путем изменения MaxConnectionDistance, ValidationDistance, EnableConnectHeuristic свойства на панели planner объект.
Установка MaxConnectionDistance свойство до большего значения вызывает более длинные движения в запланированном пути, но также позволяет планировщику жадно исследовать пространство. Использовать tic и toc функции во времени plan для справки о том, как эти параметры могут влиять на время выполнения.
planner.MaxConnectionDistance = 5; tic path = plan(planner,startConfig,goalConfig); toc
Elapsed time is 11.217983 seconds.
Обратите внимание на изменение траектории. Рука робота качается намного выше из-за большего расстояния соединения.
interpStates = interpolate(planner, path); for i = 1:2:size(interpStates, 1) show(franka,interpStates(i,:),... "PreservePlot",false,... "Visuals","off",... "Collisions","on"); title("Plan 2: MaxConnectionDistance = 5") drawnow; end
Установка ValidationDistance до меньшего значения позволяет более точно проверить движение вдоль кромки в запланированном пути. Увеличение количества конфигураций, подлежащих проверке по пути, приводит к увеличению времени планирования. Меньшее значение полезно в случае загроможденной среды с большим количеством объектов столкновения. Из-за малого расстояния проверки, interpStates имеет большее количество элементов. Для более быстрой визуализации for цикл пропускает больше состояний на этом шаге для более быстрой визуализации.
planner.MaxConnectionDistance = 0.3; planner.ValidationDistance = 0.01; tic path = plan(planner,startConfig,goalConfig); toc
Elapsed time is 11.115833 seconds.
interpStates = interpolate(planner,path); for i = 1:10:size(interpStates,1) show(franka, interpStates(i,:),... "PreservePlot",false,... "Visuals","off",... "Collisions","on"); title("Plan 3: ValidationDistance = 0.01") drawnow; end
Эвристика соединения позволяет плановику жадно присоединяться к начальному и целевому деревьям. В местах, где среда менее загромождена, эта эвристика полезна для более короткого времени планирования. Однако жадное поведение в загроможденной среде приводит к бесполезным попыткам подключения. Установка EnableConnectHeuristic кому false может дать более длительное время планирования и более длинные пути, но приводит к более высокой частоте успешного поиска пути, учитывая количество итераций.
planner.ValidationDistance = 0.1; planner.EnableConnectHeuristic = false; tic path = plan(planner,startConfig,goalConfig); toc
Elapsed time is 7.556635 seconds.
interpStates = interpolate(planner,path); for i = 1:2:size(interpStates,1) show(franka, interpStates(i,:), ... "PreservePlot",false,... "Visuals","off",... "Collisions","on"); title("Plan 4: EnableConnectHeuristic = false") drawnow; end
После настройки планировщика на требуемое поведение выполните процедуру выбора и размещения, в которой робот перемещает объект по среде. В этом примере цилиндр или цилиндр может быть присоединен к концевому эффектору робота и перемещен в новое положение. Для каждой конфигурации плановик также проверяет наличие столкновений с сеткой цилиндра.
% Create can as a rigid body cylinder1 = env{3}; canBody = rigidBody("myCan"); canJoint = rigidBodyJoint("canJoint"); % Get current pose of the robot hand. startConfig = path(end, :); endEffectorPose = getTransform(franka,startConfig,"panda_hand"); % Place can into the end effector gripper. setFixedTransform(canJoint,endEffectorPose\cylinder1.Pose); % Add collision geometry to rigid body. addCollision(canBody,cylinder1,inv(cylinder1.Pose)); canBody.Joint = canJoint; % Add rigid body to robot model. addBody(franka,canBody,"panda_hand"); % Remove object from environment. env(3) = [];
После прикрепления банки к руке робота укажите конфигурацию цели для размещения объекта. Измените параметры плановика. Запланируйте путь от начала до цели. Визуализируйте путь. Обратите внимание, что баллон очищает стену.
goalConfig = [-0.6564 0.2885 -0.3187 -1.5941 0.1103 1.8678 -0.2344 0.04 0.04];
planner.MaxConnectionDistance = 1;
planner.ValidationDistance = 0.2;
planner.EnableConnectHeuristic = false;
path = plan(planner,startConfig,goalConfig);
interpStates = interpolate(planner,path);
hold off
show(franka,config,"Visuals","off","Collisions","on"); hold on for i = 1:length(env) show(env{i}); end for i = 1:size(interpStates,1) show(franka,interpStates(i,:),... "PreservePlot", false,... "Visuals","off",... "Collisions","on"); title("Plan 5: Place the Can") drawnow; if i == (size(interpStates,1)) view([80,7]) end end
Чтобы сократить путь, используйте shorten и укажите число итераций. Небольшое значение для ValidationDistance свойство в сочетании с большим количеством итераций может привести к большим временам вычисления.
shortenedPath = shorten(planner,path,20); interpStates = interpolate(planner,shortenedPath); for i = 1:size(interpStates,1) show(franka, ... interpStates(i, :), ... "PreservePlot", false, ... "Visuals", "off", ... "Collisions", "on"); drawnow; title("Plane 6: Shorten the Path") if i > (size(interpStates,1)-2) view([80,7]) end end hold off
