Документация

Создайте объект жесткого тела.

body1 = rigidBody('body1');

Создайте соединение и назначьте его жесткому телу. Определите свойство исходного положения соединения, HomePosition. Задание преобразования «соединение-родитель» с помощью однородного преобразования. tform. Используйте trvec2tform функция преобразования из вектора трансляции в однородное преобразование.ChildToJointTransform устанавливается в единичную матрицу.

jnt1 = rigidBodyJoint('jnt1','revolute');
jnt1.HomePosition = pi/4;
tform = trvec2tform([0.25, 0.25, 0]); % User defined
setFixedTransform(jnt1,tform);
body1.Joint = jnt1;

Создание дерева жесткого тела. Это дерево инициализируется базовой координатной рамкой для присоединения тел.

robot = rigidBodyTree;

Добавьте первое тело в дерево. Укажите, что оно прикрепляется к основанию дерева. Фиксированное преобразование, определенное ранее, от базового (родительского) к первому телу.

addBody(robot,body1,'base')

Создайте второе тело. Определите свойства этого тела и прикрепите его к первому жесткому телу. Определите преобразование относительно предыдущего корпуса.

body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
jnt2.HomePosition = pi/6; % User defined
tform2 = trvec2tform([1, 0, 0]); % User defined
setFixedTransform(jnt2,tform2);
body2.Joint = jnt2;
addBody(robot,body2,'body1'); % Add body2 to body1

Добавьте другие тела. Закрепите корпуса 3 и 4 на корпусе 2.

body3 = rigidBody('body3');
body4 = rigidBody('body4');
jnt3 = rigidBodyJoint('jnt3','revolute');
jnt4 = rigidBodyJoint('jnt4','revolute');
tform3 = trvec2tform([0.6, -0.1, 0])*eul2tform([-pi/2, 0, 0]); % User defined
tform4 = trvec2tform([1, 0, 0]); % User defined
setFixedTransform(jnt3,tform3);
setFixedTransform(jnt4,tform4);
jnt3.HomePosition = pi/4; % User defined
body3.Joint = jnt3
body4.Joint = jnt4
addBody(robot,body3,'body2'); % Add body3 to body2
addBody(robot,body4,'body2'); % Add body4 to body2

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

bodyEndEffector = rigidBody('endeffector');
tform5 = trvec2tform([0.5, 0, 0]); % User defined
setFixedTransform(bodyEndEffector.Joint,tform5);
addBody(robot,bodyEndEffector,'body4');

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

config = randomConfiguration(robot)
tform = getTransform(robot,config,'endeffector','base')

config = 

  1×2 struct array with fields:

    JointName
    JointPosition


tform =

   -0.5484    0.8362         0         0
   -0.8362   -0.5484         0         0
         0         0    1.0000         0
         0         0         0    1.0000

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

newArm = subtree(robot,'body2');
removeBody(newArm,'body3');
removeBody(newArm,'endeffector')

Можно также добавить эти поддеревы к роботу. Добавление поддерева аналогично добавлению тела. Указанное имя тела служит основой для присоединения, и все преобразования в поддереве относятся к этой рамке тела. Перед добавлением поддерева необходимо убедиться, что все имена тел и соединений уникальны. Создайте копии тел и соединений, переименуйте их и замените в поддереве. Звонить addSubtree для присоединения поддерева к указанному телу.

newBody1 = copy(getBody(newArm,'body2'));
newBody2 = copy(getBody(newArm,'body4'));
newBody1.Name = 'newBody1';
newBody2.Name = 'newBody2';
newBody1.Joint = rigidBodyJoint('newJnt1','revolute');
newBody2.Joint = rigidBodyJoint('newJnt2','revolute');
tformTree = trvec2tform([0.2, 0, 0]); % User defined
setFixedTransform(newBody1.Joint,tformTree);
replaceBody(newArm,'body2',newBody1);
replaceBody(newArm,'body4',newBody2);

addSubtree(robot,'body1',newArm);

Наконец, вы можете использовать showdetails посмотреть на робота, которого ты построил. Проверьте правильность типов соединений.

showdetails(robot)

 Idx          Body Name             Joint Name             Joint Type          Parent Name(Idx)   Children Name(s)
 ---          ---------             ----------             ----------          ----------------   ----------------
   1              body1                   jnt1               revolute                   base(0)   body2(2)  newBody1(6)  
   2              body2                   jnt2               revolute                  body1(1)   body3(3)  body4(4)  
   3              body3                   jnt3               revolute                  body2(2)   
   4              body4                   jnt4               revolute                  body2(2)   endeffector(5)  
   5        endeffector        endeffector_jnt                  fixed                  body4(4)   
   6           newBody1                newJnt1               revolute                  body1(1)   newBody2(7)  
   7           newBody2                newJnt2               revolute               newBody1(6)   
--------------------