Настройте модель робота и среду

Этот пример использует модель LBR KUKA iiwa, 7 манипуляторов робота степени свободы. Используйте loadrobot загружать модель робота в рабочую область как rigidBodyTree объект. Установите выходной формат для настроек к "row".

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})

Запланируйте Путь Используя manipulatorRRT

Создайте планировщика RRT для модели робота использование manipulatorRRT. Установите ValidationDistance свойство увеличить число промежуточных состояний при интерполяции пути.

rrt = manipulatorRRT(robot,env);
rrt.ValidationDistance = 0.2;

Задайте запуск и целевую настройку.

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);

Интерполируйте путь и получите waypoints.

interpPath = interpolate(rrt,path);
wpts = interpPath';

Сгенерируйте полиномиальную траекторию толчка минимума

Планировщик возвращает путь как упорядоченное множество waypoints. Чтобы передать их роботу, необходимо сначала определить траекторию через них. minjerkpolytraj функция создает сглаженную траекторию с минимальным толчком, который поражает весь заданный waypoints.

Обеспечьте исходное предположение в течение времен, в которые манипулятор прибывает в waypoints.

initialGuess = linspace(0,size(wpts,2)*0.2,size(wpts,2));

Задайте количество отсчетов, чтобы взять при оценке траектории.

numSamples = 100;

Вычислите минимальную траекторию полинома толчка.

[q,qd,qdd,qddd,pp,tpts,tSamples] = minjerkpolytraj(wpts,initialGuess,numSamples);

Визуализируйте Trajectories и Waypoints

Стройте траектории и waypoints в зависимости от времени.

minJerkPath = q';
figure
plot(tSamples,q)
hold all
plot(tpts,wpts,"x")

Визуализируйте движение робота

Используйте show возразите функции, чтобы анимировать получившееся движение. Эта визуализация позволяет быстрым обновлениям гарантировать плавную анимацию.

figure;
ax = show(robot,startConfig);
hold all

% Ensure the figure pops out of the Live Editor so animations are visible
set(gcf,"Visible","on");
for i = 1:length(env)
    show(env{i},"Parent",ax);
end

for i = 1:size(minJerkPath,1)
    show(robot,minJerkPath(i,:),"PreservePlot",false,"FastUpdate",true);
    drawnow;
end

hold off