Напишите алгоритм, чтобы запланировать путь

Создайте функцию, 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 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