Загрузка модели робота

Используйте loadrobot функция для доступа к предоставленным моделям роботов как rigidBodyTree объекты.

robot = loadrobot("atlas");

Визуализация робота и сохранение Строений

Создайте интерактивный древовидный объект и связанный рисунок, задав загруженную модель робота и ее левую руку в качестве конечного эффектора.

viztree = interactiveRigidBodyTree(robot,"MarkerBodyName","l_hand");

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

The interactiveRigidBodyTree объект использует обратную кинематику, чтобы определить строение, которая достигает желаемого положения end-effector. Если связанное твердое тело не может достичь маркера, рисунок отображает лучшее строение от решателя обратной кинематики.

Программно установите текущее строение. Назначьте вектор длины, равной количеству нефиксованных соединений в RigidBodyTree на Configuration свойство.

currConfig = homeConfiguration(viztree.RigidBodyTree);
currConfig(1:10) = [ 0.2201 -0.1319 0.2278 -0.3415 0.4996 ...
                     0.0747 0.0377 0.0718 -0.8117 -0.0427]';
viztree.Configuration = currConfig;

Сохраните текущее строение робота в StoredConfigurations свойство.

addConfiguration(viztree)

Чтобы переключиться на другое твердое тело, щелкните правой кнопкой мыши требуемое тело на рисунке и выберите Задать тело как тело маркера. Этот процесс используется для выбора правой системы координат.

Можно также задать MarkerBodyName свойство для определенного имени тела.

viztree.MarkerBodyName = "r_hand";

Переместите правую руку в новое положение. Настройте строение программно. Маркер переходит в новое положение концевого эффектора.

currConfig(1:18) = [-0.1350 -0.1498 -0.0167 -0.3415 0.4996 0.0747
                     0.0377 0.0718 -0.8117 -0.0427 0 0.4349 
                    -0.5738 0.0563 -0.0095 0.0518 0.8762 -0.0895]';
                
viztree.Configuration = currConfig;

Сохраните текущее строение.

addConfiguration(viztree)

Добавление ограничений

По умолчанию модель робота уважает только пределы соединений rigidBodyJoint объекты, связанные с RigidBodyTree свойство. Чтобы добавить ограничения, сгенерируйте объекты Robot Constraint и задайте их как массив ячеек в Constraints свойство. Чтобы увидеть список роботизированных ограничений, см. «Обратная кинематика». Задайте цель положения таза, чтобы сохранить его неподвижным к исходному положению. Задайте цель положения для правой ноги, которая будет поднята спереди и выше ее текущего положения.

fixedWaist = constraintPoseTarget("pelvis");
raiseRightLeg = constraintPositionTarget("r_foot","TargetPosition",[1 0 0.5]);

Примените эти ограничения к интерактивному объекту древовидного твердого тела в качестве массива ячеек. Правая ножка на полученном рисунке изменяет положение.

viztree.Constraints = {fixedWaist raiseRightLeg};                               

Заметьте изменение положения правой ноги. Сохраните также это строение.

addConfiguration(viztree)

Воспроизведение настроек обратных строений

Чтобы воспроизвести конфигурации, итерация через индекс сохраненных конфигураций и установка текущей конфигурации равной вектору столбца сохраненной конфигурации при каждой итерации. Поскольку строения сохранены как векторы-столбцы, используйте второе измерение матрицы.

for i = 1:size(viztree.StoredConfigurations,2)
    viztree.Configuration = viztree.StoredConfigurations(:,i);
    pause(0.5)
end

Загрузка модели робота

Используйте loadrobot функция для доступа к предоставленным моделям роботов как rigidBodyTree объекты.

robot = loadrobot('abbIrb120'); 

Визуализация робота и сохранение Строений

Создайте интерактивный древовидный объект с помощью interactiveRigidBodyTree функция. По умолчанию интерактивный маркер устанавливается на тело с самым высоким индексом в RigidBodyTree свойство. Чтобы изменить это свойство с помощью рисунка, щелкните правой кнопкой по твердому телу и выберите Задать тело как тело маркера. Другой способ MarkerBodyName свойство для interactiveRigidBodyTree может быть задано с помощью пар "имя-значение".

iRBT = interactiveRigidBodyTree(robot); 

Интерактивное добавление строений

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

The interactiveRigidBodyTree объект использует обратную кинематику, чтобы определить строение, которая достигает желаемого положения end-effector. Если связанное твердое тело не может достичь маркера, рисунок отображает лучшее строение от решателя обратной кинематики.

Когда робот находится в желаемом строении, используйте addConfiguration объект для добавления строения к StoredConfiguration свойство объекта.

В этом примере 6 путевых точек создаются с помощью интерактивного маркера и addConfiguration функция объекта. Они сохраняются в wayPoints.mat. Доступ к сохранённым строениям можно получить с помощью iRBT.StoredConfigurations.

load("wayPts.mat");

Сгенерируйте сглаженную траекторию с помощью точек пути

Используйте cubicpolytraj функция для генерации гладкой траектории между путевыми точками. Задайте временные точки, которые соответствуют каждой путевой точке. Задайте временной вектор для генерации траектории. The cubicpolyTraj функция генерирует строение для каждого временного интервала во временном векторе tvec.

iRBT.StoredConfigurations = wayPts ;          % Waypoints 
tpts = [0 2 4 6 8 10];                        % Time Points 
tvec = 0:0.1:10;                              % Time Vector 
[q,qd,qdd,pp] = cubicpolytraj(iRBT.StoredConfigurations,tpts,tvec); 

Визуализация движения робота на траектории

Определите частоту симуляции с помощью rateControl объект. Используйте showFigure функция для визуализации модели робота и использования for цикл для воспроизведения всех строений робота.

r = rateControl(10);
iRBT.ShowMarker = false;  % Hide the marker 


showFigure(iRBT)

for i = 1:size(q',1)
    iRBT.Configuration = q(:,i);
    waitfor(r);
end