25 lines
924 B
Mathematica
25 lines
924 B
Mathematica
|
function pos = ur_fk(q, ur)
|
||
|
|
|
||
|
|
T_pk = zeros(4,4,6); % transformation between links
|
||
|
|
T_0k = zeros(4,4,7); T_0k(:,:,1) = eye(4);
|
||
|
|
for i = 1:6
|
||
|
|
jnt_axs_k = str2num(ur.robot.joint{i}.axis.Attributes.xyz)';
|
||
|
|
% Transformation from parent link frame p to current joint frame
|
||
|
|
rpy_k = str2num(ur.robot.joint{i}.origin.Attributes.rpy);
|
||
|
|
R_pj = RPY(rpy_k);
|
||
|
|
p_pj = str2num(ur.robot.joint{i}.origin.Attributes.xyz)';
|
||
|
|
T_pj = [R_pj, p_pj; zeros(1,3), 1]; % to avoid numerical errors
|
||
|
|
% Tranformation from joint frame of the joint that rotaties body k to
|
||
|
|
% link frame. The transformation is pure rotation
|
||
|
|
R_jk = Rot(q(i), jnt_axs_k);
|
||
|
|
p_jk = zeros(3,1);
|
||
|
|
T_jk = [R_jk, p_jk; zeros(1,3),1];
|
||
|
|
% Transformation from parent link frame p to current link frame k
|
||
|
|
T_pk(:,:,i) = T_pj*T_jk;
|
||
|
|
% Transformation to base
|
||
|
|
T_0k(:,:,i+1) = T_0k(:,:,i)*T_pk(:,:,i);
|
||
|
|
end
|
||
|
|
pos = T_0k(1:3,4,7);
|
||
|
|
|
||
|
|
% (T_0k(1:3,4,7)-T_0k(1:3,4,5))'
|