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

updateEgoPose

Actualizar estados de los cuerpos del ego.

contraer todo en la página

Sintaxis

updateEgoPose(capsuleListObj,egoIDs,poseStruct)
status = updateEgoPose(capsuleListObj,egoIDs,poseStruct)

Descripción

updateEgoPose(capsuleListObj,egoIDs,poseStruct) actualiza los estados de los cuerpos del ego especificados en la lista de cápsulas. Si aún no existe un ID de ego específico, la función agrega un nuevo cuerpo del ego con ese ID a la lista.

ejemplo

status = updateEgoPose(capsuleListObj,egoIDs,poseStruct) devuelve un indicador de si un cuerpo de ego se agregó, se actualizó o es un duplicado.

Ejemplos

contraer todo

Abrir script en vivo

Construya una ruta del cuerpo del ego y mantenga los estados de obstáculo utilizando el objeto dynamicCapsuleList. 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 dynamicCapsuleList. Extraiga la cantidad máxima de pasos para usar como cantidad de marcas de tiempo para las rutas de sus objetos.

obsList = dynamicCapsuleList;
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 = 0m hasta x = 100m. 

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

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

show(obsList,"TimeStep",[1:numSteps]);
ylim([-20 20])

Figure contains an axes object. The axes object contains an object 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];
obsState2 = states + [0 -5 0];

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

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

show(obsList,"TimeStep",[1:numSteps]);
ylim([-20 20])

Figure contains an axes object. The axes object contains 3 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 m y 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)]; % theta

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

Comprobar si existen colisiones

Visualiza los nuevos rutas. Muestra dónde ocurren 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.

show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1);
ylim([-20 20])
xlabel("X (m)")
ylabel("Y (m)")

Figure contains an axes object. The axes object with xlabel X (m), ylabel Y (m) contains 3 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 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)]; % theta

updateEgoPose(obsList,1,egoCapsule1);

show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1);
ylim([-20 20])

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

Argumentos de entrada

contraer todo

Lista de cápsulas dinámicas, especificada como un objeto dynamicCapsuleList o dynamicCapsuleList3D.

ID de cuerpos del ego para actualizar, especificados como un vector de números enteros positivos.

Estados para cuerpos de ego, especificados como un arreglo de estructuras o un arreglo de estructuras, donde cada estructura contiene los campos de la estructura en el campo Geometry del cuerpo de ego que se actualizará. Cada elemento del arreglo estructural contiene una matriz de estados para cada cuerpo del ego. El tamaño de la matriz de estado depende de si está utilizando un objeto dynamicCapsuleList o dynamicCapsuleList3D.

Tipos de datos: struct

Argumentos de salida

contraer todo

Resultado de la actualización de los cuerpos del ego, especificado como vector columna de elementos N de unos, ceros y unos negativos. N es la cantidad de cuerpos del ego especificada en el argumento egoIDs. Cada valor indica si el cuerpo está actualizado (1), no se encontró (0) o es un duplicado (-1). Si especifica el mismo ID de ego varias veces en el argumento de entrada egoIDs, todas las entradas excepto la última se marcarán como duplicadas.

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

Consulte también

Objetos

Funciones

Temas