Запись алгоритма в путь плана

Создайте функцию, 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 как r igidBodyTree объект. Установите формат данных равным "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 Coder) или приложение MATLAB Coder (MATLAB Coder) для генерации кода. В данном примере сгенерируйте файл MEX путем вызова codegen в командной строке MATLAB. Задайте выборку входных параметров для каждого входа в функцию используя -args входной параметр.

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

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