Загрузка робота

В этом примере используется робот Sawyer из Rethink Robotics ®. Импортируйте файл URDF, задающий динамику твердого тела. Установите DataFormat использовать векторы-столбцы для определения строений робота. Simulink ® использует векторы-столбцы. Пределы пространства задач заданы на основе эмпирических данных.

sawyer = importrobot('sawyer.urdf');
sawyer.DataFormat = 'column';
taskSpaceLimits = [0.25 0.5; -0.125 0.125; -0.15 0.1];
numJoints = 8; % Number of joints in robot

Сгенерируйте набор путевых точек пространства задач

В данном примере цель состоит в том, чтобы получить набор сегментов контура, которые отслеживают логотип мембраны MATLAB ®. Мембранная поверхность и сегменты контура генерируются как массивы ячеек с помощью функции helper generateMembranePaths. Чтобы визуализировать пути, наложенные на поверхность, постройте график поверхности с помощью surf и сегменты контура путем итерации через массив ячеек сегмента контура. Можно увеличить numSamples чтобы пробовать более мелко через surace.

numSamples = 7;
[pathSegments, surface] = generateMembranePaths(numSamples, taskSpaceLimits);

% Visualize the output
figure
surf(surface{:},'FaceAlpha',0.3,'EdgeColor','none');
hold all
for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),'x-','LineWidth', 2);
end
hold off

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

figure
show(sawyer);
hold all

for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),'x-','LineWidth',2);
end

view(135,20)
axis([-1 1 -.5 .5 -1 .75])
hold off

Создайте решатель обратной кинематики

Создайте обратную кинематику (IK), используя загруженную sawyer древовидное твердое тело. Первоначально он сконфигурирован с единым набором весов, используя домашнее строение в качестве исходного предположения. Установите начальное предположение в исходное строение и допуски положения с равномерными весами. Конечным эффектором для решателя IK является 'right_hand' тело робота.

ik = inverseKinematics('RigidBodyTree', sawyer);
initialGuess = sawyer.homeConfiguration;
weights = [1 1 1 1 1 1];
eeName = 'right_hand';

Преобразуйте точки пути пространства задач в пространство соединений с помощью обратной кинематики

Используйте решатель обратной кинематики, чтобы сгенерировать набор путевых точек пространства соединений, которые задают строения соединений для робота в каждой точке сгенерированного pathSegments. Каждый сегмент пространства соединений подается в матрицу, jointPathSegmentMatrix, который передается в модель Simulink как вход.

% Initialize the output matrix
jointPathSegmentMatrix = zeros(length(pathSegments),numJoints,numSamples);

% Define the orientation so that the end effector is oriented down
sawyerOrientation = axang2rotm([0 1 0 pi]);

% Compute IK at each waypoint along each segment
for i = 1:length(pathSegments)
    currentTaskSpaceSegment = pathSegments{i};
    currentJointSegment = zeros(numJoints, length(currentTaskSpaceSegment));
    for j = 1:length(currentTaskSpaceSegment)
        pose = [sawyerOrientation currentTaskSpaceSegment(j,:)'; 0 0 0 1];
        currentJointSegment(:,j) = ik(eeName,pose,weights,initialGuess);
        initialGuess = currentJointSegment(:,j);
    end
    
    jointPathSegmentMatrix(i, :, :) = (currentJointSegment);
end

Загрузка модели Simulink

Используйте shapeTracingSawyer модель, чтобы выполнить траектории и симулировать их на кинематической модели робота.

open_system("shapeTracingSawyer.slx")

Модель Simulink имеет две основные части:

Генерация траектории

Симуляция кинематики робота

Выполните траектории пространства соединений в Simulink

Симулируйте модель, чтобы выполнить генерацию траекторий.

sim("shapeTracingSawyer.slx")

Просмотр результатов генерации траектории

Модель выводит строения соединений робота и положения концевого эффектора вдоль каждой сглаженной траектории пути. Чтобы легко работать с инструментами графического изображения MATLAB, измените форму данных.

% End effector positions
xPositionsEE = reshape(eePosData.Data(1,:,:),1,size(eePosData.Data,3));
yPositionsEE = reshape(eePosData.Data(2,:,:),1,size(eePosData.Data,3));
zPositionsEE = reshape(eePosData.Data(3,:,:),1,size(eePosData.Data,3));

% Extract joint-space results
jointConfigurationData = reshape(jointPosData.Data, numJoints, size(eePosData.Data,3));

Постройте график положения нового концевого эффектора на исходной поверхности мембраны.

figure
surf(surface{:},'FaceAlpha',0.3,'EdgeColor','none');
hold all
plot3(xPositionsEE,yPositionsEE,zPositionsEE)
grid on
hold off

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

В дополнение к визуализации выше, поведение трассировки может быть воссоздано с помощью модели робота Сойера. Итерация строений соединений в jointConfigurationData для визуализации робота с помощью show и непрерывно строят график положения концевого эффектора в трехмерном пространстве.

% For faster visualization, only display every few steps
vizStep = 5;

% Initialize a new figure window
figure
set(gcf,'Visible','on');

% Iterate through all joint configurations and end-effectort positions
for i = 1:vizStep:length(xPositionsEE)
    show(sawyer, jointConfigurationData(:,i),'Frames','off','PreservePlot',false);
    hold on
    plot3(xPositionsEE(1:i),yPositionsEE(1:i),zPositionsEE(1:i),'b','LineWidth',3)
    
    view(135,20)
    axis([-1 1 -.5 .5 -1 .75])
    
    drawnow
end
hold off