Загрузка среды визуализации и построения роботов

Загрузить '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. Круги поворачивают маркер вокруг осей эквивалентного цвета.

Можно также переместить отдельные соединения, щелкнув их правой кнопкой мыши и выбрав команду Переключить метод управления маркерами.

MarkerControlMethod свойство объекта имеет значение "JointControl".

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

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

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

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

addConfiguration(iviz)

Определение ППМ для траектории

Для целей этого примера набор конфигураций представлен в .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];

Создать траекторию и воспроизвести ее назад

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

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

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

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

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