Аналитические решения обратной кинематики человекоподобного робота

Этот пример показывает, как вывести аналитические решения для обратной кинематики головной цепи человекоподобного робота.

Шаг 1: Задайте параметры

Опишите кинематику ссылки головной цепи (ссылка между туловищем и головой) человекоподобного робота NAO [1] с использованием параметров Денавита-Хартенберга (DH) и обозначений на основе исследования Kofinas et al. [2]. Следующее преобразование задает ссылку головной цепи.

T=ABase,0T0,1T1,2RxRyA2,Head

где:

  • ABase,0 - перемещение от основы (туловища) к шарниру или системе координат 0

  • T0,1 - ориентация ссылки 1 относительно 0

  • T1,2 - ориентация ссылки 2 относительно 1

  • Rx - вращение крена

  • Ry - тангаж вращения

  • A2,Head - перемещение от ссылки 2 к точке с конечным эффектором (головке)

T(1:3,4) определяет координаты головы, которые xc, yc, zc.

В этом примере вы аналитически решаете задачу обратной кинематики, возвращая все ориентации отдельных соединений в звене цепи головы с заданными координатами головы xc, yc, и zc в пределах достижимого пространства. Затем вы преобразуете аналитические результаты в чисто числовые функции для эффективности.

Решение аналитически (когда это возможно) позволяет вам выполнять расчеты в реальном времени и избегать особенностей, которые вызывают трудности для численных методов.

Шаг 2: Задайте Прямую Кинематику Цепи Головы Используя Параметры DH

Функция getKinematicChain возвращает определенную кинематическую цепь для робота NAO с точки зрения символьных переменных. Для получения дополнительной информации о getKinematicChain, см. Раздел «Вспомогательные функции».

kinChain = 'head';
dhInfo = getKinematicChain(kinChain);
T = dhInfo.T
T = 

(r11r12r13xcr21r22r23ycr31r32r33zc0001)[r11, r12, r13, xc; r21, r22, r23, yc; r31, r32, r33, zc; sym (0), sym (0), sym (0), sym (1)]

Выразите матрицу преобразования прямой кинематики T как следующая последовательность продуктов: T = ABase0*T01*T12*Rx*Ry*A2Head. Задайте отдельные матрицы следующим образом.

Задайте матрицу преобразования из туловища (основы) в соединение 0.

ABase0 = dhInfo.ABase0
ABase0 = 

(10000100001NeckOffsetZ0001)[sym (1), sym (0), sym (0), sym (0); sym (0), sym (1), sym (0), sym (0); sym (0), sym (0), sym (1), NeckOffsetZ; sym (0), sym (0), sym (0), sym (1)]

Задайте матрицу преобразования из соединения 0 в соединение 1.

T01 = dhInfo.T01
T01 = 

(cos(θ1)-sin(θ1)00sin(θ1)cos(θ1)0000100001)[cos (theta_1), -sin (theta_1), sym (0), sym (0); sin (theta_1), cos (theta_1), sym (0), sym (0); sym (0), sym (0), sym (1), sym (0); sym (0), sym (0), sym (0), sym (1)]

Задайте матрицу преобразования из соединения 1 в соединение 2.

T12 = dhInfo.T12
T12 = 

(cos(θ2-π2)-sin(θ2-π2)000010-sin(θ2-π2)-cos(θ2-π2)000001)[cos (theta_2 - sym (pi )/2), -sin (theta_2 - sym (pi )/2), sym (0), sym (0); sym (0), sym (0), sym (1), sym (0); -sin (theta_2 - sym (pi )/2), -cos (theta_2 - sym (pi )/2), sym (0), sym (0); sym (0), sym (0), sym (0), sym (1)]

Задайте матрицу поворота крена.

Rx = dhInfo.Rx
Rx = 

(100000-1001000001)[sym (1), sym (0), sym (0), sym (0); sym (0), sym (0), -sym (1), sym (0); sym (0), sym (1), sym (0), sym (0); sym (0), sym (0), sym (0), sym (1)]

Задайте матрицу поворота тангажа.

Ry = dhInfo.Ry
Ry = 

(00100100-10000001)[sym (0), sym (0), sym (1), sym (0); sym (0), sym (1), sym (0), sym (0); -sym (1), sym (0), sym (0), sym (0); sym (0), sym (0), sym (0), sym (1)]

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

A2Head = dhInfo.A2Head
A2Head = 

(100CameraX0100001CameraZ0001)[sym (1), sym (0), sym (0), CameraX; sym (0), sym (1), sym (0), sym (0); sym (0), sym (0), sym (1), CameraZ; sym (0), sym (0), sym (0), sym (1)]

Известными параметрами этой задачи являются CameraX, CameraZ, NeckOffsetZ, и положения xc, yc, и zc. Неизвестные параметры: θ1 и θ2. После нахождения θ1 и θ2, можно вычислить отдельные преобразования T. Затем робот может достичь желаемого положения xc, yc, zc.

Хотя вы можете увидеть эти параметры в матрицах преобразования, они не существуют как переменные в базовом рабочем пространстве MATLAB. Это связано с тем, что эти параметры берутся из функции. Функции не используют базовое рабочее пространство. Каждое рабочее пространство функции отделено от базового рабочего пространства и всех других рабочих областей, чтобы защитить целостность данных. Таким образом, чтобы использовать эти переменные вне функции getKinematicChain, использовать syms создать их.

syms CameraX CameraZ NeckOffsetZ xc yc zc theta_1 theta_2 real;

Шаг 3: Найти Алгебраические Решения для θ1 и θ2

Перепишите уравнение T = ABase0*T01*T12*Rx*Ry*A2Head, разделяя термины, описывающие движения туловища и головы робота: inv(T01)*inv(ABase0)*T = T12*Rx*Ry*A2Head. Упростите левую и правую стороны уравнения и задайте интересующие уравнения, соответствующие выражениям для координатных положений.

LHS = simplify(inv(T01)*inv(ABase0)*T);
RHS = simplify(T12*Rx*Ry*A2Head);
eqns = LHS(1:3,end) - RHS(1:3,end)
eqns = 

(xccos(θ1)-CameraZsin(θ2)-CameraXcos(θ2)+ycsin(θ1)yccos(θ1)-xcsin(θ1)zc-NeckOffsetZ-CameraZcos(θ2)+CameraXsin(θ2))[xc * cos (theta_1) - CameraZ * sin (theta_2) - CameraX * cos (theta_2) + yc * sin (theta_1); yc * cos (theta_1) - xc * sin (theta_1); zc - NeckOffsetZ - CameraZ * cos (theta_2) + CameraX * sin (theta_2)]

Эта система уравнений содержит две переменные, для которых вы хотите найти решения, θ1 и θ2. Однако уравнения также подразумевают, что вы не можете произвольно выбирать xc, yc, и zc. Поэтому также рассмотрим yc как переменная. Все другие неизвестные системы являются символьными параметрами.

Этот пример следует типичному алгебраическому подходу для решения задач обратной кинематики [3]. Идея состоит в том, чтобы получить компактное представление решения, где выражение каждой переменной в терминах параметров и переменных, для которых вы уже решили уравнения. В качестве первого шага найдите или θ1 или θ2 с точки зрения параметров. Затем выразите другую переменную в терминах известной переменной и параметров. Для этого начните с определения уравнения, которое имеет только одну переменную.

intersect(symvar(eqns(1)),[theta_1,theta_2,yc])
ans = (θ1θ2yc)[theta_1, theta_2, yc]
intersect(symvar(eqns(2)),[theta_1,theta_2,yc])
ans = (θ1yc)[theta_1, yc]
intersect(symvar(eqns(3)),[theta_1,theta_2,yc])
ans = θ2theta_2

Третье уравнение содержит только одну переменную, θ2. Сначала решите это уравнение.

[theta_2_sol,theta_2_params,theta_2_conds] = ...
    solve(eqns(3),theta_2,'Real',true,'ReturnConditions',true)
theta_2_sol = 

(2πk-2atan(CameraX-CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc)2πk-2atan(CameraX+CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc))[2 * sym (pi) * k - 2 * atan ((Camera X - sqrt (Camera X ^ 2 + Camera Z ^ 2 - Neck Offset Z ^ 2) )/( Camera Z - Neck Offset Z + 2 * sym (pi) * k - 2 * atan ((Camera X + sqrt (Camera X ^ 2 + Camera Z ^ 2 - Neck Offset Z ^ 2) )/( Camera Z - Neck Offset Z + zc

theta_2_params = kk
theta_2_conds = 

(kZCameraZ+zcNeckOffsetZNeckOffsetZ-zc2CameraX2+CameraZ2kZCameraZ+zcNeckOffsetZNeckOffsetZ-zc2CameraX2+CameraZ2)[в (k, 'целое число') &CameraZ + zc ~ = NeckOffsetZ& (NeckOffsetZ - zc) ^2 <= CameraX^2 + CameraZ^2; в (k, 'целое число') &CameraZ + zc ~ = NeckOffsetZ& (NeckOffsetZ - zc) ^2 <= CameraX^2 + CameraZ^2]

Решения имеют добавку 2πk термин с параметром k. Без потери обобщения можно задать k равным 0 в решениях и условиях.

theta_2_sol = subs(theta_2_sol,theta_2_params,0)
theta_2_sol = 

(-2atan(CameraX-CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc)-2atan(CameraX+CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc))[-2 * atan ((CameraX - sqrt (CameraX ^ 2 + CameraZ ^ 2 - NeckOffsetZ ^ 2) )/( CameraZ - NeckOffsetZ + zc)); -2 * atan

for p = 1:numel(theta_2_conds)
    theta_2_conds(p) = simplify(subs(theta_2_conds(p),theta_2_params,0));
end

Теперь решите для переменных θ1 и yc в терминах переменной θ2 и другие символические параметры.

[theta_1_sol,yc_sol,yc_theta_1_params,yc_theta_1_conds] = ...
    solve(eqns(1:2),theta_1,yc,'Real',true,'ReturnConditions',true);

Отобразите решения для θ1 и yc.

theta_1_sol
theta_1_sol = 

(2πk-σ1σ2+2πk2πk-σ22πk-π2σ1+2πk2atan(x)+2πkπ2+2πk2πk-π2π2+2πk)where  σ1=2atan(CameraX+xcCameraX-xcCameraX-xc)  σ2=2atan(σ3σ5+1+σ5σ3σ5+1σ4)  σ3=-xc-CameraX+CameraXσ5+xcσ5-2CameraZtan(θ22)σ4  σ4=CameraX+xc-CameraXσ5+xcσ5+2CameraZtan(θ22)  σ5=tan(θ22)2[2 * sym (pi) * k - 2 * atan (sqrt ((CameraX + xc) * (CameraX - xc) )/( CameraX - xc)); 2*atan ((sqrt (-(xc - CameraX + CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 - 2*CameraZ*tan (theta_2/2)) * (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2))) / (tan (theta_2/2) ^2 + 1) + (tan (theta_2/2) ^2*sqrt (-(xc - CameraX + CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 - 2*CameraZ*tan (theta_2/2)) * (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2)))) / (tan (theta_2/2) ^2 + 1)) / (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2))) + 2*sym (пи) *k; 2*sym (пи) *k - 2*atan ((sqrt (-(xc - CameraX + CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 - 2*CameraZ*tan (theta_2/2)) * (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2))) / (tan (theta_2/2) ^2 + 1) + (tan (theta_2/2) ^2*sqrt (-(xc - CameraX + CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 - 2*CameraZ*tan (theta_2/2)) * (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2)))) / (tan (theta_2/2) ^2 + 1)) / (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2))); 2 * sym (pi) * k - sym (pi )/2; 2 * atan (sqrt ((CameraX + xc) * (CameraX - xc) )/( CameraX - xc)) + 2 * sym (pi) * k; 2 * atan (x) + 2 * sym (pi) * k; sym (pi )/2 + 2 * sym (pi) * k; 2 * sym (pi) * k - sym (pi )/2; sym (pi )/2 + 2 * sym (pi) * k]

yc_sol
yc_sol = 

(CameraX+xcCameraX-xcσ1-σ1CameraX-CameraX+xcCameraX-xc0σ2-σ2-CameraX)where  σ1=-xc-CameraX+σ3+xctan(θ22)2-σ4CameraX+xc-σ3+xctan(θ22)2+σ4tan(θ22)2+1  σ2=-σ3+σ4+CameraXtan(θ22)2+1  σ3=CameraXtan(θ22)2  σ4=2CameraZtan(θ22)[sqrt ((CameraX + xc) * (CameraX - xc)); sqrt (-(xc - CameraX + CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 - 2*CameraZ*tan (theta_2/2)) * (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2))) / (tan (theta_2/2) ^2 + 1);-sqrt (-(xc - CameraX + CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 - 2*CameraZ*tan (theta_2/2)) * (CameraX + xc - CameraX*tan (theta_2/2) ^2 + xc*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2))) / (tan (theta_2/2) ^2 + 1); CameraX; -sqrt ((CameraX + xc) * (CameraX - xc)); sym (0); (-CameraX*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2) + CameraX) / (tan (theta_2/2) ^2 + 1); - (-CameraX*tan (theta_2/2) ^2 + 2*CameraZ*tan (theta_2/2) + CameraX) / (tan (theta_2/2) ^2 + 1);-CameraX]

Решения зависят от двух параметров, k и x.

yc_theta_1_params
yc_theta_1_params = (kx)[k, x]

Решения имеют добавку 2πk срок. Без потери обобщения можно задать k равным 0 в решении для theta_1_sol и условия yc_theta_1_conds.

theta_1_sol = simplify(subs(theta_1_sol,yc_theta_1_params,[0,0]))
theta_1_sol = 

(-2atan(CameraX+xcCameraX-xcCameraX-xc)σ1-σ1-π22atan(CameraX+xcCameraX-xcCameraX-xc)0π2-π2π2)where  σ1=2atan(CameraXcos(θ2)-xc+CameraZsin(θ2)xc+CameraXcos(θ2)+CameraZsin(θ2)xc+CameraXcos(θ2)+CameraZsin(θ2))[-2 * atan (sqrt ((CameraX + xc) * (CameraX - xc) )/( CameraX - xc)); 2*atan (sqrt ((CameraX*cos (theta_2) - xc + CameraZ*sin (theta_2)) * (xc + CameraX*cos (theta_2) + CameraZ*sin (theta_2))) / (xc + CameraX*cos (theta_2) + CameraZ*sin (theta_2)));-2*atan (sqrt ((CameraX*cos (theta_2) - xc + CameraZ*sin (theta_2)) * (xc + CameraX*cos (theta_2) + CameraZ*sin (theta_2))) / (xc + CameraX*cos (theta_2) + CameraZ*sin (theta_2)));-sym (пи)/2; 2 * atan (sqrt ((CameraX + xc) * (CameraX - xc) )/( CameraX - xc)); sym (0); sym (pi )/2; -sym (pi )/2; sym (pi )/2]

for p = 1:numel(yc_theta_1_conds)
    yc_theta_1_conds(p) = simplify(subs(yc_theta_1_conds(p),yc_theta_1_params,[0,0]));
end

Аналогичная замена не требуется для yc_sol поскольку нет зависимости от параметров.

intersect(symvar(yc_sol),yc_theta_1_params)
 
ans =
 
Empty sym: 1-by-0
 

Шаг 4: Проверьте решения

Начиная с произвольного набора известных числовых значений для θ1 и θ2, вычислите числовые значения положений end-effector xc, yc, и zc с кинематикой вперед. Затем работайте назад от xc, yc, и zc для вычисления всех возможных числовых значений для θ1 и θ2 использование выражений обратной кинематики из предыдущего расчета. Проверьте, что один набор обратных решений совпадает с начальным набором числовых значений для θ1 и θ2.

Установите фиксированные параметры для робота. Значения в этом примере предназначены только для рисунка.

CameraXValue = 53.9; 
CameraZValue = 67.9; 
NeckOffsetZValue = -5;

Используя прямые расчеты, найдите положения end-effector, соответствующие значениям θ1 и θ2.

Tfk = ABase0*T01*T12*Rx*Ry*A2Head;
xyz = Tfk(1:3,end);

Создайте символическую функцию для этих положений.

xyzFunc = symfun(subs(xyz,[CameraX,CameraZ,NeckOffsetZ], ...
  [CameraXValue,CameraZValue,NeckOffsetZValue]),[theta_1,theta_2]);

Для кинематики вперед задайте две переменные theta_1_known и theta_2_known которые содержат произвольное стартовое множество значений для θ1 и θ2.

theta_1_known = [pi/4,pi/6,pi/8,pi/10];
theta_2_known = [pi/8,-pi/12,pi/16,-pi/10];
numPts = numel(theta_1_known);
num_theta_2 = numel(theta_2_sol);
num_theta_1 = numel(theta_1_sol);

Обратите внимание, что потенциально существуют num_theta_1 решения для каждого num_theta_2 решение.

Используйте следующую последовательность операций для проверки решений.

  1. Цикл (theta_1_known,theta_2_known). Для каждой точки вычислите конечные положения x_known, y_known, и z_known, которые затем «известны».

  2. Цикл над решениями для θ2 соответствующий x_known и z_known. Для каждого решения проверьте, соответствует ли соответствующее условие cond_theta_2 является допустимым. Если условие верно, вычислите соответствующее числовое решение theta_2_derived.

  3. Цикл над решениями для θ1 соответствующий x_known, z_known, и theta_2_derived. Для каждого решения проверьте, соответствует ли соответствующее условие cond_theta_1 является допустимым. Если условие верно, проверьте, является ли y_derived численно совпадает с y_known в пределах относительных и абсолютных погрешностей через условие cond_yc. Если это условие допустимо, вычислите theta_1_derived.

  4. Соберите результаты в таблицу T в целях отображения.

T = table([],[],[],[],'VariableNames',{'theta_1_known','theta_2_known',...
    'theta_1_derived','theta_2_derived'});

for ix1 = 1:numPts
xyz_known = double(xyzFunc(theta_1_known(ix1),theta_2_known(ix1)));
x_known = xyz_known(1);
y_known = xyz_known(2);
z_known = xyz_known(3);
for ix2 = 1:num_theta_2 % theta_2 loop
  cond_theta_2 = subs(theta_2_conds(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],...
                 [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]);
  if isAlways(cond_theta_2) % if theta_2 is valid
    theta_2_derived = subs(theta_2_sol(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],...
                      [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]);
    theta_2_derived = double(theta_2_derived);
    for ix3 = 1:num_theta_1 % theta_1 loop
      cond_theta_1 = subs(yc_theta_1_conds(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],...
              [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);
      if isAlways(cond_theta_1) % if theta_1 is valid
        y_derived = subs(yc_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],...
                [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);
        y_derived = double(y_derived);
        cond_yc = abs(y_known - y_derived) < max(100*eps,1e-6*abs(y_known)); % check rounding
        if isAlways(cond_yc) % if yc is valid
          theta_1_derived = subs(theta_1_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],...
                  [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);
          theta_1_derived = double(theta_1_derived);
          T = vertcat(T,table(theta_1_known(ix1),theta_2_known(ix1),theta_1_derived,...
              theta_2_derived,'VariableNames',T.Properties.VariableNames));
        end
      end
    end
  end
end
end
T
T=8×4 table
    theta_1_known    theta_2_known    theta_1_derived    theta_2_derived
    _____________    _____________    _______________    _______________

        0.7854           0.3927            0.7854             0.3927    
        0.7854           0.3927           -2.3562            -1.7346    
        0.5236          -0.2618            0.5236            -0.2618    
        0.5236          -0.2618            -2.618            -1.0801    
        0.3927          0.19635            0.3927            0.19635    
        0.3927          0.19635           -2.7489            -1.5383    
       0.31416         -0.31416           0.31416           -0.31416    
       0.31416         -0.31416           -2.8274            -1.0278    

Заметьте, что существует две возможные пары решений (theta_1_derived,theta_2_derived) полученный с использованием обратной кинематики для каждой пары стартовых углов (theta_1_known,theta_2_known). Один набор обратных решений совпадает с начальными углами.

Ссылки

  1. SoftBank Robotics. НАО. https://www.softbankrobotics.com/emea/en/nao.

  2. Кофинас, Н., Э. Орфанудакис, и М. Г. Лагудакис. «Полная аналитическая обратная кинематика для NAO». В 2013 году 13-я Международная конференция по автономным Роботам системам. Лиссабон, Португалия: Robotica, 2013.

  3. Кендрикс, К. «Решение обратной кинематической робототехнической задачи: сравнительное исследование матрицы базиса Денавита-Хартенберга и Гребнера». Доктор философии. Дипломная работа. Auburn University, Auburn, AL, 2007.

Вспомогательные функции

function kinChain = getKinematicChain(link)
% This function returns the kinematic chain of the NAO humanoid robot
% represented using Denavit-Hartenberg (DH) parameters.
% The function uses as a reference the paper: Complete analytical inverse
% kinematics for NAO by Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G.,
% 2013 13th International Conference on Autonomous Robot Systems
% (Robotica), Publication Year: 2013.
    if nargin < 1
        link = 'head';
    end
    % Notation: A, R, and T are the translation, rotation, and DH
    % transformation matrices, respectively.
    % Specify DH parameters for the desired end configuration.
    syms r11 r12 r13 r21 r22 r23 r31 r32 r33 xc yc zc real;
    R = [r11 r12 r13;r21 r22 r23;r31 r32 r33];
    kinChain.T = [R,[xc;yc;zc];0,0,0,1];
    switch link
        case 'head' % Kinematic chain from torso (base) to head.
            syms CameraX CameraZ NeckOffsetZ real;
            % Translation from torso (base) to joint 0.
            ABase0 = getA([0 0 NeckOffsetZ]);
            % Translation from joint 2 to head.
            A2Head = getA([CameraX 0 CameraZ]);
            % Transformation from joint 0 to joint 1.
            % theta_1 is the rotation angle corresponding to the 0-1 link.
            syms theta_1 real;
            alpha1 = 0; a1 = 0; d1 = 0;
            T01 = getT(a1,alpha1,d1,theta_1);
            % Transformation from joint 1 to joint 2.
            % theta_2 is the rotation angle corresponding to the 1-2 link.
            syms theta_2 real;
            piby2 = str2sym('pi/2');
            d2 = 0; a2 = 0; alpha2 = -piby2;
            T12 = getT(a2,alpha2,d2,theta_2-piby2);
            % Rx is the roll rotation.
            Rx = getR('x',piby2);
            % Ry is the pitch rotation.
            Ry = getR('y',piby2);
            % Capture the kinematic chain as a string.
            % The transformation is from the base to the head.
            kinChain.DHChain = 'ABase0*T01*T12*Rx*Ry*A2Head';
            kinChain.ABase0 = ABase0;
            kinChain.T01 = T01;
            kinChain.T12 = T12;
            kinChain.Rx = Rx;
            kinChain.Ry = Ry;
            kinChain.A2Head = A2Head;
        case 'lefthand' % Kinematic chain from torso to left hand.
            syms ShoulderOffsetY ElbowOffsetY ShoulderOffsetZ real;
            ABase0 = getA([0 (ShoulderOffsetY+ElbowOffsetY) ShoulderOffsetZ]);
            syms HandOffsetX LowerArmLength real;
            AEnd4 = getA([(HandOffsetX+LowerArmLength) 0 0]);
            piby2 = str2sym('pi/2');
            syms theta_1 real;
            alpha1 = -piby2; a1 = 0; d1 = 0;
            T01 = getT(a1,alpha1,d1,theta_1);
            syms theta_2 real;
            d2 = 0; a2 = 0; alpha2 = piby2;
            T12 = getT(a2,alpha2,d2,theta_2-piby2);
            syms UpperArmLength theta_3 real;
            d3 = UpperArmLength; a3 = 0; alpha3 = -piby2;
            T32 = getT(a3,alpha3,d3,theta_3);
            syms theta_4 real;
            d4 = 0; a4 = 0; alpha4 = piby2;
            T43 = getT(a4,alpha4,d4,theta_4);
            Rz = getR('z',piby2);
            kinChain.DHChain = 'ABase0*T01*T12*T32*T43*Rz*AEnd4';
            kinChain.ABase0 = ABase0;
            kinChain.T01 = T01;
            kinChain.T12 = T12;
            kinChain.T32 = T32;
            kinChain.T43 = T43;
            kinChain.Rz = Rz;
            kinChain.AEnd4 = AEnd4;
    end
end

function A = getA(vec)
% This function returns the translation matrix.
    A = [1 0 0 vec(1);
        0 1 0 vec(2);
        0 0 1 vec(3);
        0 0 0 1];
end

function R = getR(orientation,theta)
% This function returns the rotation matrix.
    switch orientation
        case 'x'
            R = [1 0 0 0;
                0 cos(theta) -sin(theta) 0;
                0 sin(theta) cos(theta) 0
                0 0 0 1];
        case 'y'
            R = [cos(theta) 0 sin(theta) 0;
                0 1 0 0;
                -sin(theta) 0 cos(theta) 0;
                0 0 0 1];
        case 'z'
            R = [cos(theta) -sin(theta) 0 0;
                sin(theta) cos(theta) 0 0;
                0 0 1 0;
                0 0 0 1];
    end
end

function T = getT(a,alpha,d,theta)
% This function returns the Denavit-Hartenberg (DH) matrix.
    T = [cos(theta),-sin(theta),0,a;
         sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d;
         sin(theta)*sin(alpha),cos(theta)*sin(alpha),cos(alpha),cos(alpha)*d;
         0,0,0,1];
end
Для просмотра документации необходимо авторизоваться на сайте