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

Импортируйте файл URDF KUKA® IIWA-14 последовательный манипулятор как rigidBodyTree модель. URDF получает файлы mesh столкновения для твердых тел в роботе. Индивидуально добавить столкновение возражает против твердого тела, можно использовать addCollision функция. Чтобы загрузить модель робота, которой предоставляют, с присоединенными объектами столкновения, смотрите loadrobot функция.

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

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

Задайте настройку начала и конца в виде набора объединенных положений. Эти положения должны быть свободным столкновением.

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]';

Найдите объединенную пространственную траекторию, которая соединяет эти две настройки в течение трех секунд.

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

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

При итерации через траекторию q, вызовите checkCollision функция на каждой настройке в траектории. Включите исчерпывающую проверку, чтобы продолжить проверять на столкновения после того, как первое будет обнаружено.

isConfigInCollision переменная отслеживает состояние столкновения каждой настройки. sepDistForConfig отслеживает разделительное расстояние между корпусами робота. Для каждого столкновения пара индексов тела хранится в configCollisionPairs переменная. Обратите внимание на то, что соседние тела не проверяются, как они всегда находятся в контакте через соединение, которое соединяет их.

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
sepDistForConfig = zeros(iiwa.NumBodies+1,iiwa.NumBodies+1,100);
for i = 1:length(q)
    [isColliding, sepDist] = checkCollision(iiwa,q(:,i),'Exhaustive','on');
    isConfigInCollision(i) = isColliding;
    sepDistForConfig(:,:,i) = sepDist;
end

Чтобы узнать индексы тел в столкновении, найдите который записи в sepDistForConfig NaN. septDist симметрическая матрица, таким образом, то же значение возвращено в индексах с инвертированными индексами. Упростите список при помощи unique.

for i = 1:length(q)
    sepDist = sepDistForConfig(:,:,i);
    [body1Idx,body2Idx] = find(isnan(sepDist));

    collidingPairs = unique(sort([body1Idx,body2Idx],2));
    configCollisionPairs{i} = collidingPairs;
end

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

any(isConfigInCollision)

ans = logical
   1

firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that is in collision.
figure;
show(iiwa,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwa,configCollisionPairs{firstCollisionIdx}+1,gca);

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

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

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

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

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    isColliding = checkCollision(iiwa,q(:,i),'Exhaustive','on');
    isConfigInCollision(i) = isColliding;
end

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

any(isConfigInCollision)

ans = logical
   0