updateObstaclePose

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

Описание

пример

updateObstaclePose(capsuleListObj,obstacleIDs,poseStruct) обновляет состояния указанных препятствий в списке капсул. Если указанный идентификатор препятствия еще не существует, функция добавляет к списку новое тело ego с этим идентификатором.

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

Примеры

свернуть все

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

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

obsList = dynamicCapsuleList;
numSteps = obsList.MaxNumSteps;

Добавить тело Ego

Задайте тело ego путем определения идентификатора, геометрии и состояния вместе в структуре. Геометрия капсулы имеет длину 3 м и радиус 1 м. Задайте состояние как линейный путь от x = 0m до 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. 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.

Обновление пути Ego

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

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 поле обновляемого препятствия. Каждый элемент массива структур содержит матрицу состояний для каждого тела ego. Размер матрицы состояний зависит от того, используете ли вы dynamicCapsuleList или dynamicCapsuleList3D объект.

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

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

свернуть все

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

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

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

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