% Jerry Pratt
% March 31, 2000
% m file for static plot of Flamingo.  Copied from Allerton '98 paper.

dir = -1.0;

L = 0.406;
W = L/12;
V = L/4;

FOOT_H = 0.064;
FOOT_BACK = 0.134;
FOOT_FRONT = .082;


foot = [
-W/2 W/2    1
W/2 W/2     1
W/2 -FOOT_H+W  1
FOOT_BACK -FOOT_H+W 1
FOOT_BACK -FOOT_H    1
-FOOT_FRONT -FOOT_H    1
-FOOT_FRONT -FOOT_H+W    1
-W/2 -FOOT_H+W  1
-W/2 W/2 1
];

leg = [
-W/2 W/2    1
W/2 W/2     1
W/2 -L-W/2  1
-W/2 -L-W/2 1
-W/2 W/2    1
];

W = W*1.5;

body = [
3*W -W/2  1
3*W W   1
W W     1
W 2.5*W   1
-W 2.5*W  1
-W W    1
-3*W W  1
-3*W -W/2 1
3*W -W/2  1
];


body = [
V/2 -V/2  1
3*V 2*V   1
V 2.5*V   1
-V 2.5*V  1
-3*V 2*V  1
-V/2 -V/2 1
V/2 -V/2  1
];


%skip = 12.499/1.3  %625;
%plot_skip = 1;

%num_figs = 16;

figure(1);clf;

%for n = 1:length(q_theta)/skip + 1;
%for i = 1:skip:length(q_theta);

%i = floor((n-1)*skip) + 1;


% Plot Cartoon Body and Legs...
LEN = 250
% (10 means 0.2 seconds between image)
for i = 1:10:LEN

T_O_B = [
cos(q_theta(i))   -sin(q_theta(i))   -q_x(i)
sin(q_theta(i))  cos(q_theta(i))   q_z(i)
     0                     0                  1
];

T_B_LH = [
cos(q_lh(i))   -sin(q_lh(i))       0
sin(q_lh(i))  cos(q_lh(i))       0
     0                     0              1
];

T_B_RH = [
cos(q_rh(i))   -sin(q_rh(i))       0
sin(q_rh(i))  cos(q_rh(i))       0
     0                     0              1
];


T_LH_LK = [
cos(q_lk(i))   -sin(q_lk(i))       0
sin(q_lk(i))  cos(q_lk(i))       -L
     0                     0              1];

T_RH_RK = [
cos(q_rk(i))   -sin(q_rk(i))   0
sin(q_rk(i))  cos(q_rk(i))   -L
     0                     0          1];


T_LK_LA = [
cos(q_la(i))   -sin(q_la(i))       0
sin(q_la(i))  cos(q_la(i))       -L
     0                     0              1];

T_RK_RA = [
cos(q_ra(i))   -sin(q_ra(i))   0
sin(q_ra(i))  cos(q_ra(i))   -L
     0                     0          1];


body_2 = (T_O_B * body')';

left_upper_leg = (T_O_B * T_B_LH * leg')';
left_lower_leg = (T_O_B * T_B_LH * T_LH_LK * leg')';
left_foot = (T_O_B * T_B_LH * T_LH_LK * T_LK_LA * foot')';

right_upper_leg = (T_O_B * T_B_RH * leg')';
right_lower_leg = (T_O_B * T_B_RH * T_RH_RK * leg')';
right_foot = (T_O_B * T_B_RH * T_RH_RK * T_RK_RA * foot')';


figure(1);

plot(-dir*body_2(:,1),body_2(:,2));hold on;

plot(-dir*left_upper_leg(:,1),left_upper_leg(:,2));
plot(-dir*left_lower_leg(:,1),left_lower_leg(:,2));
plot(-dir*left_foot(:,1),left_foot(:,2));

plot(-dir*right_upper_leg(:,1),right_upper_leg(:,2),':');
plot(-dir*right_lower_leg(:,1),right_lower_leg(:,2),':');
plot(-dir*right_foot(:,1),right_foot(:,2),':');

%pause;
%end

%axis([-.6,3.2,0,1.0]);
%axis('off');



%axis off;
%pause;
end;

axis('equal');
axis('off');


plot(dir*q_x(1:LEN),q_z(1:LEN),'.');

lt = zeros(LEN,1);
rt = zeros(LEN,1);

for i = 1:LEN; 
%length(q_theta);

T_O_B = [
cos(q_theta(i))   -sin(q_theta(i))   -q_x(i)
sin(q_theta(i))  cos(q_theta(i))   q_z(i)
     0                     0                  1
];

T_B_LH = [
cos(q_lh(i))   -sin(q_lh(i))       0
sin(q_lh(i))  cos(q_lh(i))       0
     0                     0              1
];


T_LH_LK = [
cos(q_lk(i))   -sin(q_lk(i))       0
sin(q_lk(i))  cos(q_lk(i))       -L
     0                     0              1];


T_LK_LA = [
cos(q_la(i))   -sin(q_la(i))       0
sin(q_la(i))  cos(q_la(i))       -L
     0                     0              1];


T_LA_LT = [
1   0       FOOT_BACK
0   1      -FOOT_H
0   0       1];


T_B_RH = [
cos(q_rh(i))   -sin(q_rh(i))       0
sin(q_rh(i))  cos(q_rh(i))       0
     0                     0              1
];


T_RH_RK = [
cos(q_rk(i))   -sin(q_rk(i))   0
sin(q_rk(i))  cos(q_rk(i))   -L
     0                     0          1];

T_RK_RA = [
cos(q_ra(i))   -sin(q_ra(i))   0
sin(q_ra(i))  cos(q_ra(i))   -L
     0                     0              1];


T_RA_RT = [
1   0       FOOT_BACK
0   1      -FOOT_H
0   0       1];



left_toe = (T_O_B * T_B_LH * T_LH_LK * T_LK_LA * T_LA_LT);
right_toe = (T_O_B * T_B_RH * T_RH_RK * T_RK_RA * T_RA_RT);

lt_x(i) = left_toe(1,3);
lt_z(i) = left_toe(2,3);

rt_x(i) = right_toe(1,3);
rt_z(i) = right_toe(2,3);


end;

plot(-dir*lt_x,lt_z);
plot(-dir*rt_x,rt_z,':');

