Добавить препятствия 3-D список капсул
addObstacle( добавляет одно или несколько препятствий к списку динамических капсул 3-D с указанными значениями идентификатора, состояния и геометрии, указанными в capsuleListObj,obstacleStruct)obstacleStruct.
status = addObstacle(capsuleListObj,obstacleStruct)
Построить путь к эго-телу и поддерживать состояния препятствий с помощью dynamicCapsuleList3D объект. Визуализация состояний всех объектов среды в различных временных метках. Проверьте путь тела эго, проверив наличие столкновений с препятствиями в среде.
Создать dynamicCapsuleList3D объект. Извлеките максимальное количество шагов для использования в качестве временных отметок для путей объектов.
obsList = dynamicCapsuleList3D; numSteps = obsList.MaxNumSteps;
Добавить Ego Body
Определите эго-тело, указав идентификатор, геометрию и состояние вместе в структуре. Геометрия капсулы имеет длину 3 м и радиус 1 м. Укажите состояние как линейный путь от x = 0 м до x = 100 м.
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
Добавить препятствия
Укажите состояния для двух препятствий, отделенных от тела эго на 5 м в противоположных направлениях по оси Y. Предположим, что препятствия имеют одинаковую геометрию geom как тело эго.
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);
Обновить препятствия
Изменение местоположения препятствий и размеров геометрии с течением времени. Используйте ранее созданную структуру, измените поля и обновите препятствия, используя updateObstacleGeometry и updateObstaclePose функции объекта. Уменьшите радиус первого препятствия до 0,5 м и измените траекторию, чтобы переместить его в сторону тела эго.
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);
Проверка наличия коллизий
Визуализация новых путей. Показать, где происходят столкновения между эго-телом и препятствием, которое выделяется красным цветом. Обратите внимание, что столкновения между препятствиями не проверяются.
cla show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);
Программно проверять наличие конфликтов с помощью 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
Укажите новый путь для тела эго. Снова визуализируйте контуры, отображая конфликты.
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);
obstacleStruct - Параметры препятствияПараметры препятствия, указанные как структура N-элемента или структурный массив, где N - количество добавленных эго-тел. Поля каждой структуры определяют идентификатор, геометрию и состояния препятствия:
ID - Целое число, идентифицирующее каждый объект. Хранится в ObstacleIDs имущества dynamicCapsuleList3D объект, указанный capsuleListObj аргумент.
States - Расположение и ориентация объекта как матрицы M-by-6, где каждая строка имеет вид [x y z qW qX qY qZ], и M - количество состояний вдоль пути объекта в мировой рамке. Список состояний предполагает, что каждое состояние разделено фиксированным временным интервалом. xyz-положения находятся в метрах, а ориентация является четырехэлементным кватернионным вектором. Локальное начало координат по умолчанию расположено в центре левого полушария капсулы.
Geometry - Структура с полями Length, Radius, и FixedTransform. Эти поля определяют размер объекта на основе капсулы, используя заданную длину для цилиндра и радиус полусферы для торцевых крышек. Чтобы сместить геометрию капсулы от исходной точки по умолчанию, укажите FixedTransform поле как фиксированное преобразование относительно локального кадра капсулы. Чтобы сохранить начало капсулы по умолчанию, укажите преобразование как eye(4).
status - Результат добавления препятствийРезультат добавления препятствий, возвращаемый в виде N-элементного вектора столбцов единиц, нулей и отрицательных единиц. N - количество препятствий, указанных в obstacleStruct аргумент. Каждое значение указывает, добавляется ли связанное тело (1Обновлено0) или дубликат (-1). При добавлении препятствий, если в массиве структуры обнаружено несколько структур с одинаковым идентификатором тела obstaclesStruct, затем функция помечает предыдущую запись как повторяющуюся и игнорирует ее.