Esta página es para la versión anterior. La página correspondiente en inglés ha sido eliminada en la versión actual.
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 área de trabajo. El robot también debe evitar una lámpara ubicada en la parte superior del área 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);
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);
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);
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 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);
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);
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
Represente las posiciones de las articulaciones a lo largo del tiempo.
figure plot(t,q) xlabel("Time") ylabel("Joint Position")