Выберите и поместите Используя 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 13.060740 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 12.624720 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 8.949604 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

Для просмотра документации необходимо авторизоваться на сайте