【個人的備忘録】7-DoFのロボットアームの手先位置をMATLABで求めてみた

What?

　手計算だと不安だったので，検算のために下図の７DoFのロボットアームの手先位置をMATLABで求めてみた．
ついでに有顔ベクトルも求めた．

実行コード

syms theta1 theta2 theta3 theta4 theta5 theta6 theta7
syms l1 l2 l3 l4

%手先位置ベクトル

%%回転行列
R1 = [cos(theta1), 0, sin(theta1);
    0, 1, 0;
    -sin(theta1), 0, cos(theta1)]

R2 = [1, 0, 0;
    0, cos(theta2), -sin(theta2);
    0, sin(theta2), cos(theta2)]

R3 = [cos(theta3), -sin(theta3), 0;
    sin(theta3), cos(theta3), 0;
    0, 0, 1]

R4 = [1, 0, 0;
    0, cos(theta4), -sin(theta4);
    0, sin(theta4), cos(theta4)];

R5 = [cos(theta5), 0, sin(theta5)
    0, 1, 0;
    -sin(theta5), 0, cos(theta5)]

R6 = [1, 0, 0;
    0, cos(theta6), -sin(theta6);
    0, sin(theta6), cos(theta6)];

R7 = [cos(theta7), -sin(theta7), 0;
    sin(theta7), cos(theta7), 0;
    0, 0, 1]

%%初期状態のリンクベクトル
l1 = [l1;
    0;
    0]
l2 = [0;
    0;
    -l2]
l3 = [0;
    l3;
    0]
l4 = [0;
    l4;
    0]

%%演算
L1 = l1
L2 = R1*R2*R3*l2
L3 = R1*R2*R3*R4*R5*l3
L4 = R1*R2*R3*R4*R5*R6*R7*l4

Pr = L1 + L2 + L3 + L4

%有顔ベクトル
%%初期状態の有顔ベクトル
a = [0;
    1;
    0]
b = [0;
    0;
    1]

%%演算
A = R1*R2*R3*R4*R5*R6*R7*a
B = R1*R2*R3*R4*R5*R6*R7*b

結果は手計算と同じだったので安心．