Сгенерируйте код для планирования движения Используя модель робота, импортированную из URDF

В этом примере показано, как выполнить генерацию кода, чтобы запланировать движение с помощью модели робота, импортированной из файла URDF. В данном примере вы используете manipulatorRRT объект с импортированным rigidBodyTree модель робота, чтобы найти путь без препятствий между двумя настройками робота. После того, как вы проверите алгоритм в MATLAB®, используйте сгенерированный файл MEX в алгоритме, чтобы визуализировать перемещение робота.

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

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

Figure contains an axes. The axes contains 60 objects of type patch, line. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

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

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

Figure contains an axes. The axes contains 31 objects of type patch, line. This object represents world.

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

Можно использовать любого 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

Figure contains an axes. The axes contains 31 objects of type patch, line. This object represents world.

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