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

Используйте 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