Загрузите робота

Этот пример использует робота Сойера от 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

Сгенерируйте набор пробела задачи Waypoints

В данном примере цель состоит в том, чтобы получить набор сегментов пути, которые прослеживают логотип мембраны MATLAB®. Мембранная поверхность и сегменты пути сгенерированы как массивы ячеек с помощью функции помощника 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';

Преобразуйте пробел задачи Waypoints в Объединенный Пробел Используя инверсную кинематику

Используйте решатель инверсной кинематики, чтобы сгенерировать набор объединенного пробела waypoints, которые дают объединенные настройки для робота в каждой точке сгенерированного 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