Импорт модели робота из файла 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. Выходное инверсно-кинематическое решение подается обратно в качестве начального предположения для следующего решения. Это начальное предположение помогает отслеживать положение конечного эффектора и создавать плавные конфигурации.

Можно нажать кнопку обратного вызова для регенерации только что определенной модели робота и траектории.

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