Main Content

Comprobar las colisiones del entorno con los manipuladores

Genere una trayectoria sin colisiones en un área de trabajo restringida. Este ejemplo muestra cómo inspeccionar las trayectorias en busca de colisiones y diseñar trayectorias de forma manual. Una alternativa es utilizar un planificador de trayectorias como en Pick and Place Using RRT for Manipulators.

Definir el entorno de colisión

Cree un entorno sencillo mediante primitivas de colisión. En este ejemplo se crea una escena en la que un robot tiene que mover objetos de una mesa a otra en un espacio de trabajo. El robot también debe evitar una lámpara ubicada en la parte superior del espacio de trabajo. Modele las mesas como dos cajas y una esfera y especifique su posición en el mundo real. Se pueden crear entornos más complejos utilizando objetos collisionMesh.

% Create two platforms
platform1 = collisionBox(0.5,0.5,0.25);
platform1.Pose = trvec2tform([-0.5 0.4 0.2]);

platform2 = collisionBox(0.5,0.5,0.25);
platform2.Pose = trvec2tform([0.5 0.2 0.2]);

% Add a light fixture, modeled as a sphere
lightFixture = collisionSphere(0.1);
lightFixture.Pose = trvec2tform([.2 0 1]);

% Store in a cell array for collision-checking
worldCollisionArray = {platform1 platform2 lightFixture};

Visualice el entorno mediante una función helper que itere a través del arreglo de colisiones.

ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

Figure contains an axes object. The axes object contains 3 objects of type patch.

Añadir un robot al entorno

Cargue un modelo de manipulador Kinova como objeto rigidBodyTree mediante la función loadrobot.

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

Muestre el robot en el entorno utilizando los mismos ejes que los objetos de colisión. La base del robot se fija al origen del mundo real.

show(robot,homeConfiguration(robot),"Parent",ax);

{"String":"Figure contains an axes object. The axes object contains 28 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.","Tex":[],"LaTex":[]}

Generar una trayectoria y comprobar si existen colisiones

Defina una configuración de articulación inicial sobre la plataforma 1 y una configuración de articulación final sobre la plataforma 2.

startConfig = [3.8906    1.1924    0.0000    0.0000    0.0001    1.9454    0.7491]';
endConfig = [-0.9240    1.2703    1.9865    1.2394    1.7457   -2.0500    0.4222]';

% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);

{"String":"Figure contains an axes object. The axes object contains 78 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.","Tex":[],"LaTex":[]}

Utilice un perfil de velocidad trapezoidal para generar una trayectoria sin obstáculos desde la posición inicial a la posición de partida y hasta la posición final.

q = trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);

Compruebe si existen colisiones con los obstáculos del entorno mediante la función checkCollision. Active los argumentos de nombre-valor IgnoreSelfCollision y Exhaustive . Las autocolisiones se ignoran porque los límites de las articulaciones del modelo de robot evitan la mayoría de dichas colisiones. La comprobación exhaustiva garantiza que la función calcule todas las distancias de separación y continúe buscando colisiones después de detectar la primera colisión.

La salida sepDist almacena las distancias entre los cuerpos del robot y los objetos de colisión del mundo real como una matriz. Cada fila corresponde a un determinado objeto de colisión del mundo real. Cada columna corresponde a un cuerpo de robot. Los valores de NaN indican una colisión. Almacene los índices de la colisión como un arreglo de celdas.

% Initialize outputs
inCollision = false(length(q), 1); % Check whether each pose is in collision
worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision

for i = 1:length(q)
    
    [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on","SkippedSelfCollisions","parent");
    
    [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % Find collision pairs
    worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; 
    worldCollisionPairIdx{i} = worldCollidingPairs;
    
end
isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision = logical
   1

Inspeccionar las colisiones detectadas

A partir del último paso, se detectan dos colisiones. Visualice estas configuraciones para seguir investigando. Utilice la función exampleHelperHighlightCollisionBodies para resaltar los cuerpos según los índices. Puede ver que se produce una colisión en la esfera y la mesa.

collidingIdx1 = find(inCollision,1);
collidingIdx2 = find(inCollision,1,"last");

% Identify the colliding rigid bodies.
collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';
collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';

% Visualize the environment.
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

% Add the robotconfigurations & highlight the colliding bodies.
show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax);
show(robot,q(:,collidingIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);

{"String":"Figure contains an axes object. The axes object contains 53 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.","Tex":[],"LaTex":[]}

Generar una trayectoria sin colisiones

Para evitar estas colisiones, añada configuraciones de articulaciones intermedias para garantizar que el robot se desplace alrededor del obstáculo.

intermediateConfig1 = [-0.3644   -1.8365    0.0430    1.6606    0.2889    0.9386    0.1271]';
intermediateConfig2 =[-1.8895    0.6716    2.2225    1.4608    2.5503   -2.0500    0.1536]';

% Show the new intermediate poses
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);
show(robot,intermediateConfig2,"Parent",ax);

{"String":"Figure contains an axes object. The axes object contains 53 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.","Tex":[],"LaTex":[]}

Genere una trayectoria nueva.

[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],200,"EndTime",2);

Verifique que no haya colisiones.

%Initialize outputs
inCollision = false(length(q),1); % Check whether each pose is in collision
for i = 1:length(q)
    inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","SkippedSelfCollisions","parent");
end
isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision = logical
   0

Visualizar la trayectoria generada

Anime el resultado.

% Plot the environment
ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

% Visualize the robot in its home configuration
show(robot,startConfig,"Parent",ax2);

% Update the axis size
axis equal

% Loop through the other positions
for i = 1:length(q)
    show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);
    
    % Update the figure    
    drawnow
end

{"String":"Figure contains an axes object. The axes object contains 28 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.","Tex":[],"LaTex":[]}

Represente las posiciones de las articulaciones a lo largo del tiempo.

figure
plot(t,q)
xlabel("Time")
ylabel("Joint Position")

Figure contains an axes object. The axes object contains 7 objects of type line.