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

Загрузите '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];

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

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

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

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

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.