matlab dynamic model which allows the visualisation of the subject walking (one leg only) as a stick figure, up to the centre of mass, calculated as geometric model functions of joint angles and foot to ground angle.


____________________________________________________________
STA = csvread('Staticvalues.csv');
ANG = csvread('Book1.csv');
iGL = [1 0 0];% define x axes versor (a)
HEEL= ANG(:,4:5);
TOE= ANG(:,6:7);
bTIME = zeros(100,2);
vpTIME = zeros(100,1);
thGTIME = zeros(100,3);
AJCTIME= zeros(100,2);
KJCTIME= zeros(100,2);
HJCTIME= zeros(100,2);
HCMTIME= zeros(100,2);

%segment length in metres
LHM = STA(1,1)/1000;
LAM = STA(1,2)/1000;
LAK = STA(1,3)/1000;
LKH = STA(1,4)/1000;
LHCM = STA(1,5)/1000;
LHA = STA(1,8)/1000;

for i=1:100
    % angle in stance phase in radians
    thD = -ANG(i,1)*pi/180;% Ankle dorsi/plantarflexion
    thE = ANG(i,2)*pi/180; % Knee flexion/extension 
    thF = ANG(i,3)*pi/180; % Hip flexion/extension
    % constant angles at the foot in radians
    Lalpha = STA(1,6); % Heel angle 
    Lbeta = STA(1,7); %Toe angle
    
    %calculation of foot to ground angle
    C(1,:) = HEEL(i,:)/1000; %calacaneus
    Cx=C(1,1);
    Cy=C(1,2);
    M(1,:) = TOE(i,:)/1000;%Mid Foot
    Mx = M(1,1);
    My = M(1,2);
    b= (M-C)./norm(M-C);
    bTIME(i,:) = b;
    b= [b(1,1) b(1,2) 0];
    vp = cross(iGL,b); % vector product between a and b
    vpTIME(i,1)= vp(1,3);
    thG = asin(vp(1,3)); %angle to the ground of the foot 
    thGTIME(i,:) = thG;
    if thG >= 0
        xA = Cx+LHA*cos(thG+Lalpha);
        yA = Cy+LHA*sin(thG+Lalpha);
    else
        xA = Mx-LAM*cos(thG-Lbeta);
        yA = My-LAM*sin(thG-Lbeta);    
    end
    AJCTIME(i,1) = xA;
    AJCTIME(i,2) = yA;
    xK = xA+LAK*cos(thD+thG+pi/2);
    yK = yA+LAK*sin(thD+thG+pi/2); 
    KJCTIME(i,1) = xK;
    KJCTIME(i,2) = yK;
    xH = xK+LKH*cos(thE+thD+thG+pi/2);
    yH = yK+LKH*sin(thE+thD+thG+pi/2); 
    HJCTIME(i,1) = xH;
    HJCTIME(i,2) = yH;
    xCM = xH+LHCM*cos(thE+thD-thF+thG+pi/2);
    yCM = yH+LHCM*sin(thE+thD-thF+thG+pi/2);
    HCMTIME(i,1)= xCM;
    HCMTIME(i,2)= yCM;
    if thG > 0
        figure(1)
        hold off
        plot([ Cx xA ],[Cy yA ],'o-b',Cx,Cy,'xr',xA,yA,'xr')
        hold on
        plot([ xA xK ],[yA yK ],'o-b',xA,yA,'xr',xK,yK,'xr')
        plot([ Mx Cx ],[My Cy],'o-b',Mx,My,'xr')
        plot([ Mx xA ],[My yA],'o-b')
        plot([ xK xH ],[yK yH ],'o-b',xK,yK,'xr',xH,yH,'xr')
        plot([ xH xCM ],[yH yCM ],'o-b',xH,yH,'xr',xCM,yCM,'xr')
%         axis([ -100 80 0  3 ])
        grid on
        axis equal
        pause(0.05);
    else
        figure(1)
        hold off
        plot([ Mx xA ],[My yA ],'o-b',Mx,My,'xr',xA,yA,'xr')
        hold on
        plot([ Mx Cx ],[My Cy],'o-b',Cx,Cy,'xr')
        plot([ Cx xA ],[Cy yA],'o-b')
        plot([ xA xK ],[yA yK ],'o-b',xA,yA,'xr',xK,yK,'xr')
        plot([ xK xH ],[yK yH ],'o-b',xK,yK,'xr',xH,yH,'xr')
        plot([ xH xCM ],[yH yCM ],'o-b',xH,yH,'xr',xCM,yCM,'xr')
%         axis([ -100 80 0  3 ])
        grid on
        axis equal
        pause(0.05);
    end
end