ヒューマノイドロボットの逆運動学の解析解

この例では、ヒューマノイドロボットの頭部連鎖の逆運動学に対する解析解を導き出す方法を示します。

手順 1: パラメーターの定義

Kofinas らの研究 [2] に基づいた Denavit-Hartenberg (DH) パラメーターと表記法を使用して、NAO ヒューマノイドロボット [1] の頭部連鎖のリンク (胴体と頭部との間のリンク) の運動学を説明します。以下の変換は頭部連鎖のリンクを定義します。

$T = A_{B a s e, 0} T_{0, 1} T_{1, 2} R_{x} R_{y} A_{2, H e a d}$

ここで

$A_{B a s e, 0}$ はベース (胴体) からジョイントまたは基準座標系 0 への変換
$T_{0, 1}$ は 0 を基準とする参照 1 の向き
$T_{1, 2}$ は 1 を基準とする参照 2 の向き
$R_{x}$ はロールの回転
$R_{y}$ はピッチの回転
$A_{2, H e a d}$ は参照 2 からエンドエフェクタ点 (頭部) への変換

$T (1 : 3, 4)$ は頭部の座標 ( $x c$ 、 $y c$ 、 $z c$ ) を定義します。

この例では、到達可能な空間内で頭部の座標 xc、yc、および zc が与えられた頭部連鎖のリンクで個々のジョイントの向きをすべて返すことにより、逆運動学問題を解析的に解きます。次に、効率性を高めるため、解析結果を数値のみの関数に変換します。

解析的に解くことで (可能な場合)、リアルタイムに計算を実行し、特異性を回避できます。ただし、数値的な方法は難しくなります。

手順 2: DH パラメーターを使用した頭部連鎖の順運動学の定義

関数 getKinematicChain は NAO ロボットの特定の運動学的連鎖をシンボリック変数として返します。getKinematicChain の詳細については、補助関数のセクションを参照してください。

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

T = 
 $(\begin{array}{c} r_{11} & r_{12} & r_{13} & xc \\ r_{21} & r_{22} & r_{23} & yc \\ r_{31} & r_{32} & r_{33} & zc \\ 0 & 0 & 0 & 1 \end{array})$

順運動学の変換行列 T を T = ABase0*T01*T12*Rx*Ry*A2Head の一連の積として表します。個々の行列を次のように定義します。

胴体 (ベース) からジョイント 0 への変換行列を指定します。

ABase0 = dhInfo.ABase0

ABase0 = 
 $(\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & NeckOffsetZ \\ 0 & 0 & 0 & 1 \end{array})$

ジョイント 0 からジョイント 1 への変換行列を指定します。

T01 = dhInfo.T01

T01 = 
 $(\begin{array}{c} \cos (θ_{1}) & - \sin (θ_{1}) & 0 & 0 \\ \sin (θ_{1}) & \cos (θ_{1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

ジョイント 1 からジョイント 2 への変換行列を指定します。

T12 = dhInfo.T12

T12 = 
 $(\begin{array}{c} \cos (θ_{2} - \frac{π}{2}) & - \sin (θ_{2} - \frac{π}{2}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ - \sin (θ_{2} - \frac{π}{2}) & - \cos (θ_{2} - \frac{π}{2}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

ロールの回転行列を指定します。

Rx = dhInfo.Rx

Rx = 
 $(\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

ピッチの回転行列を指定します。

Ry = dhInfo.Ry

Ry = 
 $(\begin{array}{c} 0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 \\ - 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

ジョイント 2 から頭部への変換行列を指定します。

A2Head = dhInfo.A2Head

A2Head = 
 $(\begin{array}{c} 1 & 0 & 0 & CameraX \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & CameraZ \\ 0 & 0 & 0 & 1 \end{array})$

この問題の既知のパラメーターは 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 = 
 $(\begin{array}{c} xc \cos (θ_{1}) - CameraZ \sin (θ_{2}) - CameraX \cos (θ_{2}) + yc \sin (θ_{1}) \\ yc \cos (θ_{1}) - xc \sin (θ_{1}) \\ zc - NeckOffsetZ - CameraZ \cos (θ_{2}) + CameraX \sin (θ_{2}) \end{array})$

この方程式系には、解を求めるための 2 つの変数 $θ_{1}$ と $θ_{2}$ が含まれています。ただし、この方程式は xc、yc、および zc を任意に選択できないことも示しています。そのため、yc も変数として考慮します。系のその他すべての未知なものは、シンボリックパラメーターです。

この例は、逆運動学問題を解くための一般的な代数アプローチに従います [3]。考え方は、各変数の式が、既に方程式を解いたパラメーターおよび変数として、解のコンパクトな表現を取得することです。最初のステップとして、 $θ_{1}$ または $θ_{2}$ をパラメーターとして求めます。次に、既知の変数およびパラメーターとしてその他の変数を表します。これを行うには、変数が 1 つだけ含まれている方程式を特定することから始めます。

intersect(symvar(eqns(1)),[theta_1,theta_2,yc])

ans =  $(\begin{array}{c} θ_{1} & θ_{2} & yc \end{array})$

intersect(symvar(eqns(2)),[theta_1,theta_2,yc])

ans =  $(\begin{array}{c} θ_{1} & yc \end{array})$

intersect(symvar(eqns(3)),[theta_1,theta_2,yc])

ans =  $θ_{2}$

3 番目の方程式には変数 $θ_{2}$ が 1 つだけ含まれています。この方程式を最初に解きます。

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

theta_2_sol = 
 $(\begin{array}{c} 2 π k - 2 atan (\frac{CameraX - \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \\ 2 π k - 2 atan (\frac{CameraX + \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \end{array})$

theta_2_params =  $k$

theta_2_conds = 
 $(\begin{array}{c} k \in Z \land CameraZ + zc \neq NeckOffsetZ \land {(NeckOffsetZ - zc)}^{2} \leq {CameraX}^{2} + {CameraZ}^{2} \\ k \in Z \land CameraZ + zc \neq NeckOffsetZ \land {(NeckOffsetZ - zc)}^{2} \leq {CameraX}^{2} + {CameraZ}^{2} \end{array})$

解にはパラメーター $k$ をもつ加法的な $2 π k$ 項があります。汎化を失うことなく、 $k$ を解および条件で 0 に等しくなるように設定できます。

theta_2_sol = subs(theta_2_sol,theta_2_params,0)

theta_2_sol = 
 $(\begin{array}{c} - 2 atan (\frac{CameraX - \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \\ - 2 atan (\frac{CameraX + \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \end{array})$

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 = 
 $\begin{array}{l} (\begin{array}{c} 2 π k - σ_{1} \\ σ_{2} + 2 π k \\ 2 π k - σ_{2} \\ 2 π k - \frac{π}{2} \\ σ_{1} + 2 π k \\ 2 atan (x) + 2 π k \\ \frac{π}{2} + 2 π k \\ 2 π k - \frac{π}{2} \\ \frac{π}{2} + 2 π k \end{array}) \\ where \\ σ_{1} = 2 atan (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ σ_{2} = 2 atan (\frac{\frac{σ_{3}}{σ_{5} + 1} + \frac{σ_{5} σ_{3}}{σ_{5} + 1}}{σ_{4}}) \\ σ_{3} = \sqrt{- (xc - CameraX + CameraX σ_{5} + xc σ_{5} - 2 CameraZ \tan (\frac{θ_{2}}{2})) σ_{4}} \\ σ_{4} = CameraX + xc - CameraX σ_{5} + xc σ_{5} + 2 CameraZ \tan (\frac{θ_{2}}{2}) \\ σ_{5} = {\tan (\frac{θ_{2}}{2})}^{2} \end{array}$

yc_sol

yc_sol = 
 $\begin{array}{l} (\begin{array}{c} \sqrt{(CameraX + xc) (CameraX - xc)} \\ σ_{1} \\ - σ_{1} \\ CameraX \\ - \sqrt{(CameraX + xc) (CameraX - xc)} \\ 0 \\ σ_{2} \\ - σ_{2} \\ - CameraX \end{array}) \\ where \\ σ_{1} = \frac{\sqrt{- (xc - CameraX + σ_{3} + xc {\tan (\frac{θ_{2}}{2})}^{2} - σ_{4}) (CameraX + xc - σ_{3} + xc {\tan (\frac{θ_{2}}{2})}^{2} + σ_{4})}}{{\tan (\frac{θ_{2}}{2})}^{2} + 1} \\ σ_{2} = \frac{- σ_{3} + σ_{4} + CameraX}{{\tan (\frac{θ_{2}}{2})}^{2} + 1} \\ σ_{3} = CameraX {\tan (\frac{θ_{2}}{2})}^{2} \\ σ_{4} = 2 CameraZ \tan (\frac{θ_{2}}{2}) \end{array}$

解は $k$ および $x$ の 2 つのパラメーターによって異なります。

yc_theta_1_params

yc_theta_1_params =  $(\begin{array}{c} k & x \end{array})$

解には加法的な $2 π k$ 項があります。汎化を失うことなく、 $k$ を theta_1_sol の解および条件 yc_theta_1_conds で 0 に等しくなるように設定できます。

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

theta_1_sol = 
 $\begin{array}{l} (\begin{array}{c} - 2 atan (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ σ_{1} \\ - σ_{1} \\ - \frac{π}{2} \\ 2 atan (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ 0 \\ \frac{π}{2} \\ - \frac{π}{2} \\ \frac{π}{2} \end{array}) \\ where \\ σ_{1} = 2 atan (\frac{\sqrt{(CameraX \cos (θ_{2}) - xc + CameraZ \sin (θ_{2})) (xc + CameraX \cos (θ_{2}) + CameraZ \sin (θ_{2}))}}{xc + CameraX \cos (θ_{2}) + CameraZ \sin (θ_{2})}) \end{array}$

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}$ に対する既知の数値の任意のセットから開始して、順運動学でエンドエフェクタの位置 xc、yc、および zc の数値を計算します。次に、前の計算からの逆運動学式を使用して、xc、yc、zc から逆方向に、 $θ_{1}$ および $θ_{2}$ に対して可能な数値をすべて計算します。逆の解の 1 つのセットが $θ_{1}$ および $θ_{2}$ に対して開始した数値セットと一致することを確認します。

ロボットの固定パラメーターを設定します。この例の値は説明目的のみで提供されています。

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

順方向の計算を使用して、値 $θ_{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]);

順運動学では、 $θ_{1}$ および $θ_{2}$ に対する任意の開始値のセットが含まれる 2 つの変数 theta_1_known および theta_2_known を指定します。

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_2 の解ごとに num_theta_1 の解がある可能性に注意してください。

次の一連の操作を使用して解を確認します。

(theta_1_known,theta_2_known) をループします。各点で、終了位置 x_known、y_known、および z_known を計算します。その後、これらは "既知" になります。
x_known と z_known に対応する $θ_{2}$ の解をループします。各解について、対応する条件 cond_theta_2 が有効かどうかを調べます。条件が有効な場合、対応する数値解 theta_2_derived を計算します。
x_known、z_known、theta_2_derived に対応する $θ_{1}$ の解をループします。各解について、対応する条件 cond_theta_1 が有効かどうかを調べます。条件が有効な場合、条件 cond_yc を介した相対許容誤差と絶対許容誤差内で y_derived が数値的に y_known と一致するかどうかを調べます。この条件が有効である場合は、theta_1_derived を計算します。
表示目的で table 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_known,theta_2_known) の各ペアに対し、逆運動学を使用して取得した 2 つの可能性のある解のペア (theta_1_derived,theta_2_derived) があることを確認します。逆の解の 1 つのセットが開始角と一致します。

参考文献

SoftBank Robotics. NAO. https://www.aldebaran.com/en/nao.
Kofinas, N., E. Orfanoudakis, and M. G. Lagoudakis"Complete Analytical Inverse Kinematics for NAO"In 2013 13th International Conference on Autonomous Robot Systems.Lisbon, Portugal: Robotica, 2013
Kendricks, K."Solving the Inverse Kinematic Robotics Problem: A Comparison Study of the Denavit-Hartenberg Matrix and Groebner Basis Theory."Ph.D. Thesis.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

参考

関数

simplify | solve | subs | double

ヒューマノイド ロボットの逆運動学の解析解