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

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

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

The 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

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