updateEgoPose

Обновите состояния тел эго

Описание

пример

updateEgoPose(capsuleListObj,egoIDs,poseStruct) обновляет состояния заданных тел эго в списке капсул. Если заданный ID эго уже не существует, функция добавляет новое тело эго с тем ID к списку.

status = updateEgoPose(capsuleListObj,egoIDs,poseStruct) возвращает индикатор того, добавляется ли тело эго, обновляется, или копия.

Примеры

свернуть все

Создайте путь к телу эго и обеспечьте состояния препятствия с помощью dynamicCapsuleList объект. Визуализируйте состояния всех объектов в среде в различных метках времени. Подтвердите путь тела эго путем проверки на столкновения с препятствиями в среде.

Создайте dynamicCapsuleList объект. Извлеките максимальное количество шагов, чтобы использовать в качестве количества меток времени для ваших контуров объектов.

obsList = dynamicCapsuleList;
numSteps = obsList.MaxNumSteps;

Добавьте тело эго

Задайте тело эго путем определения ID, геометрии, и утвердите вместе в структуре. Краткая геометрия имеет длину 3 м и радиус 1 м. Задайте состояние как линейный контур от x = 0m к x = 100 м.

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. The axes contains an object of type patch.

Добавьте препятствия

Задайте состояния для двух препятствий, которые разделяются от тела эго на 5 м в противоположных направлениях на оси Y.. Примите, что препятствия имеют ту же геометрию geom как тело эго.

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. The axes contains 3 objects of type patch.

Обновите препятствия

Изменяйте свои местоположения препятствия и размерности геометрии в зависимости от времени. Используйте ранее сгенерированную структуру, измените поля и обновите препятствия с помощью updateObstacleGeometry и updateObstaclePose функции объекта. Уменьшает радиус первого препятствия 0,5 м, и измените путь, чтобы переместить его к телу эго.

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

Проверяйте на столкновения

Визуализируйте новые пути. Покажите, где столкновения между телом эго и препятствием, которое отображение подсвечивает в красном. Заметьте, что столкновения между препятствиями не проверяются.

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

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

Программно проверяйте на столкновения при помощи checkCollision объектная функция. Функция возвращает вектор из логических значений, который указывает на состояние каждого временного шага. Вектор транспонирован в целях отображения.

collisions = checkCollision(obsList)'
collisions = 1x31 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

Чтобы подтвердить пути с большим количеством шагов, используйте any функция на векторе из значений столкновения.

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

Обновите путь к эго

Задайте новый путь для тела эго. Визуализируйте пути снова, отображая столкновения.

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. The axes contains 3 objects of type patch.

Входные параметры

свернуть все

Динамический список капсул в виде dynamicCapsuleList или dynamicCapsuleList3D объект.

Идентификаторы тел эго, чтобы обновиться в виде вектора из положительных целых чисел.

Состояния для тел эго в виде массива структур или массива структур, где каждая структура содержит поля структуры в Geometry поле тела эго, которое будет обновлено. Каждый элемент массива структур содержит матрицу состояний для каждого тела эго. Матричный размер состояния зависит от того, используете ли вы dynamicCapsuleList или dynamicCapsuleList3D объект.

Типы данных: struct

Выходные аргументы

свернуть все

Результат обновляющихся тел эго в виде N - вектор-столбец элемента из единиц, нулей и отрицательных единиц. N является количеством тел эго, заданных в egoIDs аргумент. Каждое значение указывает, обновляется ли тело (1), не найденный (0), или копия (-1). Если вы задаете тот же ID эго многократно в egoIDs входной параметр, затем все записи помимо последнего отмечены как копия.

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Введенный в R2020b
Для просмотра документации необходимо авторизоваться на сайте