Добавьте препятствия 2D списку капсул
addObstacle( добавляют одно или несколько препятствий 2D динамическому списку капсул с заданным ID, состоянием и значениями геометрии, данными в capsuleListObj,obstacleStruct)obstacleStruct.
status = addObstacle(capsuleListObj,obstacleStruct)
Создайте путь к телу эго и обеспечьте состояния препятствия с помощью 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])
Добавьте препятствия
Задайте состояния для двух препятствий, которые разделяются от тела эго на 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])
Обновите препятствия
Изменяйте свои местоположения препятствия и размерности геометрии в зависимости от времени. Используйте ранее сгенерированную структуру, измените поля и обновите препятствия с помощью 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)")
Программно проверяйте на столкновения при помощи 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])
capsuleListObj — Динамический список капсулdynamicCapsuleList объектДинамический список капсул в виде dynamicCapsuleList объект.
obstacleStruct — Параметры препятствияПараметры препятствия в виде N - структура элемента или массив структур, где N является количеством добавленных препятствий. Поля каждой структуры задают ID, геометрию и состояния препятствия:
ID – Целое число, которое идентифицирует каждый объект. Сохраненный в ObstacleIDs свойство dynamicCapsuleList объект задан capsuleListObj аргумент.
States – Местоположение и ориентация объекта как M-by-3 матрица, где каждая строка имеет форму [x y theta], и M является количеством состояний для заданного препятствия в мировой системе координат. Список состояний принимает, что каждое состояние разделяется фиксированным временным интервалом. xy - положения исчисляются в метрах и theta исчисляется в радианах.
Geometry – Структура с полями Length, Radius, и FixedTransform. Эти поля задают размер основанного на капсуле объекта с помощью заданной длины для цилиндра и радиуса полукруга для заглушек. Чтобы переключить краткую геометрию от источника по умолчанию, задайте FixedTransform поле как фиксированное преобразование относительно локальной системы координат капсулы. Чтобы сохранить краткий источник по умолчанию, задайте преобразование как eye(3).
