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

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

Создайте представление робота KUKA IIWA

Создайте rigidBodyTree объект для KUKA® IIWA-14 последовательный манипулятор.

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

Создайте сетки столкновения для дерева твердого тела

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

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

Существует несколько способов заполнить этот массив. Создайте Объекты Столкновения для покрытий Проверки Столкновения Манипулятора три различных способа сгенерировать объекты столкновения для ссылок в дереве твердого тела. Поскольку манипулятор IIWA обеспечивает определенные сетки столкновения в директории URDF, этот пример создает сетки столкновения из информации в пути к mesh.

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

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)
        % As a simplifying assumption, assume that the first visual is the 
        % only associated collision mesh.
        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
        collisionArray{i,1} = collisionMesh(stlData.Points);
        
        % For imported meshes, the origin of the imported mesh and the
        % rigid body origin are the same
        collisionArray{i,2} = eye(4);
    end
end

Сгенерируйте траекторию и проверку на столкновения

Задайте запуск и настройку конца и найдите объединенную пространственную траекторию, которая соединяет их:

% Define collision-free start & goal configurations
startConfig = [0 -pi/4 pi 3*pi/2 0 -pi/2 pi/8]';
goalConfig = [0 -pi/4 pi 3*pi/4 0 -pi/2 pi/8]';

% Define a trapezoidal velocity trajectory between the two configurations
q = trapveltraj([startConfig goalConfig],100,'EndTime',3);

Проверять эту выходную траекторию не содержит самостолкновения, выполняет итерации по выходным выборкам и видит, находятся ли какие-либо точки в столкновении. Проверка столкновения выполняется с помощью помощника в качестве примера, который использует два цикла for, чтобы проверить каждое тело по любому телу. Следующие определенные предположения рассматриваются здесь:

Тела не проверяются по соседним телам, поскольку они всегда находятся в контакте в соединении. Это принято, что объединенные пределы были бы установлены, чтобы препятствовать тому, чтобы тела столкнулись со своими ближайшими соседями
Все тела проверяются друг по другу только однажды (т.е. тело 1 проверяется на столкновение с телом 5, но не наоборот).

Когда робот имеет сам столкновения и m-2 матрица пар столкновений, collisionPairIdx, пример выводит флаг, isInCollision, как верный. Элементы этой матрицы являются индексами, которые сопоставляют столкновения с телами в связанном collisionArray массив ячеек сеток столкновения.

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    [isConfigInCollision(i),configCollisionPairs{i}] = exampleHelperManipCheckSelfCollisions(iiwaCollision,collisionArray,q(:,i),false);
end

Запланированная траектория проходит серию столкновений. Визуализируйте настройку, где первое столкновение происходит, и подсветите тела.

any(isConfigInCollision)

ans = logical
   1

firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that causes problems
figure;
show(iiwaCollision,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwaCollision,configCollisionPairs{firstCollisionIdx},gca);

Сгенерируйте траекторию без коллизий

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

Сгенерируйте новую траекторию и проверку на столкновения снова. Чтобы визуализировать части траектории, не прокомментируйте show вызов функции.

newStartConfig = wrapToPi(startConfig);
q = trapveltraj([newStartConfig goalConfig],100,'EndTime',3);

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    [isConfigInCollision(i),configCollisionPairs{i}] = exampleHelperManipCheckSelfCollisions(iiwaCollision,collisionArray,q(:,i),false);
%     if rem(i,10) == 0
%         show(iiwaCollision,q(:,i));
%         drawnow
%     end
end

После проверки целой траектории не найдены никакие столкновения.

any(isConfigInCollision)

ans = logical
   0

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