Проверяйте на столкновения между телами эго и препятствиями
collisionFound = checkCollision(capsuleListObj)
[ проверки каждое тело эго для столкновений с препятствиями в среде, и возвращают результаты с помощью дополнительных заданных опций обнаружения столкновений fullResults,distance] = checkCollision(capsuleListObj,options)options.
Создайте путь к телу эго и обеспечьте состояния препятствия с помощью 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])
