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