Выбор и размещение использования RRT для манипуляторов

Использование манипуляторов для выбора и размещения объектов в окружении может потребовать алгоритмов планирования пути, таких как быстро исследуемый случайный планировщик дерева. Планировщик исследует в пространстве конфигурации соединений и ищет путь без столкновения между различными строениями робота. В этом примере показано, как использовать 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

Присоедините банку к End-Effector

После настройки планировщика на желаемое поведение следуйте рабочему процессу выбора и размещения, где робот перемещает объект через окружение. Этот пример присоединяет цилиндр, или может, к эффектору конца робота и перемещает его в новое место. Для каждого строения планировщик проверяет также столкновения с 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

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