Добавьте препятствия 3-D списку капсул
addObstacle( добавляют одно или несколько препятствий 3-D динамическому списку капсул с заданным ID, состоянием и значениями геометрии, данными в capsuleListObj,obstacleStruct)obstacleStruct.
status = addObstacle(capsuleListObj,obstacleStruct)
Создайте путь к телу эго и обеспечьте состояния препятствия с помощью dynamicCapsuleList3D объект. Визуализируйте состояния всех объектов в среде в различных метках времени. Подтвердите путь тела эго путем проверки на столкновения с препятствиями в среде.
Создайте dynamicCapsuleList3D объект. Извлеките максимальное количество шагов, чтобы использовать в качестве количества меток времени для ваших контуров объектов.
obsList = dynamicCapsuleList3D; numSteps = obsList.MaxNumSteps;
Добавьте тело эго
Задайте тело эго путем определения ID, геометрии, и утвердите вместе в структуре. Краткая геометрия имеет длину 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.
Обновите путь к эго
Задайте новый путь для тела эго. Визуализируйте пути снова, отображая столкновения.
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, геометрию и состояния препятствия:
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). При добавлении препятствий, если несколько структур с тем же ID тела найдены в массиве структур obstaclesStruct, затем функция отмечает предыдущую запись как копия и игнорирует его.