addEgo
Добавьте тела эго в 3D список капсул
Описание
addEgo( добавляют одно или несколько тел эго к 3-D динамическому списку капсул с заданным ID, состоянием и значениями геометрии, данными в capsuleListObj,egoStruct)egoStruct.
status = addEgo(capsuleListObj,egoStruct)
Примеры
Создайте 3-D пути к телу эго и проверку на столкновения с 3-D препятствиями
Создайте путь к телу эго и обеспечьте состояния препятствия с помощью 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);
Входные параметры
capsuleListObj — Динамический список капсул
dynamicCapsuleList3D объект
Динамический список капсул в виде dynamicCapsuleList3D объект.
egoStruct — Параметры тела эго
структура | массив структур
Параметры тела эго в виде N - структура элемента или массив структур, где N является количеством добавленных тел эго. Поля каждой структуры задают ID, геометрию и состояния тела эго:
ID– Целое число, которое идентифицирует каждый объект. Сохраненный вEgoIDsсвойствоdynamicCapsuleList3Dобъект заданcapsuleListObjаргумент.States– Местоположение и ориентация объекта как M-by-6 матрица, где каждая строка имеет форму[x y z qW qX qY qZ], и M является количеством состояний вдоль пути объекта в мировой системе координат. Список состояний принимает, что каждое состояние разделяется фиксированным временным интервалом. xyz - положения исчисляются в метрах, и ориентация является четырехэлементным вектором кватерниона. Локальный источник по умолчанию расположен в центре левого полушария капсулы.Geometry– Структура с полямиLength,Radius, иFixedTransform. Эти поля задают размер основанного на капсуле объекта с помощью заданной длины для цилиндра и радиуса полушария для заглушек. Чтобы переключить краткую геометрию от источника по умолчанию, задайтеFixedTransformполе как фиксированное преобразование относительно локальной системы координат капсулы. Чтобы сохранить краткий источник по умолчанию, задайте преобразование какeye(4).
