addEgo

Добавьте тела эго в 3D список капсул

Описание

пример

addEgo(capsuleListObj,egoStruct) добавляют одно или несколько тел эго к 3-D динамическому списку капсул с заданным ID, состоянием и значениями геометрии, данными в egoStruct.

status = addEgo(capsuleListObj,egoStruct) дополнительно возвращает индикатор того, было ли каждое заданное тело эго добавлено, обновлено, или копия.

Примеры

свернуть все

Создайте путь к телу эго и обеспечьте состояния препятствия с помощью 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

Figure contains an axes object. The axes object contains 31 objects of type patch.

Добавьте препятствия

Задайте состояния для двух препятствий, которые разделяются от тела эго на 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);

Figure contains an axes object. The axes object contains 93 objects of type patch.

Обновите препятствия

Изменяйте свои местоположения препятствия и размерности геометрии в зависимости от времени. Используйте ранее сгенерированную структуру, измените поля и обновите препятствия с помощью 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);

Figure contains an axes object. The axes object contains 93 objects of type patch.

Программно проверяйте на столкновения 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);

Figure contains an axes object. The axes object contains 93 objects of type patch.

Входные параметры

свернуть все

Динамический список капсул в виде dynamicCapsuleList3D объект.

Параметры тела эго в виде 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).

Capsule geometry image showing the position and orientation of the capsule dimensions. Positive X is the right direction in the world frame. Positive Y is up. Positive Z is into the page. The origin of the capsule local frame is at the center of the left hemisphere end-cap. The end-caps have a given radius. The length defines the cylindrical length between ends.

Выходные аргументы

свернуть все

Результат добавляющих тел эго, возвращенных как N - вектор-столбец элемента из единиц, нулей и отрицательных единиц. N является количеством тел эго, заданных в egoStruct аргумент. Каждое значение указывает, добавляется ли связанное тело (1), обновленный (0), или копия (-1). При добавлении тел эго, если несколько структур с тем же ID тела найдены в массиве структур egoStruct, затем функция отмечает предыдущую запись как копия и игнорирует его.

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Введенный в R2020b
Для просмотра документации необходимо авторизоваться на сайте