La traducción de esta página aún no se ha actualizado a la versión más reciente. Haga clic aquí para ver la última versión en inglés.

Comprobar las autocolisiones de los manipuladores mediante mallas de colisión

En este ejemplo se muestra cómo comprobar las autocolisiones de los manipuladores cuando se ejecuta una trayectoria. Las mallas de colisión se cargan por medio de la etiqueta <collision> definida en el URDF de un modelo de robot. En los siguientes ejemplos se muestra cómo cargar los datos de colisión de otras maneras y cómo comprobar las colisiones del entorno:

• Obtain Collision Data for Manipulator Collision Checking

• Comprobar las colisiones del entorno con los manipuladores

Crear una representación del robot

Importe un archivo URDF del manipulador en serie KUKA® IIWA-14 como modelo rigidBodyTree. El URDF captura los archivos de malla de colisión de los cuerpos rígidos del robot. Para añadir por separado objetos de colisión a un cuerpo rígido, se puede utilizar la función addCollision. Para cargar un modelo de robot proporcionado con objetos de colisión acoplados, consulte la función loadrobot.

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

Generar una trayectoria y comprobar si existen colisiones

Especifique una configuración inicial y final como un conjunto de posiciones de articulación. Estas posiciones no deben tener colisiones.

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

Encuentre una trayectoria en el espacio articular que conecte las dos configuraciones en tres segundos.

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

Para verificar que esta trayectoria de salida no contenga autocolisiones, itere sobre las muestras de salida y vea si algún punto está en colisión mediante la función checkCollision.

Cuando realice iteraciones sobre la trayectoria q, llame a la función checkCollision en cada configuración de la trayectoria. Active la comprobación exhaustiva para seguir comprobando si existen más colisiones después de detectar la primera.

La variable isConfigInCollision sigue el estado de colisión de cada configuración. La variable sepDistForConfig sigue la distancia de separación entre los cuerpos del robot. Para cada colisión, se almacena el par de índices del cuerpo en la variable configCollisionPairs. Tenga en cuenta que los cuerpos contiguos no se comprueban, ya que siempre están en contacto a través de la junta que los une.

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','SkippedSelfCollisions','parent');
    isConfigInCollision(i) = isColliding;
    sepDistForConfig(:,:,i) = sepDist;
end

Para averiguar los índices de los cuerpos en colisión, encuentre qué entradas de la variable sepDistForConfig son NaN. septDist es una matriz simétrica, por lo que se devuelve el mismo valor en los índices con índices invertidos. Simplifique la lista mediante el uso de unique.

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

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

Cuando inspeccione la salida, podrá ver que la trayectoria prevista pasa por una serie de colisiones. Visualice la configuración en la que se produce la primera colisión y resalte los cuerpos.

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);

Generar una trayectoria sin colisiones

Esta primera colisión se produce en realidad en la configuración inicial porque se especifica una posición de la articulación más allá de sus límites. Llame a wrapToPi para limitar las posiciones iniciales de las articulaciones.

Genere una trayectoria nueva y vuelva a comprobar si existen colisiones.

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','SkippedSelfCollisions','parent');
    isConfigInCollision(i) = isColliding;
end

Tras comprobar toda la trayectoria, no se encuentra ninguna colisión.

any(isConfigInCollision)

ans = logical
   0