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

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

robot = loadrobot("atlas");

Визуализация роботов и сохранение конфигураций

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

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

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

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

Программно установите текущую конфигурацию. Назначьте вектор длины, равный числу нефиксированных соединений в 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)

Чтобы переключить конечный эффектор на другое жесткое тело, щелкните правой кнопкой мыши требуемое тело на рисунке и выберите Задать тело как тело маркера (Set body as marker body). Этот процесс используется для выбора правой рамки.

Можно также установить 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 собственность. Чтобы изменить это свойство с помощью фигуры, щелкните правой кнопкой мыши жесткое тело и выберите Задать тело как тело маркера (Set body as marker body). В качестве альтернативы, MarkerBodyName для свойства interactiveRigidBodyTree можно задать с помощью пар имя-значение.

iRBT = interactiveRigidBodyTree(robot); 

Интерактивное добавление конфигураций

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

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

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

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

load("wayPts.mat");

Создание гладкой траектории с помощью ППМ

Используйте cubicpolytraj для создания плавной траектории между ППМ. Определите моменты времени, соответствующие каждому ППМ. Определите вектор времени для формирования траектории. 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