Загрузите визуализацию робота и среду сборки

Загрузите 'abbYumi' модель робота. Инициализируйте интерактивную фигуру, использующую interactiveRigidBodyTree. Сохраните текущую систему координат.

robot = loadrobot('abbYumi', 'Gravity', [0 0 -9.81]);
iviz = interactiveRigidBodyTree(robot);
ax = gca;

Создайте среду, состоящую из столкновения поля, которые представляют пол, две полки с объектами и центральную таблицу.

plane = collisionBox(1.5,1.5,0.05);
plane.Pose = trvec2tform([0.25 0 -0.025]);
show(plane,'Parent', ax);

leftShelf = collisionBox(0.25,0.1,0.2);
leftShelf.Pose = trvec2tform([0.3 -.65 0.1]);
[~, patchObj] = show(leftShelf,'Parent',ax);
patchObj.FaceColor = [0 0 1];

rightShelf = collisionBox(0.25,0.1,0.2);
rightShelf.Pose = trvec2tform([0.3 .65 0.1]);
[~, patchObj] = show(rightShelf,'Parent',ax);
patchObj.FaceColor = [0 0 1];

leftWidget = collisionCylinder(0.01, 0.07);
leftWidget.Pose = trvec2tform([0.3 -0.65 0.225]);
[~, patchObj] = show(leftWidget,'Parent',ax);
patchObj.FaceColor = [1 0 0];

rightWidget = collisionBox(0.03, 0.02, 0.07);
rightWidget.Pose = trvec2tform([0.3 0.65 0.225]);
[~, patchObj] = show(rightWidget,'Parent',ax);
patchObj.FaceColor = [1 0 0];

centerTable = collisionBox(0.5,0.3,0.05);
centerTable.Pose = trvec2tform([0.75 0 0.025]);
[~, patchObj] = show(centerTable,'Parent',ax);
patchObj.FaceColor = [0 1 0];

В интерактивном режиме сгенерируйте настройки

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

Чтобы выбрать тело как исполнительный элемент конца, щелкните правой кнопкой по телу, чтобы выбрать его.

Тело маркера может также быть присвоено из командной строки:

iviz.MarkerBodyName = "gripper_r_base";

Если тело было установлено, используйте обеспеченные элементы маркера, чтобы переместить маркер, и выбранное тело следует. Перетаскивание центрального серого маркера перемещает маркер в Декартов пробел. Красные, зеленые, и синие оси перемещают маркер вдоль xyz-осей. Круги вращают маркер об осях эквивалентного цвета.

Можно также переместить отдельные соединения путем щелчка правой кнопкой по соединению и нажать метод управления маркера Toggle. MarkerControlMethod свойство объекта установлено в "JointControl".

Эти шаги могут также быть выполнены путем изменения свойств на объекте непосредственно.

iviz.MarkerBodyName = "yumi_link_2_r";
iviz.MarkerControlMethod = "JointControl";

Изменение в объединенное управление производит желтый маркер, который позволяет объединенному положению быть установленным непосредственно.

Iteractively перемещают робота, пока у вас нет желаемой настройки. Сохраните настройки с помощью addConfiguration. Каждый вызов добавляет текущую настройку в StoredConfigurations свойство.

addConfiguration(iviz)

Задайте Waypoints для траектории

В целях этого примера набор настроек обеспечивается в .mat файл.

Загрузите настройки и задайте их как набор сохраненных настроек. Первая настройка добавляется путем обновления Configuration свойство и вызов addConfiguration, который вы могли сделать в интерактивном режиме, но остальные просто добавляются путем присвоения StoredConfigurations свойство непосредственно.

load abbYumiSaveTrajectoryWaypts.mat

removeConfigurations(iviz) % Clear stored configurations

% Start at a valid starting configuration
iviz.Configuration = startingConfig;

addConfiguration(iviz)

% Specify the entire set of waypoints
iviz.StoredConfigurations = [startingConfig, ...
                             graspApproachConfig, ...
                             graspPoseConfig, ...
                             graspDepartConfig, ...
                             placeApproachConfig, ...
                             placeConfig, ...
                             placeDepartConfig, ...
                             startingConfig];

Сгенерируйте траекторию и воспроизведите ее

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

numSamples = 100*size(iviz.StoredConfigurations, 2) + 1;
[q,~,~, tvec] = trapveltraj(iviz.StoredConfigurations,numSamples,'EndTime',2);

Воспроизведите сгенерированную траекторию путем итерации сгенерированного q матрица, которая представляет серию объединенных настроек, которые перемещаются между каждым waypoint. В этом случае объект управления уровня используется, чтобы гарантировать, что скорость воспроизведения является отражающей из фактической скорости выполнения.

iviz.ShowMarker = false;
showFigure(iviz)
rateCtrlObj = rateControl(numSamples/(max(tvec) + tvec(2)));
for i = 1:numSamples
    iviz.Configuration = q(:,i);
    waitfor(rateCtrlObj);
end

Рисунок показывает, что робот выполняет сглаженную траекторию между всем заданным waypoints.