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

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