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

Импорт файла URDF последовательного манипулятора KUKA ® IIWA-14 в качестве rigidBodyTree модель. URDF захватывает файлы сетей столкновения для жестких тел робота. Для индивидуального добавления объектов столкновения к жесткому телу можно использовать 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