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

Создайте объект твердого тела.

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');

Теперь, когда вы создали своего робота, можно сгенерировать настройки робота. С данной настройкой можно также получить преобразование между двумя системами координат тела с помощью rigidBodyTree.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,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)   
--------------------