Использование манипуляторов для выбора и размещения объектов в окружении может потребовать алгоритмов планирования пути, таких как быстро исследуемый случайный планировщик дерева. Планировщик исследует в пространстве конфигурации соединений и ищет путь без столкновения между различными строениями робота. В этом примере показано, как использовать manipulatorRRT объект, чтобы настроить параметры планировщика и планировать путь между двумя строениями соединений на основе rigidBodyTree Робот модель Emika™ Франки робота Панды. После настройки параметров планировщика, манипулятор робота планирует путь для перемещения банки из одного места в другое.
Загрузите робота и его окружение с помощью 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
После настройки планировщика на желаемое поведение следуйте рабочему процессу выбора и размещения, где робот перемещает объект через окружение. Этот пример присоединяет цилиндр, или может, к эффектору конца робота и перемещает его в новое место. Для каждого строения планировщик проверяет также столкновения с mesh гидроцилиндра.
% 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
