机械臂正运动学（1）——MDH下的正解（自编）

%% 该函数执行 MDH参数下正运动学求解功能
function [result] = myfkine_MDH(q,a,alpha,d,offset)
% q为输入的关节转角，a为杆件长度，alpha为杆件扭角，d为关节距离 ，offset为关节变量偏移（MDH参数）
% 注意：矩阵计算是现成的，但是记得加上关节变量的偏移量offset
[N,D] = size(q);
result=[];
for j = 1:1:N
    for i = 1:1:6
        T(:,:,i) = [       cos(q(j,i) + offset(i))                 -sin(q(j,i)+ offset(i))                 0                   a(i);
                    sin(q(j,i)+ offset(i))*cos(alpha(i))   cos(q(j,i)+ offset(i))*cos(alpha(i))     -sin(alpha(i))    -d(i)*sin(alpha(i));
                    sin(q(j,i)+ offset(i))*sin(alpha(i))   cos(q(j,i)+ offset(i))*sin(alpha(i))      cos(alpha(i))     d(i)*cos(alpha(i));
                              0                           0                         0                     1         ];
    end
    result(:,:,j) = T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6);
end