Проверяйте на Манипулятор Сам Столкновения с помощью Сеток Столкновения
В этом примере показано, как проверять на манипулятор сам, столкновения с помощью столкновения сцепляются в исходной папке 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, чтобы проверить каждое тело по любому телу. Следующие определенные предположения рассматриваются здесь:
Тела не проверяются по соседним телам, поскольку они всегда находятся в контакте в соединении. Это принято, что объединенные пределы были бы установлены, чтобы препятствовать тому, чтобы тела столкнулись со своими ближайшими соседями
Все тела проверяются друг по другу только однажды (i.e. тело 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