40 lines
1.3 KiB
Mathematica
40 lines
1.3 KiB
Mathematica
|
function plnr_visualize(q, plnr)
|
||
|
|
|
||
|
|
figure
|
||
|
|
for j = 1:10:length(q)
|
||
|
|
clf
|
||
|
|
qj = q(j,:);
|
||
|
|
|
||
|
|
T_0k(:,:,1) = eye(4);
|
||
|
|
for i = 1:2
|
||
|
|
R_pj = RPY(str2num(plnr.robot.joint{i}.origin.Attributes.rpy));
|
||
|
|
p_pj = str2num(plnr.robot.joint{i}.origin.Attributes.xyz)';
|
||
|
|
T_pj = [R_pj, p_pj; zeros(1,3), 1];
|
||
|
|
|
||
|
|
R_jk = Rot(qj(i),plnr.k(:,i));
|
||
|
|
p_jk = zeros(3,1);
|
||
|
|
T_jk = [R_jk, p_jk; zeros(1,3),1];
|
||
|
|
T_pk(:,:,i) = T_pj*T_jk;
|
||
|
|
|
||
|
|
T_0k(:,:,i+1) = T_0k(:,:,i)*T_pk(:,:,i);
|
||
|
|
z_0k(:,i) = T_0k(1:3,1:3,i+1)*plnr.k(:,i);
|
||
|
|
r_0k(:,i) = [eye(3),zeros(3,1)]*T_0k(:,:,i+1)*[plnr.r_com(:,i);1];
|
||
|
|
end
|
||
|
|
R_pee = RPY(str2num(plnr.robot.joint{3}.origin.Attributes.rpy));
|
||
|
|
p_pee = str2num(plnr.robot.joint{3}.origin.Attributes.xyz)';
|
||
|
|
T_pee = [R_pee, p_pee; zeros(1,3), 1];
|
||
|
|
T_0ee = T_0k(:,:,3)*T_pee;
|
||
|
|
|
||
|
|
MARKER_SIZE = 100; %marker size of the scatter function
|
||
|
|
scatter(T_0k(1,4,2), T_0k(3,4,2),MARKER_SIZE,'k','filled')
|
||
|
|
hold on
|
||
|
|
scatter(T_0k(1,4,3), T_0k(3,4,3),MARKER_SIZE,'k','filled')
|
||
|
|
% scatter(Tee0(1,3), Tee0(2,3))
|
||
|
|
line([T_0k(1,4,2) T_0k(1,4,3)], [T_0k(3,4,2), T_0k(3,4,3)],'Color','k','LineWidth',1.5)
|
||
|
|
line([T_0k(1,4,3) T_0ee(1,4)], [T_0k(3,4,3), T_0ee(3,4)],'Color','k','LineWidth',1.5)
|
||
|
|
xlim([-0.5 0.5])
|
||
|
|
ylim([-0.6 0.2])
|
||
|
|
|
||
|
|
pause(1e-1)
|
||
|
|
end
|