Codes: MODERN ROBOTICS Ch.4_基于PoE的正运动学代码实现

%%1 基于PoE space form 的正运动学求解
% 输入M矩阵、螺旋轴列表Slist（column vector）、关节角向量qlist(column vector)，输出齐次变换矩阵T
function T = FKinSpacer(M, Slist, qlist)
T = M;
num = size(qlist);
for i = num(1):-1:1
    S = Slist(:,i);
    q = qlist(i);
    Sq = S .* q;
    Sq_mat = VecTose3r(Sq);
    T = MatExp6r(Sq_mat) * T;
end
end
%--------------EOF---------------

%%2 基于PoE body form的正运动学求解
% 输入M, Slist, qList, 输出T
function T = FKinBodyr(M, BList, qList)
T = M;
num = size(qList);
for i = 1:1:num(1)
   B = BList(:,i); 
   q = qList(i);
   Bq = B .* q;
   Bq_mat = VecTose3r(Bq);
   T = T * MatExp6r(Bq_mat);
end
end
%--------------EOF---------------