Contenido principal

Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

addObstacle

Agregue obstáculos a la lista de cápsulas 3-D

Descripción

addObstacle(capsuleListObj,obstacleStruct) agrega uno o más obstáculos a la lista de cápsulas dinámicas 3-D con los valores de ID, estado y geometría especificados en obstacleStruct.

ejemplo

status = addObstacle(capsuleListObj,obstacleStruct) además devuelve un indicador de si cada obstáculo especificado fue agregado, actualizado o duplicado.

Ejemplos

contraer todo

Construya una ruta del cuerpo del ego y mantenga los estados de obstáculo utilizando el objeto dynamicCapsuleList3D. Visualice los estados de todos los objetos del entorno en diferentes marcas de tiempo. Valide la ruta del cuerpo del ego comprobando colisiones con obstáculos en el entorno.

Cree el objeto dynamicCapsuleList3D. Extraiga la cantidad máxima de pasos para usar como cantidad de marcas de tiempo para las rutas de sus objetos.

obsList = dynamicCapsuleList3D;
numSteps = obsList.MaxNumSteps;

Añadir cuerpo del ego

Defina un cuerpo del ego especificando la identificación, la geometría y el estado juntos en una estructura. La geometría de la cápsula tiene una longitud de 3 my un radio de 1 m. Especifique el estado como una ruta lineal desde x = 0 m hasta x = 100 m.

egoID1 = 1;
geom = struct("Length",3,"Radius",1,"FixedTransform",eye(4));
states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0];
states = [states ones(numSteps,2) zeros(numSteps,2)];

egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom);
addEgo(obsList,egoCapsule1);

show(obsList,"TimeStep",1:numSteps);
ylim([-20 20])
zlim([-5 20])
view(-45,25)
hold on

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

Agregar obstáculos

Especifique estados para dos obstáculos que están separados del cuerpo del ego por 5 m en direcciones opuestas en el eje y. Supongamos que los obstáculos tienen la misma geometría geom que el cuerpo del ego.

obsState1 = states + [0 5 0 0 0 0 0];
obsState2 = states + [0 -5 0 0 0 0 0];

obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom);
obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom);

addObstacle(obsList,obsCapsule1);
addObstacle(obsList,obsCapsule2);

cla
show(obsList,"TimeStep",1:numSteps);

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

Actualizar obstáculos

Modifique las ubicaciones de los obstáculos y las dimensiones geométricas con el tiempo. Utilice la estructura generada previamente, modifique los campos y actualice los obstáculos utilizando las funciones de objeto updateObstacleGeometry y updateObstaclePose. Reduce el radio del primer obstáculo a 0,5 my cambia la ruta para moverlo hacia el cuerpo del ego.

obsCapsule1.Geometry.Radius = 0.5;

obsCapsule1.States = ...
    [linspace(0,100,numSteps)' ... % x
     linspace(5,-4,numSteps)' ... % y
     zeros(numSteps,1) ... % z
     ones(numSteps,2) zeros(numSteps,2)]; % quaternion                               % quaternion

updateObstacleGeometry(obsList,1,obsCapsule1);
updateObstaclePose(obsList,1,obsCapsule1);

Comprobar si existen colisiones

Visualiza los nuevos rutas. Muestra dónde se producen las colisiones entre el cuerpo del ego y un obstáculo, que la pantalla resalta en rojo. Tenga en cuenta que las colisiones entre los obstáculos no se controlan.

cla
show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);

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

Verifique programáticamente si hay colisiones mediante la función de objeto checkCollision. La función devuelve un vector de valores lógicos que indica el estado de la colisión en cada paso de tiempo. El vector se transpone con fines de visualización.

collisions = checkCollision(obsList)'
collisions = 1×31 logical array

   0   0   0   0   0   0   0   0   0   0   0   0   1   1   1   1   1   1   1   1   1   1   0   0   0   0   0   0   0   0   0

Para validar rutas con una gran cantidad de pasos, utilice la función any en el vector de valores de colisión.

if any(collisions)
    disp("Collision detected.")
end
Collision detected.

Actualizar ruta del ego

Especifica una ruta nueva para el cuerpo del ego. Visualice las rutas nuevamente, mostrando colisiones.

egoCapsule1.States = ...
    [linspace(0,100,numSteps)' ... % x
    3*sin(linspace(0,2*pi,numSteps))' ... % y
    zeros(numSteps,1)... % z
    ones(numSteps,2) zeros(numSteps,2)]; %quaternion                                  % quaternion

updateEgoPose(obsList,1,egoCapsule1);

cla
show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);

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

Argumentos de entrada

contraer todo

Lista de cápsulas dinámica, especificada como un objeto dynamicCapsuleList3D.

Argumentos de salida

contraer todo

Parámetros de obstáculo, especificados como una estructura de N elementos o un arreglo de estructuras, donde N es el número de cuerpos de ego agregados. Los campos de cada estructura definen el ID, la geometría y los estados de un obstáculo:

  • ID –– Entero que identifica cada objeto. Se almacena en la propiedad ObstacleIDs del objeto dynamicCapsuleList3D especificado por el argumento capsuleListObj.

  • States –– Ubicación y orientación del objeto como una matriz de M por 6, donde cada fila tiene la forma [x y z qW qX qY qZ] y M es el número de estados a lo largo de la ruta del objeto en el marco mundial. La lista de estados asume que cada estado está separado por un intervalo de tiempo fijo. Las posiciones xyz están en metros y la orientación es un vector de cuaternión de cuatro elementos. El origen local predeterminado se encuentra en el centro del hemisferio izquierdo de la cápsula.

  • Geometry –– Estructura con los campos Length, Radius y FixedTransform. Estos campos definen el tamaño del objeto basado en cápsula utilizando la longitud especificada para el cilindro y el radio del hemisferio para las tapas de los extremos. Para cambiar la geometría de la cápsula desde el origen predeterminado, especifique el campo FixedTransform como una transformación fija relativa al marco local de la cápsula. Para mantener el origen de la cápsula predeterminado, especifique la transformación como eye(4).

Capsule geometry image showing the position and orientation of the capsule dimensions. Positive X is the right direction in the world frame. Positive Y is up. Positive Z is into the page. The origin of the capsule local frame is at the center of the left hemisphere end-cap. The end-caps have a given radius. The length defines the cylindrical length between ends.

Resultado de agregar obstáculos, devuelto como un vector columna de elementos N de unos, ceros y unos negativos. N es la cantidad de obstáculos especificados en el argumento obstacleStruct. Cada valor indica si el cuerpo asociado se agregó (1), se actualizó (0) o es un duplicado (-1). Al agregar obstáculos, si se encuentran múltiples estructuras con el mismo ID de cuerpo en el arreglo de estructuras obstaclesStruct, la función marca la entrada anterior como duplicada y la ignora.

Capacidades ampliadas

expandir todo

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2020b