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

Импорт файла 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 функция на каждом строении в траектории. Включите исчерпывающую проверку, чтобы продолжить проверку на наличие конфликтов после обнаружения первого.

The isConfigInCollision переменная отслеживает состояние столкновения каждого строения. The 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