Добавьте препятствия к 3-D капсулы списка
addObstacle( добавляет одно или несколько препятствий к 3-D списку динамических капсул с заданными значениями идентификаторов, состояний и геометрии, приведенными в capsuleListObj,obstacleStruct)obstacleStruct.
status = addObstacle(capsuleListObj,obstacleStruct)
Создайте путь к эго-телу и сохраните состояния препятствий, используя dynamicCapsuleList3D объект. Визуализируйте состояния всех объектов в окружении в различных временных метках. Проверьте путь тела ego путем проверки на столкновения с препятствиями в окружении.
Создайте dynamicCapsuleList3D объект. Извлеките максимальное количество шагов для использования в качестве количества меток времени для путей к объекту.
obsList = dynamicCapsuleList3D; numSteps = obsList.MaxNumSteps;
Добавить тело Ego
Задайте тело ego путем определения идентификатора, геометрии и состояния вместе в структуре. Геометрия капсулы имеет длину 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);
Программно проверяйте на столкновения bu с помощью 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)... % z ones(numSteps,2) zeros(numSteps,2)]; %quaternion % quaternion updateEgoPose(obsList,1,egoCapsule1); cla show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);
obstacleStruct - Параметры препятствийПараметры препятствия, заданные как структура N-element или массив структур, где N количество добавленных тел ego. Поля каждой структуры определяют идентификатор, геометрию и состояния препятствия:
ID -- Целое число, которое идентифицирует каждый объект. Хранится в ObstacleIDs свойство dynamicCapsuleList3D объект, заданный как capsuleListObj аргумент.
States -- Расположение и ориентация объекта как матрицы M-на-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, затем функция помечает предыдущую запись как повторяющуюся и игнорирует ее.