Используйте manipulatorRRT объект для планирования траектории для модели робота дерева жесткого тела в среде с препятствиями. Визуализация запланированного пути с интерполированными состояниями.

Загрузка модели робота в рабочее пространство. Используйте манипулятор KUKA LBR iiwa ©.

robot = loadrobot("kukaIiwa14","DataFormat","row");

Создайте среду для робота. Создайте объекты столкновения и задайте их положения относительно основания робота. Визуализация среды.

env = {collisionBox(0.5, 0.5, 0.05) collisionSphere(0.3)};
env{1}.Pose(3, end) = -0.05;
env{2}.Pose(1:3, end) = [0.1 0.2 0.8];

show(robot);
hold on
show(env{1})
show(env{2})

Создайте планировщик RRT для модели робота.

rrt = manipulatorRRT(robot,env);

Укажите конфигурацию начала и цели.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Запланируйте путь. Из-за случайности алгоритма RRT установите rng затравка для повторяемости.

rng(0)
path = plan(rrt,startConfig,goalConfig);

Визуализируйте путь. Чтобы добавить дополнительные промежуточные состояния, выполните интерполяцию пути. По умолчанию interpolate объектная функция использует значение ValidationDistance для определения числа промежуточных состояний. for цикл показывает каждый 20-й элемент интерполированного тракта.

interpPath = interpolate(rrt,path);
clf
for i = 1:20:size(interpPath,1)
    show(robot,interpPath(i,:));
    hold on
end
show(env{1})
show(env{2})
hold off