Импортируйте модель робота из файла URDF (унифицированный формат описания робота) как RigidBodyTree объект.

robot = importrobot('iiwa14.urdf');
robot.DataFormat = 'column';

Посмотрите робота.

ax = show(robot);

Задайте траекторию робота. Эти xyz-координаты рисуют N-образную форму перед роботом.

x = 0.5*zeros(1,4)+0.25;
y = 0.25*[-1 -1 1 1];
z = 0.25*[-1 1 -1 1] + 0.75;

hold on
plot3(x,y,z,'--r','LineWidth',2,'Parent',ax)
hold off

Откройте модель, которая выполняет обратную кинематику. xyz-координаты, определенные в MATLAB ®, преобразуются в однородные преобразования и вводятся как желаемые Pose. Обратно-кинематическое решение выхода подается назад как начальное предположение для следующего решения. Это начальное предположение помогает отслеживать положение end-effector и генерировать плавные строения.

Можно нажать кнопку коллбэка, чтобы регенерировать только что заданные модель робота и траекторию.

close
open_system('sm_ik_trajectory_model.slx')

% Запустите симуляцию. Модель должна сгенерировать строения робота (configs), которые следуют указанной траектории для конечного эффектора.

sim('sm_ik_trajectory_model.slx')

Прокрутите строения робота и отобразите робота для каждого временного шага. Сохраните положения концевого эффектора в xyz.

figure('Visible','on');
tformIndex = 1;
for i = 1:10:numel(configs.Data)/7
    currConfig = configs.Data(:,1,i);
    show(robot,currConfig);
    drawnow

    xyz(tformIndex,:) = tform2trvec(getTransform(robot,currConfig,'iiwa_link_ee'));
    tformIndex = tformIndex + 1;
end

Нарисуйте окончательную траекторию концевого эффектора в виде черной линии. Рисунок показывает конечный эффектор, отслеживающий первоначально заданную N-форму (красная пунктирная линия).

figure('Visible','on')
show(robot,configs.Data(:,1,end));

hold on
plot3(xyz(:,1),xyz(:,2),xyz(:,3),'-k','LineWidth',3);
plot3(x,y,z,'--r','LineWidth',3)
hold off