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

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

body1 = rigidBody('body1');

Создать соединение и назначить его твердому телу. Задайте свойство исходного положения соединения, HomePosition. Установите преобразование joint-to-parent с помощью однородного преобразования, 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)   
--------------------