% Jerry Pratt
% April 2, 1999
% m file for static plot of M2 for CLAWAR '99 paper.
% Single step to highlight the passive swing leg.

dir = 1.0;
x_offset = 6.0;

L = 0.406;
W = L/12;

LW = L/6;
BW = 0.22;

FOOT_H = 0.064;
FOOT_BACK = 0.082;
FOOT_FRONT = .134;

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

leg = [
-LW/2 LW/2    1
LW/2 LW/2     1
LW/2 -L-LW/20  1
-LW/2 -L-LW/20 1
-LW/2 LW/2    1
];


NN = 50;
body = zeros(NN,3);
body(:,3) = ones(NN,1);
ang = linspace(0,pi,NN-1)'
body(1:NN-1,1) = BW*cos(ang);
body(1:NN-1,2) = 2.5*BW*sin(ang);
body(NN,:) = [BW,0,1];

skip = 10  %625;
plot_skip = 1;

num_figs = 16;

plot_begin=50;
plot_end=58;

figure(1);%clf;
hold on;

for n = plot_begin:plot_end;
%for i = 1:skip:length(q_pitch);

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

t(i);

T_O_B = [
cos(q_pitch(i))   -sin(q_pitch(i))   -q_x(i)+x_offset
sin(q_pitch(i))  cos(q_pitch(i))   q_z(i)
     0                     0                  1
];

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

T_B_RH = [
cos(q_rh_pitch(i))   -sin(q_rh_pitch(i))       0
sin(q_rh_pitch(i))  cos(q_rh_pitch(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_pitch(i))   -sin(q_la_pitch(i))       0
sin(q_la_pitch(i))  cos(q_la_pitch(i))       -L
     0                     0              1];

T_RK_RA = [
cos(q_ra_pitch(i))   -sin(q_ra_pitch(i))   0
sin(q_ra_pitch(i))  cos(q_ra_pitch(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);

%if (rem(n,(num_figs*num_figs*2)) == 0)
%figure(ceil(n/(num_figs*num_figs*2))+1);clg;
%end;

%h=subplot(num_figs*num_figs,2,rem(n,num_figs*num_figs*2)+1);

%pos=get(h,'position');
%pos(1)=.9;
%pos(2)=.9;
%set(h,'position',pos);

if (rem(n,plot_skip) == 0)
%clg;
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');


