Personal tools

draw.m

draw.m — Objective-C source code, 2Kb

File contents

function [kk]=draw(X)
theta    = X(1);
phi      = X(2);
thetap   = X(3);
phip     = X(4);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------+--------------------------+-------------------+--------------
% Quantity  | Value                    | Units             | Description
%-----------|--------------------------|-------------------|--------------
GAMMA       = 0.009;            % ground slope, (rad)
L           = 1.00;             % leg length, (m)
g           = 1.00;             % "gravitational acceleration constant"
BETA        = 1/100;            % mass foot/mass hip, (dimensionless)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%% DIRECTION COSINE MATRICES
% N = Newtonian Frame
% F = "Floor" Frame (describes ground slope) 
% C = Contact Leg Frame (A.K.A. (stance leg or theta leg)
% S = Swing Leg Frame (A.K.A. phi leg): 
% GAMMA: measured in a CCW sense about -n3>
% theta: measured in a CW sense about f3>
% phi:   measured in a CCW sesnse about -c3>
        N_F = [ cos(GAMMA),   sin(GAMMA),   0; ...
               -sin(GAMMA),   cos(GAMMA),   0; ...
                0,            0,           -1];
        F_C = [ cos(theta),  -sin(theta),   0;  ...
                sin(theta),   cos(theta),   0; ... 
                0,            0,           +1];
        C_S = [ cos(phi),     sin(phi),     0; ...
               -sin(phi),     cos(phi),     0; ...
                0,            0,           -1];

% Ground Slope
gg=zeros(2,3);
gg(1,:)=[-1.5*cos(GAMMA),+1.5*sin(GAMMA),0];   % ground (1st point)
gg(2,:)=[+1.5*cos(GAMMA),-1.5*sin(GAMMA),0];   % ground (2nd point)

% Walker
pp=zeros(3,3);
pp(1,:)=[0,0,0];                               % locates CPth
pp(2,:)=pp(1,:)+(N_F*F_C*[0;L;0])';            % locates hip mass
pp(3,:)=pp(2,:)+(N_F*F_C*C_S*[0;-L;0])';       % locates CPph


% Ground Points and Point Masses expressed in 2D (Real,Imag) Coordinates 
pp_x(1)=gg(1,1);        pp_y(1)=gg(1,2);       % left end of ground plane
pp_x(2)=gg(2,1);        pp_y(2)=gg(2,2);       % right end of ground plane
pp_x(3)=pp(1,1);        pp_y(3)=pp(1,2);       % locates CPth
pp_x(4)=pp(2,1);        pp_y(4)=pp(2,2);       % locates hip mass
pp_x(5)=pp(3,1);        pp_y(5)=pp(3,2);       % locates CPph

% Plot the plot points
hold on
plot(pp_x(1:2),pp_y(1:2),'k-');
plot(pp_x(3:4),pp_y(3:4),'bo-','LineWidth',3);
plot(pp_x(4:5),pp_y(4:5),'go-'),grid;
% axis([-0.70 0.70 -0.1 0.90]);
title('Walker Configuration')
xlabel('X-Coordinates')
ylabel('Y-Coordinates')
hold off
% pause(0.06),clf

end


Document Actions