function cg_draw(X,isLF)
% Function to plot compass gait walker
% Katie Byl (1/19/2010). ECE594D, Winter 2010

% Constants defining geometry and mass distribution:
cg_params

q1 = X(2)+pi/2; % stance leg, wrt x axis (for plotting, only)
q2 = X(1)+pi/2-pi; % swing (non-stance) leg, wrt x axis (for plotting, only)

c1 = [0 0 1];  % Color for link 1
c2 = [1 0 0];  % Color for link 2
if ~isLF % then switch colors for legs...
    ctemp = c1;
    c1=c2; c2=ctemp;
end

c_hip = [.5 .5 .5];  % Color for mass at hip

xhip = L*cos(q1);
yhip = L*sin(q1);

cla; hold on % clear axes; turn "hold" on

% Draw the GROUND, at slope -gam_slope:
plot([-2 2]*cos(-gam_slope),[-2 2]*sin(-gam_slope),'k-','LineWidth',2)

% Draw the WALKER:
plot([0 xhip],[0 yhip],'k-','LineWidth',3,'Color',c1);
plot(xhip+[0 L]*cos(q2),yhip+[0 L]*sin(q2),...
    'k-','LineWidth',3,'Color',c2);
plot(xhip,yhip,'ko','MarkerSize',10,'MarkerFaceColor',c_hip);
plot(a*cos(q1),a*sin(q1),'ko','MarkerSize',10,'MarkerFaceColor',c1);
plot(xhip+b*cos(q2),yhip+b*sin(q2),...
    'ko','MarkerSize',10,'MarkerFaceColor',c2);

axis equal; axis off
xswing=xhip+cos(q2);
yswing=yhip+sin(q2);
axis([1/3*(xhip+xswing)+[-1 1], ...
    yhip+[-1.5 .5]])


