台大 林沛群 《机器人学》Quiz3 matlab代码（matlab2020b）

clc
clear
%2.
%首先建立DH表
%i    alpha(i-1)    a(i-1)    d(i)    theta(i)
%1    0    0    0    theta1
%2    -90    0    220    theta2
%3    0    430    -90    theta3
%4    -90    0    430    theta4
%5    90    0    0    theta5
%6    -90    0    0    theta6

%参考第四版《机器人学导论》第53页的DH表到变换矩阵的公式定义符号变量，之后再将数值带入
syms theta_i d_i alpha_i_1 a_i_1
T_ = [cosd(theta_i) -sind(theta_i) 0 a_i_1;
        sind(theta_i)*cosd(alpha_i_1) cosd(theta_i)*cosd(alpha_i_1) -sind(alpha_i_1) -sind(alpha_i_1)*d_i;
        sind(theta_i)*sind(alpha_i_1) cosd(theta_i)*sind(alpha_i_1) cosd(alpha_i_1) cosd(alpha_i_1)*d_i;
        0 0 0 1]
T01 = double(subs(T_, [theta_i d_i alpha_i_1 a_i_1], [15 0 0 0]))
T12 = double(subs(T_, [theta_i d_i alpha_i_1 a_i_1], [-40 220 -90 0]))
T23 = double(subs(T_, [theta_i d_i alpha_i_1 a_i_1], [-50 -90 0 430]))
T34 = double(subs(T_, [theta_i d_i alpha_i_1 a_i_1], [30 430 -90 0]))
T45 = double(subs(T_, [theta_i d_i alpha_i_1 a_i_1], [70 0 90 0]))
T56 = double(subs(T_, [theta_i d_i alpha_i_1 a_i_1], [25 0 -90 0]))

T04 = T01*T12*T23*T34
T06 = T04*T45*T56