Алгоритм записи в путь планирования

Создайте функцию, iiwaPathPlanner, который использует manipulatorRRT объект для планирования пути между двумя конфигурациями для модели робота KUKA LBR iiwa 14 в среде, заполненной препятствием.

function path = iiwaPathPlanner(startConfig, goalConfig, collisionDims, collisionPoses)
    %#codegen
    robot = iiwaForCodegen('row');
    collisionObjects = cell(1,length(collisionDims));
    for i=1:length(collisionDims)
        switch length(collisionDims{i})
            case 1
                sphereRadius = collisionDims{i};
                collisionObjects{i} = collisionSphere(sphereRadius{1});
                collisionObjects{i}.Pose = collisionPoses{i};
            case 2
                cylinderDims = collisionDims{i};
                collisionObjects{i} = collisionCylinder(cylinderDims{1}, cylinderDims{2});
                collisionObjects{i}.Pose = collisionPoses{i};
            case 3
                boxDims = collisionDims{i};
                collisionObjects{i} = collisionBox(boxDims{1}, boxDims{2}, boxDims{3});
                collisionObjects{i}.Pose = collisionPoses{i};
        end
    end

    planner = manipulatorRRT(robot, collisionObjects);
    path = plan(planner, startConfig, goalConfig);
end

Алгоритм действует как оболочка для стандартного вызова планирования движения RRT. Он принимает стандартные входные данные и возвращает набор векторов конфигурации робота в качестве пути. Поскольку нельзя использовать объекты-дескрипторы в качестве входных или выходных данных для функций, поддерживаемых для генерации кода. Загрузите робота внутрь функции. Сохранить iiwaPathPlanner в текущей папке.

Проверка алгоритма планирования движения в MATLAB

Перед созданием кода проверьте алгоритм планирования движения в MATLAB.

Импорт модели робота KUKA LBR iiwa 14 в виде rigidBodyTree объект. Задайте формат данных как "row".

robot = importrobot('iiwa14.urdf');
robot.DataFormat = 'row';

Создать rigidBodyTree функция генерации кода для модели робота.

writeAsFunction(robot,'iiwaForCodegen');

Создайте простую среду с препятствиями, используя примитивы столкновений.

env = {collisionBox(0.5, 0.5, 0.05),collisionSphere(0.15)};
env{1}.Pose = trvec2tform([0.35 0.35 0.3]);
env{2}.Pose = trvec2tform([-0.25 0.1 0.6]);

Определите конфигурацию начала и цели. Необходимо указать начало и цель, которые не перекрываются с препятствиями. В противном случае плановик выдает ошибку.

startConfig = robot.homeConfiguration;
goalConfig = [-2.9236 1.8125 0.6484 1.6414 -0.4698 -0.4181 0.3295];

Визуализация начального и конечного положений робота.

figure; 
show(robot,startConfig); 
hold all; 
show(robot,goalConfig);
show(env{1});
show(env{2});

Извлеките данные о столкновениях из среды. Несмотря на то, что данные коллизий можно извлечь с помощью кода внутри функции, извлечение данных таким образом удобно для создания кода, поскольку можно изменять объекты коллизий без необходимости регенерации кода.

collisionDims = {{env{1}.X env{1}.Y env{1}.Z},{env{2}.Radius}};
collisionPoses = cellfun(@(x)(x.Pose),env,'UniformOutput',false);

Запланируйте путь.

path = iiwaPathPlanner(startConfig,goalConfig,collisionDims,collisionPoses);

Визуализируйте робота. Установите 'FastUpdate' вариант show метод для true для получения гладкой анимации.

figure;
ax = show(robot,startConfig);
hold all
show(env{1},'Parent',ax);
show(env{2},'Parent',ax);
rrt = manipulatorRRT(robot,env);
interpPath = interpolate(rrt,path);
for i = 1:size(interpPath,1)
    show(robot,interpPath(i,:),'PreservePlot',false,'FastUpdate',true);
    drawnow;
end

Создание кода для алгоритма планирования движения

Вы можете использовать либо codegen (Кодер MATLAB) или приложение Кодер MATLAB (Кодер MATLAB) для создания кода. В этом примере создайте файл MEX путем вызова codegen в командной строке MATLAB. Укажите примеры входных аргументов для каждого ввода в функцию с помощью -args входной аргумент.

Позвоните в codegen и укажите входные аргументы в массиве ячеек. Эта функция создает отдельный iiwaPathPlanner_mex функция для использования. Можно также создать код C с помощью входного аргумента опций. Этот шаг может занять некоторое время.

codegen iiwaPathPlanner -args {startConfig,goalConfig,collisionDims,collisionPoses}

Code generation successful.

Проверка результатов с использованием сгенерированной функции MEX

Запланируйте путь путем вызова MEX-версии алгоритма планирования движения для указанных конфигураций начала и цели и данных коллизий из среды.

path = iiwaPathPlanner_mex(startConfig,goalConfig,collisionDims,collisionPoses);

Визуализация робота с конфигурациями робота, вычисленными с использованием MEX-версии алгоритма планирования движения. Установите 'FastUpdate' вариант show метод для true для получения гладкой анимации.

figure;
ax = show(robot, startConfig);
hold all
show(env{1},'Parent',ax);
show(env{2},'Parent',ax);

interpPath = interpolate(rrt,path);
for i = 1:size(interpPath,1)
    show(robot,interpPath(i,:),'PreservePlot',false,'FastUpdate',true);
    drawnow;
end