Marche_humanoide/DrawFrame.m at master · Matth26/Marche_humanoide · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
function DrawFrame(T, s, lw)
%
% function DrawFrame(T, s, lw)
%
% Draw a 3D frame associated with the
% homogeneous transformation T
%
% T: homogeneous matrx
% s: scale factor
% lw: line width
%

[m,n]=size(T);
if m ~= 4,
    disp('Invalid dimension of T');
    return;
end
if n ~= 4,
    disp ('Invalid dimension of T');
    return;
end


%s=50;
x(1)=T(1,4);y(1)=T(2,4);z(1)=T(3,4);
plot3(x,y,z);

k=1;
x(2)=x(1) + s*T(1,k);
y(2)=y(1) + s*T(2,k);
z(2)=z(1) + s*T(3,k);
plot3(x, y, z, 'r', 'LineWidth', lw);

k=2;
x(2)=x(1) + s*T(1,k);
y(2)=y(1) + s*T(2,k);
z(2)=z(1) + s*T(3,k);
plot3(x, y, z, 'g', 'LineWidth', lw);

k=3;
x(2)=x(1) + s*T(1,k);
y(2)=y(1) + s*T(2,k);
z(2)=z(1) + s*T(3,k);
plot3(x, y, z, 'b', 'LineWidth', lw);