% Jerry Pratt
% April 2, 1999

% Plot M2 Simulation walking data for CLAWAR '99 paper.

figure(2);clf;

stop_time = 6.0;

subplot(3,3,4);plot(t,q_x);
axis([0 stop_time 0 6]);
hy = ylabel('x(meters)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Bottom');

subplot(3,3,6);plot(t,q_z);
axis([0 stop_time 0.90 0.96]);
hy = ylabel('z(meters)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Bottom');

subplot(3,3,5);plot(t,q_y);
axis([0 stop_time 0.1 0.3]);
hy = ylabel('y(meters)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Bottom');

subplot(3,3,2);plot(t,xd);
axis([0 stop_time 0 1.5]);
hy = ylabel('forward vel (m/s)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Bottom');

subplot(3,3,3);plot(t,yd);
axis([0 stop_time -0.5 0.5]);
hy = ylabel('lateral vel (m/s)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Bottom');

subplot(3,3,7);plot(t,q_pitch);
axis([0 stop_time -0.05 0.05]);
hy = ylabel('pitch(rad)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Top');
hx = xlabel('time(sec)');
set(hx,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Top');

subplot(3,3,8);plot(t,q_roll);
axis([0 stop_time -0.05 0.05]);
hy = ylabel('roll(rad)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Top');
hx = xlabel('time(sec)');
set(hx,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Top');

subplot(3,3,9);plot(t,q_yaw);
axis([0 stop_time -0.05 0.05]);
hy = ylabel('yaw(rad)');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Top');
hx = xlabel('time(sec)');
set(hx,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Top');

subplot(3,3,1);plot(t,left_state);
hold on;plot(t,right_state,':');
axis([0 stop_time 0 3]);
hy = ylabel('state');
set(hy,'FontSize',14,'FontName','NewCenturySchlbk','VerticalAlignment','Bottom');
%xlabel('time(sec)');



%subplot(3,3,4);plot(t,pow);
%axis([0 stop_time 0 40]);
%ylabel('total power (Watts)');
%xlabel('time(sec)');

%subplot(3,3,7);plot(t,lh_pow);
%axis([0 stop_time -20 20]);
%ylabel('left hip power (Watts)');
%xlabel('time(sec)');

%subplot(3,3,8);plot(t,lk_pow);
%axis([0 stop_time -20 20]);
%ylabel('left knee power (Watts)');
%xlabel('time(sec)');

%subplot(3,3,9);plot(t,la_pow);
%axis([0 stop_time -20 20]);
%ylabel('left ankle power (Watts)');
%xlabel('time(sec)');






