Выберите и поместите Используя RRT для манипуляторов
Используя манипуляторы, чтобы выбрать и поместить объекты в среду может потребовать алгоритмов планирования пути как быстро исследующий случайный древовидный планировщик. Планировщик исследует на пробеле объединенной настройки и ищет путь без коллизий между различными настройками робота. В этом примере показано, как использовать manipulatorRRT возразите, чтобы настроить параметры планировщика и запланировать путь между двумя объединенными настройками на основе rigidBodyTree модель робота робота Panda Emika™ Франки. После настройки параметров планировщика манипулятор робота планирует путь, чтобы переместить банку от одного места до другого.
Загрузите модель робота и среду
Загрузите робота и его среду с помощью exampleHelperLoadPickAndPlaceRRT функция. Функциональные выходные параметры три переменные:
franka— Модель робота Panda Emika Франки как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 для результатов repeatabile.
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 and 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
Присоедините в-конец-исполнительный-элемент банки
После настройки планировщика для желаемого поведения следуйте за pick-place рабочим процессом, куда робот перемещает объект через среду. Этот пример присоединяет цилиндр, или может, в-конец-исполнительный-элемент робота, и перемещает его в новое местоположение. Для каждой настройки планировщик проверяет на столкновения с цилиндрической 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
