Создайте объекты столкновения для проверки столкновения манипулятора

Скрипт Open Live Script

Этот пример показывает три различных способа создать объекты столкновения для проверки столкновения манипулятора. Чтобы видеть больше всесторонних примеров, которые проверяют на самостолкновения или обнаружение столкновений среды, попробуйте следующие примеры:

Обзор проверки столкновения манипуляторы

checkCollisions функция является типовой к любому приложению или определенной настройке mesh. Это означает:

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

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

Массив столкновения твердого тела: массив ячеек 1x2 ячейки, где каждая ячейка имеет столкновение примитивное соответствие iтело th в векторном [base robot.Bodies] и преобразование, которое связывает тот источник объекта столкновения со связанным соединением твердого тела.
Мировой массив столкновения: массив ячеек мировых объектов столкновения.

Существует три предложенных способа инициализировать массив столкновения твердого тела:

Используя сетки, импортированные из известной директории.
Извлечение сеток от rigidBodyTree объект.
Используя примитивы столкновения, чтобы представлять вашу геометрию.

Метод 1: используйте сетки в известной директории

Много роботов идут с сетками столкновения, заданными в их файле Объединенного формата определения робота (URDF).

Робот IIWA идет с набором сеток столкновения, которые являются упрощенными версиями сеток визуализации. Вызовите importrobot сгенерировать rigidBodyTree объект из файла URDF. Установите выходной формат для настроек к "column".

iiwa = importrobot('iiwa14.urdf');
iiwa.DataFormat = 'column';

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

Если дерево твердого тела было создано, сгенерируйте набор сеток столкновения для каждой ссылки дерева твердого тела. Это будет достигнуто путем генерации массива ячеек n+1 для n тел в объекте rigidBodyTree. Дополнительный элемент хранит объект столкновения для основы.

collisionArrayFromMesh = cell(iiwa.NumBodies+1, 2);

Создайте rigidBodyTree возразите и задайте путь папки, содержащей сетки столкновения. Этот синтаксис сопоставил объекты твердого тела с файлами mesh STL на основе их указанного имени.

% Create a rigid body tree using the collision mesh path to associate rigid
% bodies with collision mesh STL files
collisionMeshPath = fullfile(matlabroot, 'toolbox', 'robotics', ...
        'robotexamples', 'robotmanip', 'data', 'iiwa_description', ...
        'meshes', 'iiwa14', 'collision');
iiwaCollision = importrobot('iiwa14.urdf','MeshPath', collisionMeshPath);
iiwaCollision.DataFormat = 'column';

Создайте определения mesh столкновения для каждого твердого тела из исходной директории:

Преобразуйте каждый файл STL в поверхности и вершины путем чтения STL: stldata = stlread(file.STL)
Создайте collisionMesh от vertices: mesh = collisionMesh(stldata.Points);
Присвойте mesh iтело th путем размещения его в (i+1)элемент th collisionArray массива ячеек. Основная mesh занимает первый элемент массива ячеек.

% For each body, read the corresponding STL file
robotBodies = [{iiwaCollision.Base} iiwaCollision.Bodies];
for i = 1:numel(robotBodies)
    if ~isempty(robotBodies{i}.Visuals)
        % Assumes the first Visuals element is the correct one.
        visualDetails = robotBodies{i}.Visuals{1};
        
        % Extract the part of the visual that actually specifies the STL name
        visualParts = strsplit(visualDetails, ':');
        stlFileName = visualParts{2};
        
        % Read the STL file
        stlData = stlread(fullfile(collisionMeshPath, stlFileName));
        
        % Create a collisionMesh object from the vertices
        collisionArrayFromMesh{i,1} = collisionMesh(stlData.Points);
        
        % Transform is always identity
        collisionArrayFromMesh{i,2} = eye(4);
    end
end

Проверяйте на самостолкновения в заданной настройке

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

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwaCollision, collisionArrayFromMesh, {}, config, true);
disp(isCollision)

Визуализируйте настройку робота и подсветку, сталкивающуюся тела с помощью exampleHelperHighlightCollisionBodies.

show(iiwa, config);
exampleHelperHighlightCollisionBodies(iiwaCollision, selfCollisionPairIdx, gca);

Метод 2: извлеките сетки из дерева твердого тела

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

collisionArrayFromVisuals = exampleHelperManipCollisionsFromVisuals(iiwa);

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwa, collisionArrayFromVisuals, {}, config, true);
disp(isCollision)

Визуализируйте робота и подсветите объекты в столкновении.

show(iiwa,config);
exampleHelperHighlightCollisionBodies(iiwa, selfCollisionPairIdx, gca);

Метод 3: задайте примитивы столкновения

Создайте примитивы столкновения, то использование упростило геометрию. Вручную задайте размерности. Этот метод обычно менее точен, чем использование более высоких определений mesh точности.

В данном примере используйте collisionCylinder объекты, которые имеют высоту и радиус. Задайте каждый размер твердого тела как массив.

dimensionArray = [...
    .1 0; ... % Base
    .15, 0.1575; ... % iiwa_link_0
    .12, 0.3; ... % iiwa_link_1
    .13, 0.3; ... % iiwa_link_2
    .1, 0.3; ... % iiwa_link_3
    .1, 0.25; ... % iiwa_link_4
    .1, 0.2155; ... % iiwa_link_5
    .08, 0.17; ... % iiwa_link_6
    .05, 0.06; ... % iiwa_link_7
    .01, 0; ... % iiwa_link_ee_kuka
    .01, 0;]; % iiwa_link_ee

Создайте массив столкновения из данных размерностей. Создайте collisionCylinder возразите и задайте преобразование между каждым на основе их конфигураций.

primitiveCollisionArray = { ...
    [] eye(4); ... %Base (world)
    collisionCylinder(dimensionArray(2,1), dimensionArray(2,2)) trvec2tform([0 0 dimensionArray(2,2)/2]); ... % iiwa_link_0
    collisionCylinder(dimensionArray(3,1), dimensionArray(3,2)) trvec2tform([0 0 dimensionArray(3,2)/2]); ... % iiwa_link_1
    collisionCylinder(dimensionArray(4,1), dimensionArray(4,2)) axang2tform([1 0 0 -pi/2])*trvec2tform([0 0 dimensionArray(4,2)/2]); ... % iiwa_link_2
    collisionCylinder(dimensionArray(5,1), dimensionArray(5,2)) trvec2tform([0 .025 dimensionArray(5,2)/2]); ... % iiwa_link_3
    collisionCylinder(dimensionArray(6,1), dimensionArray(6,2)) axang2tform([1 0 0 -pi/2])*trvec2tform([0 -.02 dimensionArray(6,2)/2]); ... % iiwa_link_4
    collisionCylinder(dimensionArray(7,1), dimensionArray(7,2)) trvec2tform([0 0 dimensionArray(7,2)/2]); ... % iiwa_link_5
    collisionCylinder(dimensionArray(8,1), dimensionArray(8,2)) axang2tform([1 0 0 -pi/2]); ... % iiwa_link_6
    collisionCylinder(dimensionArray(9,1), dimensionArray(9,2)) trvec2tform([0 0 dimensionArray(9,2)/2]); ... % iiwa_link_7
    [] eye(4); ... % iiwa_link_ee_kuka
    [] eye(4); ... % iiwa_link_ee
    };

Проверяйте на столкновения. Поскольку они менее точны, проверка столкновения возвращает больше столкновений в этом экземпляре.

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
show(iiwa, config);
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwa, primitiveCollisionArray, {}, config, true);
exampleHelperHighlightCollisionBodies(iiwa, selfCollisionPairIdx, gca);

Визуализируйте примитивы столкновения, чтобы показать, что они менее точны.

exampleHelperShowCollisionTree(iiwa, primitiveCollisionArray, config);
title('Collision Array from Collision Primitives')

Документация