Динамический основанный на капсуле список препятствий
dynamicCapsuleList объект управляет двумя списками основанных на капсуле объектов столкновения на 2D пробеле. Объекты столкновения разделены на два списка, тела эго и препятствия. Для тел эго и препятствий в 3-D, смотрите dynamicCapsuleList3D объект.
Каждый объект столкновения в двух списках имеет три основных элемента:
ID – Целое число, которое идентифицирует каждый объект, хранивший в EgoIDs свойство для тел эго и ObstacleIDs свойство для препятствий.
Состояния – Местоположение и ориентация объекта как M-by-3 матрица, где каждая строка имеет форму [x y theta] и M является количеством состояний вдоль пути объекта в мировой системе координат. Список состояний принимает, что каждое состояние разделяется фиксированным временным интервалом. xy - положения исчисляются в метрах, и theta исчисляется в радианах. Локальный источник по умолчанию расположен в центре левого полукруга капсулы.
Геометрия – Размер основанного на капсуле основанного на объектах на заданной длине и радиусе. Радиус применяется к заглушкам полукруга, и длина применяется к длине центрального прямоугольника. Чтобы переключить краткую геометрию и локальный источник относительно точки источника по умолчанию, задайте фиксированное преобразование относительно локальной системы координат капсулы.
Используйте объектные функции, чтобы динамически добавить, удалить, и обновить конфигурации и состояния различных объектов в вашей среде. Чтобы добавить тело эго, смотрите addEgo объектная функция. Чтобы добавить препятствие, смотрите addObstacle объектная функция.
После определения всех объектных состояний подтвердите пути тела эго и проверку на столкновения с препятствиями на каждом шаге с помощью checkCollision объектная функция. Функция только проверяет, сталкивается ли тело эго с препятствием, игнорируя столкновения только между препятствиями или только телами эго.
obstacleList = dynamicCapsuleList создает динамический основанный на капсуле список препятствий без тел эго или препятствий. Чтобы начать создавать список препятствий, используйте addEgo или addObstacle функции объекта.
addEgo | Добавьте тела эго в список капсул |
addObstacle | Добавьте препятствия 2D списку капсул |
checkCollision | Проверяйте на столкновения между телами эго и препятствиями |
egoGeometry | Геометрические свойства тел эго |
egoPose | Положения тел эго |
obstacleGeometry | Геометрические свойства препятствий |
obstaclePose | Положения препятствий |
removeEgo | Удалите тела эго из списка капсул |
removeObstacle | Удалите препятствия из списка капсул |
show | Отобразите тела эго и препятствия в среде |
updateEgoGeometry | Обновите геометрические свойства тел эго |
updateEgoPose | Обновите состояния тел эго |
updateObstacleGeometry | Обновите геометрические свойства препятствий |
updateObstaclePose | Обновите состояния препятствий |
Создайте путь к телу эго и обеспечьте состояния препятствия с помощью 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])
