%{ Description: This script is an implementation of the model by Taga et al. See 'A model of the neuro-musculo-skeletal system for human locomotion - I. emergence of basic gait' (1995) for more information. %} MODEL = 'Non-faller OA'; close all clear cdt = clock; disp(['start time: ',num2str(cdt(4)),'h ',num2str(round(cdt(5))),'m']) tic %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % differences from published results % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %{ %%%%%%%%%%%%%% % Equation 8 % %%%%%%%%%%%%%% published: du_i = (-u_i - beta*f(v_i) + sigma(w_ij*f(u_j)) + u0 + Q_i + S_i)/tau_i revised: du_i = (-u_i - beta*f(v_i) + SI*(sigma(w_ij*f(u_j)) + u0 + Q_i + S_i))/tau_i NOTES: 1) not multiplying S_i by SI also works. 2) SI = 0.945, 0.95, and 0.955 work for 60-second simulations %%%%%%%%%%%%%% % Equation 8 % %%%%%%%%%%%%%% published: Tp(1,4) = -b(2)*(dtinc(1,3)-dtinc(1,5)) - b(4)*f(tinc(1,3)-tinc(1,5))*(dtinc(1,3)-dtinc(1,5)) - k(1)*f(tinc(1,3)-tinc(1,5)) - b(3)*f(tinc(1,5)-tinc(1,3)-5*pi/6)*(dtinc(1,3)-dtinc(1,5)) - k(1)*f(tinc(1,5)-tinc(1,3)-5*pi/6); Tp(1,7) = b(2)*(dtinc(1,6)-dtinc(1,8)) + b(3)*f(tinc(1,6)-tinc(1,8)-0.576-2*pi/9)*(dtinc(1,6)-dtinc(1,8)) + k(1)*f(tinc(1,6)-tinc(1,8)-0.576-2*pi/9) + b(3)*f(tinc(1,8)-tinc(1,6)-5*pi/18+0.576)*(dtinc(1,6)-dtinc(1,8)) - k(1)*f(-tinc(1,8)-tinc(1,6)-5*pi/18+0.576); revised: Tp(1,4) = -b(2)*(dtinc(1,3)-dtinc(1,5)) - b(4)*f(tinc(1,3)-tinc(1,5))*(dtinc(1,3)-dtinc(1,5)) - k(1)*f(tinc(1,3)-tinc(1,5)) - b(3)*f(tinc(1,5)-tinc(1,3)-5*pi/6)*(dtinc(1,3)-dtinc(1,5)) + k(1)*f(tinc(1,5)-tinc(1,3)-5*pi/6); Tp(1,7) = b(2)*(dtinc(1,6)-dtinc(1,8)) + b(3)*f(tinc(1,6)-tinc(1,8)-0.576-2*pi/9)*(dtinc(1,6)-dtinc(1,8)) + k(1)*f(tinc(1,6)-tinc(1,8)-0.576-2*pi/9) + b(3)*f(tinc(1,8)-tinc(1,6)-5*pi/18+0.576)*(dtinc(1,6)-dtinc(1,8)) - k(1)*f(tinc(1,8)-tinc(1,6)-5*pi/18+0.576); NOTES: 1) either set of equations works for 60-second simulations %%%%%%%%%%%%%%%%% % Fig. 8A and B % %%%%%%%%%%%%%%%%% 1) the magnitude of du is much larger for my model. %%%%%%%%%%%% % Fig. 12C % %%%%%%%%%%%% 1) Tmi16 for my model is missing a sharp peak near the beginning of the cycle. %%%%%%%%%%%% % Fig. 14B % %%%%%%%%%%%% 1) Fg (anterior-posterior) is missing a sharp peak near the beginning of the cycle. %} %%%%%%%%%%%%% % notations % %%%%%%%%%%%%% %{ global coordinate system: x - positive points to direction of locomotion y - positive points against direction of gravity horizontal and vertical constraint forces: Fx1 and Fy1 - HAT Fx2 and Fy2 - right hip Fx3 and Fy3 - left hip Fx4 and Fy4 - right knee Fx5 and Fy5 - left knee Fx6 and Fy6 - right ankle Fx7 and Fy7 - left ankle ground reaction forces: Fgx1 and Fgy1 - right heel Fgx2 and Fgy2 - left heel Fgx3 and Fgy3 - right toe Fgx4 and Fgy4 - left toe active and passive torques: Ta1 and Tp1 - HAT Ta2 and Tp2 - right hip Ta3 and Tp3 - left hip Ta4 and Tp4 - right knee Ta5 and Tp5 - left knee Ta6 and Tp6 - right ankle Ta7 and Tp7 - left ankle directionality of joint torques: +ve Ta1 & Tp1 = posterior bending of HAT +ve Ta2 & Tp2 = extension of right hip +ve Ta3 & Tp3 = extension of left hip +ve Ta4 & Tp4 = extension of right knee +ve Ta5 & Tp5 = extension of left knee +ve Ta6 & Tp6 = plantarflexion of right foot +ve Ta7 & Tp7 = plantarflexion of left foot segment angles: t1 - HAT t2 - pelvis t3 - right thigh t4 - left thigh t5 - right shank t6 - left shank t7 - right ankle t8 - left ankle skeletal muscles: Muscle 1 causes anterior bending of HAT. Muscle 1 may be represented by the rectus abdominis muscle. Muscle 2 causes posterior bending of HAT. Muscle 2 may be represented by components of the erector spinae that originate from the sacrum and contribute to posterior bending of the upper body. Muscle 3 (right) and Muscle 5 cause hip flexion. Muscles 3 and 5 may be represented by the iliacus muscle. Muscle 4 (right) and Muscle 6 cause hip extension. Muscles 4 and 6 may be represented by the gluteus maximus. Muscle 7 (right) and Muscle 9 contribute to hip flexion and knee extension. Muscles 7 and 9 may be represented by the rectus femoris. Muscle 8 (right) and Muscle 10 contribute to hip extension and knee flexion. Muscles 8 and 10 may be represented by the long head of the biceps femoris, semitendinosus, and semimembranosus. Muscle 11 (right) and Muscle 13 cause knee flexion. Muscles 11 and 13 may be represented by the short head of the biceps femoris. Muscle 12 (right) and Muscle 14 cause knee extension. Muscles 12 and 14 may be represented by the vastus lateralis, vastus medialis, and vastus intermedius. Muscle 15 (right) and Muscle 17 cause dorsiflexion. Muscles 15 and 17 may be represented by the tibialis anterior. Muscle 16 (right) and Muscle 18 cause plantarflexion. Muscles 16 and 18 may be represented by the soleus. Muscle 19 (right) and Muscle 20 contribute to knee flexion and plantarflexion. Muscles 19 and 20 may be represented by the gastrocnemius. neural oscillators: u1 (anterior bending) and u2 (posterior bending) represent trunk neural oscillators. u3 (flexion) and u4 (extension) represent right hip neural oscillators. u5 (flexion) and u6 (extension) represent left hip neural oscillators. u7 (flexion) and u8 (extension) represent right knee neural oscillators. u9 (flnexio) and u10 (extension) represent left knee neural oscillators. u11 (dorsiflexion) and u12 (plantarflexion) represent right ankle neural oscillators. u13 (dorsiflexion) and u14 (plantarflexion) represent left ankle neural oscillators. global states: Sg1 Sg2 Sg3 Sg4 Sg5 Sg6 right (R) |---------------stance--------------|------swing------| | | | | | | | left (L) |-stance-|------swing------|----------stance----------| | | | | | | | R heel L heel L heel R heel on on on on %} %%%%%%%%%%%%%%%%%%%%% % outline of method % %%%%%%%%%%%%%%%%%%%%% %{ Numerically integrate Equations (8) and (18). Both equations use a system of ODEs with 1 indepent variable and N dependent variables. For Equation (18), double integration is required to obtain displacements. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equation 18 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%% % formulation % %%%%%%%%%%%%%%% ddq = ds = fp(xi,dxi,yi,dyi,ti,dti,uj,vj); dq = s; % where p=1:24, i=1:8, and j=1:14 %%%%%%%%%%%%%% % velocities % %%%%%%%%%%%%%% dq_p_n+1_old = dq_p_n + (stp/6)*[k1p+(2-sqrt(2))*k2p+(2+sqrt(2))*k3p+k4p]; % where n and n+1 indicate time steps dq_p_n+1_new = dq_p_n+1_old - OMG*inv(PHI*OMG)*PHI*dq_p_n+1_old; % velocity stabilization through projection f1 = ddx1; f2 = ddy1; f3 = ddt1; f4 = ddx2; f5 = ddy2; f6 = ddt2; f7 = ddx3; f8 = ddy3; f9 = ddt3; f10 = ddx4; f11 = ddy4; f12 = ddt4; f13 = ddx5; f14 = ddy5; f15 = ddt5; f16 = ddx6; f17 = ddy6; f18 = ddt6; f19 = ddx7; f20 = ddy7; f21 = ddt7; f22 = ddx8; f23 = ddy8; f24 = ddt8; k1p = fp(xi,dxi,yi,dyi,ti,dti,uj,vj); k2p = fp(xi+0.5*h1c,dxi+0.5*k1p,... yi+0.5*h1c,dyi+0.5*k1p,... ti+0.5*h1c,dti+0.5*k1p,... uj+0.5*h1c,vj+0.5*h1c); k3p = fp(xi+0.5*h1c*(sqrt(2)-1)+h2c*(1-sqrt(2)/2),... dxi+0.5*k1p*(sqrt(2)-1)+k2p*(1-sqrt(2)/2),... yi+0.5*h1c*(sqrt(2)-1)+h2c*(1-sqrt(2)/2),... dyi+0.5*k1p*(sqrt(2)-1)+k2p*(1-sqrt(2)/2),... ti+0.5*h1c*(sqrt(2)-1)+h2c*(1-sqrt(2)/2),... dti+0.5*k1p*(sqrt(2)-1)+k2p*(1-sqrt(2)/2),... uj+0.5*h1c*(sqrt(2)-1)+h2c*(1-sqrt(2)/2),... vj+0.5*h1c*(sqrt(2)-1)+h2c*(1-sqrt(2)/2)); k4p = fp(xi-h2c*(sqrt(2)/2)+h3c*(1+sqrt(2)/2),... dxi-k2p*(sqrt(2)/2)+k3p*(1+sqrt(2)/2),... yi-h2c*(sqrt(2)/2)+h3c*(1+sqrt(2)/2),... dyi-k2p*(sqrt(2)/2)+k3p*(1+sqrt(2)/2),... ti-h2c*(sqrt(2)/2)+h3c*(1+sqrt(2)/2),... dti-k2p*(sqrt(2)/2)+k3p*(1+sqrt(2)/2),... uj-h2c*(sqrt(2)/2)+h3c*(1+sqrt(2)/2),... vj-h2c*(sqrt(2)/2)+h3c*(1+sqrt(2)/2)); %%%%%%%%%%%%%%%%% % displacements % %%%%%%%%%%%%%%%%% q_c_n+1 = q_c_n + (stp/6)*[h1c+(2-sqrt(2))*h2c+(2+sqrt(2))*h3c+h4c]; % where c=1:52 h1c = dq_c_n; h2c = dq_c_n + 0.5*k1p; h3c = dq_c_n + 0.5*k1p*(sqrt(2)-1) + k2p*(1-sqrt(2)/2) h4c = dq_c_n - k2p*(sqrt(2)/2) + k3p*(1+sqrt(2)/2) dq1 = dx1; dq2 = dy1; dq3 = dt1; dq4 = dx2; dq5 = dy2; dq6 = dt2; dq7 = dx3; dq8 = dy3; dq9 = dt3; dq10 = dx4; dq11 = dy4; dq12 = dt4; dq13 = dx5; dq14 = dy5; dq15 = dt5; dq16 = dx6; dq17 = dy6; dq18 = dt6; dq19 = dx7; dq20 = dy7; dq21 = dt7; dq22 = dx8; dq23 = dy8; dq24 = dt8; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equation 8 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%% % formulation % %%%%%%%%%%%%%%% q_c_n+1 = q_c_n + (stp/6)*[h1c+(2-sqrt(2))*h2c+(2+sqrt(2))*h3c+h4c]; % where c=1:52 h1c = gi(xn,yn) h2c = gi(xn+0.5*stp,yn+0.5*h1c) h3c = gi[xn+0.5*stp,yn+0.5*h1c*(sqrt(2)-1)+h2c*(1-sqrt(2)/2)] h4c = gi[xn+stp,yn-h2c*(sqrt(2)/2)+h3c*(1+sqrt(2)/2)] % where i=1:28 g1 = dq25 = du1; g2 = dq_26 = dv1; g3 = dq27 = du2; g4 = dq28 = dv2; g5 = dq29 = du3; g6 = dq30 = dv3; g7 = dq31 = du4; g8 = dq32 = dv4; g9 = dq33 = du5; g10 = dq34 = dv5; g11 = gdq35 = du6; g12 = dq36 = dv6; g13 = dq37 = du7; g14 = dq38 = dv7; g15 = dq39 = du8; g16 = dq40 = dv8; g17 = dq41 = du9; g18 = dq42 = dv9; g19 = dq43 = du10; g20 = dq44 = dv10; g21 = dq45 = du11; g22 = dq46 = dv11; g23 = dq47 = du12; g24 = dq48 = dv12; g25 = dq49 = du13; g26 = dq50 = dv13; g27 = dq51 = du14; g28 = dq52 = dv14; %} %%%%%%%%%%%%%%%%% % sub-functions % %%%%%%%%%%%%%%%%% %{ NOTE: this script uses the following sub-functions: f, h, yg %} %%%%%%%%%%%%%%%%% % user settings % %%%%%%%%%%%%%%%%% %%% simulation %%% %%%%%%%%%%%%%%%%%%%% User Change %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% dur = 35; % duration (s) of stimulation 34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% stpsz = 0.25e-3; % time step size (s) for numerical integration time = 0:stpsz:dur; num_step = length(time); %number of time steps for the specified duration zzzzzz = 4000*dur; SI = 0.95; % constant that modified the self-inhibition of CPG neurons %%%%%%%%%%%%%%%%%%%%%%% % constant parameters % %%%%%%%%%%%%%%%%%%%%%%% %%% subfunctions %%% % threshold for subfunction h.m a = 0.01; %%% equations of motion %%% % gravitational acceleration (m/s^2) g = 9.8; %%% passive torques %%% % coefficients for joint stiffness b(1,1) = 10; b(1,2) = 1; b(1,3) = 100; b(1,4) = 1000; % coefficients for range of motion k(1,1) = 1000; k(1,2) = 500; %%% ipsilateral inhibition %%% % strength of connection from one neuronal pool to another w(1,1) = 0.1; w(1,2) = 0.2; %%% sensory signals %%% % strength of sensory inputs (a?) q(1,1) = 6; q(1,2) = 0.9; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% q(1,3) = 1.5; q(1,4) = 1.5; q(1,5) = 3; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% q(1,6) = 3; q(1,7) = 0.1; q(1,8) = 0.2; %%% muscle torques (rhythm generator) %%% % constants that determine strength of torques %%% For loop for torque parameters iii = 0; %counter for trial number si = 1; %counter for summary tc = 1; %%%%%%%% fc = 1; zcountfalltime = 1; zcountgaitparameters = 1; zcountfgyfgx = 1; zcountmoi = 1; zcountafgyfgx = 1; aafallingbefore = 0; aafalling = 0; aanofalling = 0; fall = 0; %%%%%%%% % Non-faller OA model r1=1; p(1,1) = 5*r1; p(1,2) = 10*r1; p(1,3) = 4*r1; p(1,4) = 2*r1; p(1,5) = 15*r1; p(1,6) = 4*r1; p(1,7) = 3*r1; p(1,8) = 2*r1; p(1,9) = 15*r1; p(1,10) = 8*r1; p(1,11) = 2*r1; p(1,12) = 3*r1; p(1,13) = 2*r1; p(1,14) = 8*r1; p(1,15) = 1.5*r1; p(1,16) = 12*r1; p(1,17) = 1*r1; p(1,18) = 7*r1; %%%%%%%%%%%%%%%%%%%% User Change %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% groundchangex = 15; % Where to change the ground condition, m endofsimx = 25; % Where to end the simulation, m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% muscle torques (impedance controller) %%% % constants that determine strength of torques pim(1,1) = 500; pim(1,2) = 10; pim(1,3) = 800; pim(1,4) = 20; pim(1,5) = 150; pim(1,6) = 10; pim(1,7) = 10; %%% active torques %%% % proportions of torques from two-joint muscles eps(1,1) = 1; eps(1,2) = 0.5; eps(1,3) = 1; %%% musculoskeletal system %%% % mass (kg) of HAT, pelvis, thigh, shank, and foot mH = 38; mp = 10; mt = 7; ms = 3; mf = 1; m = [mH mp mt mt ms ms mf mf]; % moment of inertia (kg*m^2) of HAT, pelvis, thigh, shank, and foot IH = 1.1399; Ip = 0.05; It = 0.0933; Is = 0.0399; If = 0.0032; % LH1 = length (m) between (x1,y1) and superior end of HAT % LH2 = length (m) between (x1,y1) and posterior end of HAT % Lp = length (m) between (x2,y2) and either end of pelvis % Lt = length (m) between (x3,y3) or (x4,y4) and either end of thigh % Ls = length (m) between (x5,y5) or (x6,y6) and either end of shank % Lf1 = length (m) between (x7,y7) or (x8,y8) and ankle joint % Lf2 = length (m) between (x7,y7) or (x8,y8) and heel % Lf3 = length (m) between (x7,y7) or (x8,y8) and toe % alp1 = angle (rad) between lf1 and lf2 % alp2 = angle (rad) between lf1 and lf3 LH1 = 0.4; LH2 = 0.3; Lp = 0.1; Lt = 0.2; Ls = 0.2; Lf1 = 0.08; Lf2 = 0.12; Lf3 = 0.10; alp1 = 1.22; alp2 = 2.44; %%% neural rhythm generator %%% %{ NOTE: connections are assumed to exist 1) within each oscillator (i.e., at each joint), 2) bilaterally between agonist neurons at equivalent joints (e.g., left and right hips joints) or between antagonist neurons at equivalent joints, and 3) from neurons for hip extension to neuron for posterior bending of trunk. %} % strengt of neural connections from jth neuron to ith neuron % j = 1 2 3 4 5 6 7 8 9 10 11 12 13 14 wo = [ 0 -2 0 0 0 0 0 0 0 0 0 0 0 0; %1 = i -2 0 0 1 0 1 0 0 0 0 0 0 0 0; %2 0 0 0 -2 -1 0 0 0 0 0 0 0 0 0; %3 0 0 -2 0 0 -1 0 0 0 0 0 0 0 0; %4 0 0 -1 0 0 -2 0 0 0 0 0 0 0 0; %5 0 0 0 -1 -2 0 0 0 0 0 0 0 0 0; %6 0 0 0 0 0 0 0 -2 -1 0 0 0 0 0; %7 0 0 0 0 0 0 -2 0 0 -1 0 0 0 0; %8 0 0 0 0 0 0 -1 0 0 -2 0 0 0 0; %9 0 0 0 0 0 0 0 -1 -2 0 0 0 0 0; %10 0 0 0 0 0 0 0 0 0 0 0 -2.0 -0.2 0; %11 0 0 0 0 0 0 0 0 0 0 -2.0 0 0 -0.2; %12 0 0 0 0 0 0 0 0 0 0 -0.2 0 0 -2.0; %13 0 0 0 0 0 0 0 0 0 0 0 -0.2 -2.0 0];%14 % time constant of inner state of ith neuron %normal=0.67, long=1.2 r6 = 1.2; tau = r6*[1/32 1/32 1/18*ones(1,12)]; % time constant of adaptation effect of ith neuron %normal=1.56, long=1.0 r7 = 1.0; dtau = r7*[1/2.656 1/2.656 1/1.494*ones(1,12)]; % coefficient of adaptation beta = 2.5; % spring coefficient (N/m) kgx1 = 24000; %%%%%%%%%%%%%%%%%%%% User Change %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for sf = 1.5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % spring coefficient (N/m) kgx2 = 18000*sf; kgy1 = 18000; kgy2 = 18000; % damping coefficient (N*s/m) bgx1 = 2000; bgx2 = kgx2/12; bgy1 = 1000; bgy2 = 1000; %%%%%%%%%%%%%%%%%%%% User Change %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for muS = 0.60:0.1:2.0 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% User Change %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% TrialNumber = 5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% counter = TrialNumber;% counter for counting while loop while counter>0 counter = counter-1; dt = datetime('now'); disp(dt) counterr = TrialNumber-counter; fprintf('Running on SF...') disp(sf) fprintf('Running on muS...') disp(muS) fprintf('Running on trial...') disp(counterr) % external input r8 = 0.86; uo = 6.0*r8; % Noise into uo %%%%%%%%%%%%%%%%%%%% User Change %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% noisepercentage = 0.02; % *100[%] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% uom = ones(1,num_step).*uo; noise = (2*uo*noisepercentage).*rand(1,num_step)-uo*noisepercentage; %Making N, range(a,b),random varieties goes with r = a + (b-a).*rand(N,1) uomm = uom+noise; %%%%%%%%%%%%%%%%%%%%%% % initial conditions % %%%%%%%%%%%%%%%%%%%%%% % musculoskeletal system t = zeros(num_step,8); t(1,1) = 1.714; t(1,2) = 1.588; t(1,3) = 0.653; t(1,4) = 1.618; t(1,5) = 1.418; t(1,6) = 1.623; t(1,7) = 0.543; t(1,8) = 0.984; dt = zeros(num_step,8); dt(1,1) = 0; dt(1,2) = 0; dt(1,3) = -1; dt(1,4) = 1; dt(1,5) = -5; dt(1,6) = 2; dt(1,7) = -8; dt(1,8) = 0; x = zeros(num_step,8); y = zeros(num_step,8); x(1,2) = 1; y(1,2) = 0.984; x(1,1) = x(1,2) - Lp*cos(t(1,2)) - LH2*cos(t(1,1)); y(1,1) = y(1,2) + Lp*sin(t(1,2)) + LH2*sin(t(1,1)); x(1,3) = x(1,2) + Lp*cos(t(1,2)) + Lt*cos(t(1,3)); y(1,3) = y(1,2) - Lp*sin(t(1,2)) - Lt*sin(t(1,3)); x(1,4) = x(1,2) + Lp*cos(t(1,2)) + Lt*cos(t(1,4)); y(1,4) = y(1,2) - Lp*sin(t(1,2)) - Lt*sin(t(1,4)); x(1,5) = x(1,3) + Lt*cos(t(1,3)) + Ls*cos(t(1,5)); y(1,5) = y(1,3) - Lt*sin(t(1,3)) - Ls*sin(t(1,5)); x(1,6) = x(1,4) + Lt*cos(t(1,4)) + Ls*cos(t(1,6)); y(1,6) = y(1,4) - Lt*sin(t(1,4)) - Ls*sin(t(1,6)); x(1,7) = x(1,5) + Ls*cos(t(1,5)) + Lf1*cos(t(1,7)); y(1,7) = y(1,5) - Ls*sin(t(1,5)) - Lf1*sin(t(1,7)); x(1,8) = x(1,6) + Ls*cos(t(1,6)) + Lf1*cos(t(1,8)); y(1,8) = y(1,6) - Ls*sin(t(1,6)) - Lf1*sin(t(1,8)); dx = zeros(num_step,8); dy = zeros(num_step,8); dx(1,2) = 0.7; dy(1,2) = 0; dx(1,1) = dx(1,2) + Lp*sin(t(1,2))*(dt(1,2)) + LH2*sin(t(1,1))*(dt(1,1)); dy(1,1) = dy(1,2) + Lp*cos(t(1,2))*(dt(1,2)) + LH2*cos(t(1,1))*(dt(1,1)); dx(1,3) = dx(1,2) - Lp*sin(t(1,2))*(dt(1,2)) - Lt*sin(t(1,3))*(dt(1,3)); dy(1,3) = dy(1,2) - Lp*cos(t(1,2))*(dt(1,2)) - Lt*cos(t(1,3))*(dt(1,3)); dx(1,4) = dx(1,2) - Lp*sin(t(1,2))*(dt(1,2)) - Lt*sin(t(1,4))*(dt(1,4)); dy(1,4) = dy(1,2) - Lp*cos(t(1,2))*(dt(1,2)) - Lt*cos(t(1,4))*(dt(1,4)); dx(1,5) = dx(1,3) - Lt*sin(t(1,3))*(dt(1,3)) - Ls*sin(t(1,5))*(dt(1,5)); dy(1,5) = dy(1,3) - Lt*cos(t(1,3))*(dt(1,3)) - Ls*cos(t(1,5))*(dt(1,5)); dx(1,6) = dx(1,4) - Lt*sin(t(1,4))*(dt(1,4)) - Ls*sin(t(1,6))*(dt(1,6)); dy(1,6) = dy(1,4) - Lt*cos(t(1,4))*(dt(1,4)) - Ls*cos(t(1,6))*(dt(1,6)); dx(1,7) = dx(1,5) - Ls*sin(t(1,5))*(dt(1,5)) - Lf1*sin(t(1,7))*(dt(1,7)); dy(1,7) = dy(1,5) - Ls*cos(t(1,5))*(dt(1,5)) - Lf1*cos(t(1,7))*(dt(1,7)); dx(1,8) = dx(1,6) - Ls*sin(t(1,6))*(dt(1,6)) - Lf1*sin(t(1,8))*(dt(1,8)); dy(1,8) = dy(1,6) - Ls*cos(t(1,6))*(dt(1,6)) - Lf1*cos(t(1,8))*(dt(1,8)); % neural rhythm generator u = zeros(num_step,14); u(1,1) = 1; u(1,2) = -1; u(1,3) = -1; u(1,4) = 1; u(1,5) = 1; u(1,6) = 1; u(1,7) = 1; u(1,8) = -1; u(1,9) = -1; u(1,10) = 1; u(1,11) = 1; u(1,12) = -1; u(1,13) = 1; u(1,14) = -1; v = zeros(num_step,14); v(1,:) = ones(1,14); %%%%%%%%%%%%%%%%%%% % allocate memory % %%%%%%%%%%%%%%%%%%% % coordinates and velocities of toes and heels xf = zeros(1,4); yf = zeros(1,4); dxf = zeros(1,4); dyf = zeros(1,4); % horizontal coordinate of toe or heel contact xft = -1*ones(1,4); % xft(1,right/left toe/heel) % vertical and horizontal components of reaction force Fgy = zeros(1,4); % Fgy(1,right/left toe/heel) Fgx = zeros(1,4); % Fgx(1,right/left toe/heel) COF = zeros(1,4); % first time derivatives of u and v du = zeros(1,14); dv = zeros(1,14); % increment for numerical integration deldq = zeros(num_step,3*size(x,2)); delq = zeros(num_step,3*size(x,2)+2*size(u,2)); %%% storage for checking output post-simulation %%% GA_store = zeros(num_step,4); dGA_store = zeros(num_step,4); sg1_store = zeros(num_step,4); sg2_store = zeros(num_step,4); sg3_store = zeros(num_step,4); sg4_store = zeros(num_step,4); sg5_store = zeros(num_step,4); sg6_store = zeros(num_step,4); Tmr1 = zeros(num_step,20); Tmi1 = zeros(num_step,20); Ta1 = zeros(num_step,7); Tp1 = zeros(num_step,7); Fgx1 = zeros(num_step,4); Fgy1 = zeros(num_step,4); COF1 = zeros(num_step,4); Fgxsum = zeros(num_step,2); Fgysum = zeros(num_step,2); COFsum = zeros(num_step,2); xf1 = zeros(num_step,4); yf1 = zeros(num_step,4); Q_store = zeros(num_step,14); S_store = zeros(num_step,14); du_store = zeros(num_step,14); dv_store = zeros(num_step,14); %%%%%%%%%%%%%%%%%%%%%%%%% % numerical integration % %%%%%%%%%%%%%%%%%%%%%%%%% for idxts=1:num_step-1 % time steps K = zeros(3*size(x,2),4); H = zeros(3*size(x,2)+2*size(u,2),4); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % increment dphi, phi, u, and v % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if idxts>1 % musculoskeletal system for idxseg=1:size(t,2) x(idxts,idxseg) = x(idxts-1,idxseg) + delq(idxts-1,3*idxseg-2); y(idxts,idxseg) = y(idxts-1,idxseg) + delq(idxts-1,3*idxseg-1); t(idxts,idxseg) = t(idxts-1,idxseg) + delq(idxts-1,3*idxseg); dx(idxts,idxseg) = dx(idxts-1,idxseg) + deldq(idxts-1,3*idxseg-2); dy(idxts,idxseg) = dy(idxts-1,idxseg) + deldq(idxts-1,3*idxseg-1); dt(idxts,idxseg) = dt(idxts-1,idxseg) + deldq(idxts-1,3*idxseg); dphi(idxseg*3-2:idxseg*3,1) = [dx(idxts,idxseg);dy(idxts,idxseg);dt(idxts,idxseg)]; end yy(idxts+1,1) = y(idxts,2); % neural rhythm generator for idxnrn=1:size(u,2) u(idxts,idxnrn) = u(idxts-1,idxnrn) + delq(idxts-1,3*size(x,2)+2*idxnrn-1); v(idxts,idxnrn) = v(idxts-1,idxnrn) + delq(idxts-1,3*size(x,2)+2*idxnrn); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % velocity stabilization through projection % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% OMG = [1/mH zeros(1,13); zeros(1,7) 1/mH zeros(1,6); -LH2*sin(t(idxts,1))/IH zeros(1,6) -LH2*cos(t(idxts,1))/IH zeros(1,6); (1/mp)*[-1 1 1] zeros(1,11); zeros(1,7) (1/mp)*[-1 1 1] 0 0 0 0; (-Lp*sin(t(idxts,2))/Ip)*ones(1,3) 0 0 0 0 (-Lp*cos(t(idxts,2))/Ip)*ones(1,3) 0 0 0 0; 0 -1/mt 0 1/mt zeros(1,10); zeros(1,8) -1/mt 0 1/mt 0 0 0; 0 -Lt*sin(t(idxts,3))/It 0 -Lt*sin(t(idxts,3))/It 0 0 0 0 -Lt*cos(t(idxts,3))/It 0 -Lt*cos(t(idxts,3))/It 0 0 0; 0 0 -1/mt 0 1/mt zeros(1,9); zeros(1,9) -1/mt 0 1/mt 0 0; 0 0 -Lt*sin(t(idxts,4))/It 0 -Lt*sin(t(idxts,4))/It 0 0 0 0 -Lt*cos(t(idxts,4))/It 0 -Lt*cos(t(idxts,4))/It 0 0; 0 0 0 -1/ms 0 1/ms zeros(1,8); zeros(1,10) -1/ms 0 1/ms 0; 0 0 0 -Ls*sin(t(idxts,5))/Is 0 -Ls*sin(t(idxts,5))/Is 0 0 0 0 -Ls*cos(t(idxts,5))/Is 0 -Ls*cos(t(idxts,5))/Is 0; 0 0 0 0 -1/ms 0 1/ms zeros(1,7); zeros(1,11) -1/ms 0 1/ms; 0 0 0 0 -Ls*sin(t(idxts,6))/Is 0 -Ls*sin(t(idxts,6))/Is 0 0 0 0 -Ls*cos(t(idxts,6))/Is 0 -Ls*cos(t(idxts,6))/Is; 0 0 0 0 0 -1/mf zeros(1,8); zeros(1,12) -1/mf 0; 0 0 0 0 0 -Lf1*sin(t(idxts,7))/If zeros(1,6) -Lf1*cos(t(idxts,7))/If 0; zeros(1,6) -1/mf zeros(1,7); zeros(1,13) -1/mf; zeros(1,6) -Lf1*sin(t(idxts,8))/If zeros(1,6) -Lf1*cos(t(idxts,8))/If]; PHI = [1 0 -LH2*sin(t(idxts,1)) -1 0 -Lp*sin(t(idxts,2)) zeros(1,18); 0 1 -LH2*cos(t(idxts,1)) 0 -1 -Lp*cos(t(idxts,2)) zeros(1,18); 0 0 0 1 0 -Lp*sin(t(idxts,2)) -1 0 -Lt*sin(t(idxts,3)) zeros(1,15); 0 0 0 0 1 -Lp*cos(t(idxts,2)) 0 -1 -Lt*cos(t(idxts,3)) zeros(1,15); 0 0 0 1 0 -Lp*sin(t(idxts,2)) 0 0 0 -1 0 -Lt*sin(t(idxts,4)) zeros(1,12); 0 0 0 0 1 -Lp*cos(t(idxts,2)) 0 0 0 0 -1 -Lt*cos(t(idxts,4)) zeros(1,12); zeros(1,6) 1 0 -Lt*sin(t(idxts,3)) 0 0 0 -1 0 -Ls*sin(t(idxts,5)) zeros(1,9); zeros(1,6) 0 1 -Lt*cos(t(idxts,3)) 0 0 0 0 -1 -Ls*cos(t(idxts,5)) zeros(1,9); zeros(1,9) 1 0 -Lt*sin(t(idxts,4)) 0 0 0 -1 0 -Ls*sin(t(idxts,6)) zeros(1,6); zeros(1,9) 0 1 -Lt*cos(t(idxts,4)) 0 0 0 0 -1 -Ls*cos(t(idxts,6)) zeros(1,6); zeros(1,12) 1 0 -Ls*sin(t(idxts,5)) 0 0 0 -1 0 -Lf1*sin(t(idxts,7)) 0 0 0; zeros(1,12) 0 1 -Ls*cos(t(idxts,5)) 0 0 0 0 -1 -Lf1*cos(t(idxts,7)) 0 0 0; zeros(1,15) 1 0 -Ls*sin(t(idxts,6)) 0 0 0 -1 0 -Lf1*sin(t(idxts,8)); zeros(1,15) 0 1 -Ls*cos(t(idxts,6)) 0 0 0 0 -1 -Lf1*cos(t(idxts,8))]; dphivs = dphi - OMG*((PHI*OMG)\(PHI*dphi)); for idxseg=1:size(t,2) dx(idxts,idxseg) = dphivs(idxseg*3-2,1); dy(idxts,idxseg) = dphivs(idxseg*3-1,1); dt(idxts,idxseg) = dphivs(idxseg*3,1); end end for idxavg=1:4 % calculating averages for numerical integration if idxavg==1 inc = zeros(size(H,1),1); dinc = zeros(size(K,1),1); elseif idxavg==2 inc = 0.5*stpsz*H(:,1); dinc = 0.5*stpsz*K(:,1); elseif idxavg==3 inc = 0.5*stpsz*H(:,1)*(sqrt(2)-1)+(1-0.5*sqrt(2))*H(:,2)*stpsz; dinc = 0.5*stpsz*K(:,1)*(sqrt(2)-1)+(1-0.5*sqrt(2))*K(:,2)*stpsz; elseif idxavg==4 inc = -H(:,2)*stpsz*(0.5*sqrt(2))+H(:,3)*stpsz*(1+0.5*sqrt(2)); dinc = -K(:,2)*stpsz*(0.5*sqrt(2))+K(:,3)*stpsz*(1+0.5*sqrt(2)); end %%%%%%%%%%%%%%%%%%%%%%% % increment variables % %%%%%%%%%%%%%%%%%%%%%%% xinc = zeros(1,8); yinc = zeros(1,8); tinc = zeros(1,8); dxinc = zeros(1,8); dyinc = zeros(1,8); dtinc = zeros(1,8); for idxseg=1:size(t,2) % positions xinc(1,idxseg) = x(idxts,idxseg) + inc(idxseg*3-2); yinc(1,idxseg) = y(idxts,idxseg) + inc(idxseg*3-1); tinc(1,idxseg) = t(idxts,idxseg) + inc(idxseg*3); % velocities dxinc(1,idxseg) = dx(idxts,idxseg) + dinc(idxseg*3-2); dyinc(1,idxseg) = dy(idxts,idxseg) + dinc(idxseg*3-1); dtinc(1,idxseg) = dt(idxts,idxseg) + dinc(idxseg*3); end for idxseg=1:size(t,2) dphiinc(idxseg*3-2:idxseg*3,1) = [dxinc(1,idxseg);dyinc(1,idxseg);dtinc(1,idxseg)]; end % neural activities uinc = zeros(1,14); vinc = zeros(1,14); for idxuv=1:size(u,2) uinc(1,idxuv) = u(idxts,idxuv)+inc(size(K,1)+2*idxuv-1); vinc(1,idxuv) = v(idxts,idxuv)+inc(size(K,1)+2*idxuv); end %%%%%%%%%%%%%%%%%%%%%%% % equations of motion % %%%%%%%%%%%%%%%%%%%%%%% % NOTE: ddphi = OMG*inv(PHI*OMG)*[eta-PHI*PSI*] + PSI; %%% heel positions %%% xf(1,1) = xinc(1,7) - Lf2*cos(alp1-tinc(1,7)); yf(1,1) = yinc(1,7) - Lf2*sin(alp1-tinc(1,7)); xf(1,2) = xinc(1,8) - Lf2*cos(alp1-tinc(1,8)); yf(1,2) = yinc(1,8) - Lf2*sin(alp1-tinc(1,8)); %%% heel velocities %%% dxf(1,1) = dxinc(1,7) + Lf2*sin(alp1-tinc(1,7))*(-dtinc(1,7)); dyf(1,1) = dyinc(1,7) - Lf2*cos(alp1-tinc(1,7))*(-dtinc(1,7)); dxf(1,2) = dxinc(1,8) + Lf2*sin(alp1-tinc(1,8))*(-dtinc(1,8)); dyf(1,2) = dyinc(1,8) - Lf2*cos(alp1-tinc(1,8))*(-dtinc(1,8)); %%% toe positions %%% xf(1,3) = xinc(1,7) - Lf3*cos(alp2+tinc(1,7)); yf(1,3) = yinc(1,7) + Lf3*sin(alp2+tinc(1,7)); xf(1,4) = xinc(1,8) - Lf3*cos(alp2+tinc(1,8)); yf(1,4) = yinc(1,8) + Lf3*sin(alp2+tinc(1,8)); %%% toe velocities %%% dxf(1,3) = dxinc(1,7) + Lf3*sin(alp2+tinc(1,7))*dtinc(1,7); dyf(1,3) = dyinc(1,7) + Lf3*cos(alp2+tinc(1,7))*dtinc(1,7); dxf(1,4) = dxinc(1,8) + Lf3*sin(alp2+tinc(1,8))*dtinc(1,8); dyf(1,4) = dyinc(1,8) + Lf3*cos(alp2+tinc(1,8))*dtinc(1,8); maxtoe(idxts,1) = idxts; maxtoe(idxts,2) = max(xf(1,3),xf(1,4)); maxtoe(idxts+1,3) = maxtoe(idxts,2); %%% ground reaction forces %%% % NOTE: ground reactions forces are calculated in the following % order: right heel, left heel, right toe, and left toe. for idxft=1:size(xft,2) if yf(1,idxft)= groundchangex % right foot if (Fgy(1,1)+Fgy(1,3)) == 0 functionjudge(1,1) = -1; Fgx(1,1) = 0; Fgx(1,3) = 0; else cfcr = (Fgx(1,1)+Fgx(1,3))/(Fgy(1,1)+Fgy(1,3)); %Fgy1(idxts-20,1) == 0 && Fgy(1,3) == 0 || ... if Fgy1(idxts-120,1) == 0 && Fgy1(idxts-50,3) == 0 || ... Fgy1(idxts-600,4) == 0 && Fgy1(idxts-100,4) > 0 && Fgy(1,1) == 0 functionjudge(1,1) = 2; Fgx(1,1) = Fgx(1,1); Fgx(1,3) = Fgx(1,3); else if abs(cfcr) <= muS functionjudge(1,1) = 0; Fgx(1,1) = Fgx(1,1); Fgx(1,3) = Fgx(1,3); else rFgxtotal = (Fgy(1,1)+Fgy(1,3)) * muS; fugou1 = sign(Fgx(1,1)); fugou3 = sign(Fgx(1,3)); gx1 = fugou1 * rFgxtotal * (abs(Fgx(1,1))/(abs(Fgx(1,1))+abs(Fgx(1,3)))); gx3 = fugou3 * rFgxtotal * (abs(Fgx(1,3))/(abs(Fgx(1,1))+abs(Fgx(1,3)))); functionjudge(1,1) = 1; Fgx(1,1) = gx1; Fgx(1,3) = gx3; end end COF(1,1) = Fgx(1,1)/Fgy(1,1); COF(1,3) = Fgx(1,3)/Fgy(1,3); end % left foot if (Fgy(1,2)+Fgy(1,4)) == 0 functionjudge(1,2) = -1; Fgx(1,2) = 0; Fgx(1,4) = 0; else cfcl = (Fgx(1,2)+Fgx(1,4))/(Fgy(1,2)+Fgy(1,4)); if Fgy1(idxts-120,2) == 0 && Fgy1(idxts-50,4) == 0 || ... Fgy1(idxts-600,3) == 0 && Fgy1(idxts-100,3) > 0 && Fgy(1,2) == 0 functionjudge(1,2) = 2; Fgx(1,2) = Fgx(1,2); Fgx(1,4) = Fgx(1,4); else if abs(cfcl) <= muS functionjudge(1,2) = 0; Fgx(1,2) = Fgx(1,2); Fgx(1,4) = Fgx(1,4); else lFgxtotal = (Fgy(1,2)+Fgy(1,4)) * muS; fugou2 = sign(Fgx(1,2)); fugou4 = sign(Fgx(1,4)); gx2 = fugou2 * lFgxtotal * (abs(Fgx(1,2))/(abs(Fgx(1,2))+abs(Fgx(1,4)))); gx4 = fugou4 * lFgxtotal * (abs(Fgx(1,4))/(abs(Fgx(1,2))+abs(Fgx(1,4)))); functionjudge(1,2) = 1; Fgx(1,2) = gx2; Fgx(1,4) = gx4; end end COF(1,2) = Fgx(1,2)/Fgy(1,2); COF(1,4) = Fgx(1,4)/Fgy(1,4); end else functionjudge(1,1) = 3; functionjudge(1,2) = 3; end if Fgy(1,1)+Fgy(1,3) == 0 zzFgyFgx(idxts,1) = 0; else zzFgyFgx(idxts,1) = (Fgx(1,1)+Fgx(1,3))/(Fgy(1,1)+Fgy(1,3)); end if Fgy(1,2)+Fgy(1,4) == 0 zzFgyFgx(idxts,2) = 0; else zzFgyFgx(idxts,2) = (Fgx(1,2)+Fgx(1,4))/(Fgy(1,2)+Fgy(1,4)); end zzFgyFgx(idxts,3) = functionjudge(1,1); zzFgyFgx(idxts,4) = functionjudge(1,2); zzFgyFgx(idxts,5) = Fgx(1,1)+Fgx(1,3); zzFgyFgx(idxts,6) = Fgy(1,1)+Fgy(1,3); zzFgyFgx(idxts,7) = Fgx(1,2)+Fgx(1,4); zzFgyFgx(idxts,8) = Fgy(1,2)+Fgy(1,4); %%% global states %%% % ON state for right foot sron = h(Fgy(1,1)+Fgy(1,3),a); % OFF state for right foot sroff = 1 - sron; % ON state for left foot slon = h(Fgy(1,2)+Fgy(1,4),a); % OFF state for left foot sloff = 1 - slon; % right foot leads sr = h(xf(1,1)-xf(1,2),a); % left foot leads sl = h(xf(1,2)-xf(1,1),a); %%% COG %%% nmtcgx = 0; nmtcgy = 0; nmtdcgx = 0; nmtdcgy = 0; for idxseg=1:8 nmtcgx = nmtcgx + m(idxseg)*xinc(1,idxseg); nmtcgy = nmtcgy + m(idxseg)*yinc(1,idxseg); nmtdcgx = nmtdcgx + m(idxseg)*dxinc(1,idxseg); nmtdcgy = nmtdcgy + m(idxseg)*dyinc(1,idxseg); end xcg = nmtcgx/sum(m); ycg = nmtcgy/sum(m); dxcg = nmtdcgx/sum(m); dycg = nmtdcgy/sum(m); %%% COP %%% nmrt = 0; for idxft=1:size(Fgy,2) nmrt = nmrt + Fgy(1,idxft)*xf(1,idxft); end xcp = nmrt/sum(Fgy(:)); ycp = yg(xcp); GA = acos((xcp-xcg)/((xcp-xcg)^2+(ycg-ycp)^2)^0.5); dGA = ((ycg-ycp)*dxcg+(xcp-xcg)*dycg)/((xcp-xcg)^2+(ycp-ycg)^2); sg(1) = sron*slon*sr; sg(2) = sron*sloff*h(pi/2-GA,a); sg(3) = sron*sloff*h(GA-pi/2,a); sg(4) = slon*sron*sl; sg(5) = slon*sroff*h(pi/2-GA,a); sg(6) = slon*sroff*h(GA-pi/2,a); % first half of gait cycle srst = sg(1) + sg(2) + sg(3); % second half of gait cycle slst = sg(4) + sg(5) + sg(6); %%% active torques %%% % NOTE: torques generated by muscles are due to 1) rhythm generator % and 2) impedance controller (i.e., Tm_i = Tmr_i + Tmi_i). % torques generated by rhythm generator Tmr(1,1) = p(1)*f(uinc(1,1)); Tmr(1,2) = p(2)*f(uinc(1,2)); Tmr(1,3) = (sron*p(3)+sroff*p(4))*f(uinc(1,3)); Tmr(1,4) = (sron*p(5)+sroff*p(6))*f(uinc(1,4)); Tmr(1,5) = (slon*p(3)+sloff*p(4))*f(uinc(1,5)); Tmr(1,6) = (slon*p(5)+sloff*p(6))*f(uinc(1,6)); Tmr(1,7) = (sron*p(7)+sroff*p(8))*f(uinc(1,3)); Tmr(1,8) = (sron*p(9)+sroff*p(10))*f(uinc(1,4)); Tmr(1,9) = (slon*p(7)+sloff*p(8))*f(uinc(1,5)); Tmr(1,10) = (slon*p(9)+sloff*p(10))*f(uinc(1,6)); Tmr(1,11) = slst*p(11)*f(uinc(1,7)); Tmr(1,12) = (sron*p(12)+sroff*p(13))*f(uinc(1,8)); Tmr(1,13) = srst*p(11)*f(uinc(1,9)); Tmr(1,14) = (slon*p(12)+sloff*p(13))*f(uinc(1,10)); Tmr(1,15) = (sron*p(14)+sroff*p(15))*f(uinc(1,11)); Tmr(1,16) = (sron*p(16)+sroff*p(17))*f(uinc(1,12)); Tmr(1,17) = (slon*p(14)+sloff*p(15))*f(uinc(1,13)); Tmr(1,18) = (slon*p(16)+sloff*p(17))*f(uinc(1,14)); Tmr(1,19) = sron*p(18)*f(uinc(1,12)); Tmr(1,20) = slon*p(18)*f(uinc(1,14)); % torques generated by impedance controller % NOTE: logic of impedance controller %{ 1) HAT and pelvis will maintain collinearity: - Tmi1 is active when anterior bending (or its velocity) of pelvis exceeds that of HAT. - Tmi2 is active when anterior bending (or its velocity) of HAT exceeds that of pelvis. 2) pelvis will (sort of) maintain 99-degree anterior bending (wrt the horizontal): - Tmi3 is active if anterior bending of pelvis is less than 99 degrees wrt the horizontal (or its velocity is negative) while right foot is on ground. - Tmi4 is active if anterior bending of pelvis is more than 99 degrees wrt the horizontal (or its velocity is positive) while right foot is on ground. - Tmi5 is active when anterior bending of pelvis is less than 99 degrees wrt the horizontal (or its velocity is negative) while left foot is on ground. - Tmi6 is active when anterior bending of pelvis is more than 99 degrees wrt the horizontal (or its velocity is positive) while left foot is on ground. 3) knee will not buckle during most of stance: - Tmi12 is active if knee is bent (or is about to bend) during most of stance of right leg (i.e., sg1, sg2, and sg3). - Tmi14 is active if knee is bent (or is about to bend) during most of stance of left leg (i.e., sg4, sg5, and sg6). 4) toe off occurs at the correct timing as leg leans forward toward the end of stance: - Tmi15 is active if plantarflexion is occurring faster than leg leaning forward during most of stance of right leg (i.e., sg1, sg2, and sg3). - Tmi16 is active if leg is leaning forward faster than the rate of plantarflexion during most of stance of right leg (i.e., sg1, sg2, and sg3). - Tmi17 is active if plantarflexion is occurring faster than leg leaning forward during most of stance of left leg (i.e., sg4, sg5, and sg6). - Tmi18 is active if leg is leaning forward faster than the rate of plantarflexion during most of stance of left leg (i.e., sg4, sg5, and sg6). %} Tmi(1,1) = pim(1)*f(tinc(1,2)-tinc(1,1)) + pim(2)*f(dtinc(1,2)-dtinc(1,1)); Tmi(1,2) = pim(1)*f(tinc(1,1)-tinc(1,2)) + pim(2)*f(dtinc(1,1)-dtinc(1,2)); Tmi(1,3) = sron*pim(3)*f(0.55*pi-tinc(1,2)) + sron*pim(4)*f(-dtinc(1,2)); Tmi(1,4) = sron*pim(3)*f(tinc(1,2)-0.55*pi) + sron*pim(4)*f(dtinc(1,2)); Tmi(1,5) = slon*pim(3)*f(0.55*pi-tinc(1,2)) + slon*pim(4)*f(-dtinc(1,2)); Tmi(1,6) = slon*pim(3)*f(tinc(1,2)-0.55*pi) + slon*pim(4)*f(dtinc(1,2)); Tmi(1,7:11) = 0; Tmi(1,12) = srst*pim(5)*f(tinc(1,5)-tinc(1,3)) + srst*pim(6)*f(dtinc(1,5)-dtinc(1,3)); Tmi(1,13) = 0; Tmi(1,14) = slst*pim(5)*f(tinc(1,6)-tinc(1,4)) + slst*pim(6)*f(dtinc(1,6)-dtinc(1,4)); Tmi(1,15) = srst*pim(7)*f(dtinc(1,7)-dtinc(1,5)); Tmi(1,16) = srst*pim(7)*f(dtinc(1,5)-dtinc(1,7)); Tmi(1,17) = slst*pim(7)*f(dtinc(1,8)-dtinc(1,6)); Tmi(1,18) = slst*pim(7)*f(dtinc(1,6)-dtinc(1,8)); Tmi(1,19:20) = 0; Tm = Tmr + Tmi; C = [-1 1 zeros(1,18); 0 0 -1 1 0 0 -1 1 zeros(1,12); 0 0 0 0 -1 1 0 0 -1 1 zeros(1,10); zeros(1,6) eps(1) -eps(2) 0 0 -1 1 zeros(1,6) -eps(3) 0; zeros(1,8) eps(1) -eps(2) 0 0 -1 1 0 0 0 0 0 -eps(3); zeros(1,14) -1 1 0 0 1 0; zeros(1,16) -1 1 0 1]; Ta = C*Tm.'; %%% passive torques %%% % NOTE: small passive torques represent joint stiffness and large % passive torques represent ranges of motion. Tp(1,1) = b(1)*(dtinc(1,1)-dtinc(1,2)) ... + b(3)*f(tinc(1,1)-tinc(1,2)-7*pi/18)*(dtinc(1,1)-dtinc(1,2)) + k(1)*f(tinc(1,1)-tinc(1,2)-7*pi/18) ... + b(3)*f(tinc(1,2)-tinc(1,1)-pi/9)*(dtinc(1,1)-dtinc(1,2)) - k(1)*f(tinc(1,2)-tinc(1,1)-pi/9); Tp(1,2) = b(2)*(dtinc(1,2)-dtinc(1,3)) ... + b(3)*f(tinc(1,2)-tinc(1,3)-pi/2)*(dtinc(1,2)-dtinc(1,3)) + k(2)*f(tinc(1,2)-tinc(1,3)-pi/2) ... + b(3)*f(tinc(1,3)-tinc(1,2)-pi/9)*(dtinc(1,2)-dtinc(1,3)) - k(2)*f(tinc(1,3)-tinc(1,2)-pi/9); Tp(1,3) = b(2)*(dtinc(1,2)-dtinc(1,4)) ... + b(3)*f(tinc(1,2)-tinc(1,4)-pi/2)*(dtinc(1,2)-dtinc(1,4)) + k(2)*f(tinc(1,2)-tinc(1,4)-pi/2) ... + b(3)*f(tinc(1,4)-tinc(1,2)-pi/9)*(dtinc(1,2)-dtinc(1,4)) - k(2)*f(tinc(1,4)-tinc(1,2)-pi/9); Tp(1,4) = -b(2)*(dtinc(1,3)-dtinc(1,5)) ... - b(4)*f(tinc(1,3)-tinc(1,5))*(dtinc(1,3)-dtinc(1,5)) - k(1)*f(tinc(1,3)-tinc(1,5)) ... - b(3)*f(tinc(1,5)-tinc(1,3)-5*pi/6)*(dtinc(1,3)-dtinc(1,5)) + k(1)*f(tinc(1,5)-tinc(1,3)-5*pi/6); Tp(1,5) = -b(2)*(dtinc(1,4)-dtinc(1,6)) ... - b(4)*f(tinc(1,4)-tinc(1,6))*(dtinc(1,4)-dtinc(1,6)) - k(1)*f(tinc(1,4)-tinc(1,6)) ... - b(3)*f(tinc(1,6)-tinc(1,4)-5*pi/6)*(dtinc(1,4)-dtinc(1,6)) + k(1)*f(tinc(1,6)-tinc(1,4)-5*pi/6); Tp(1,6) = b(2)*(dtinc(1,5)-dtinc(1,7)) ... + b(3)*f(tinc(1,5)-tinc(1,7)-0.576-2*pi/9)*(dtinc(1,5)-dtinc(1,7)) ... + k(1)*f(tinc(1,5)-tinc(1,7)-0.576-2*pi/9) ... + b(3)*f(tinc(1,7)-tinc(1,5)-5*pi/18+0.576)*(dtinc(1,5)-dtinc(1,7)) ... - k(1)*f(tinc(1,7)-tinc(1,5)-5*pi/18+0.576); Tp(1,7) = b(2)*(dtinc(1,6)-dtinc(1,8)) ... + b(3)*f(tinc(1,6)-tinc(1,8)-0.576-2*pi/9)*(dtinc(1,6)-dtinc(1,8)) ... + k(1)*f(tinc(1,6)-tinc(1,8)-0.576-2*pi/9) ... + b(3)*f(tinc(1,8)-tinc(1,6)-5*pi/18+0.576)*(dtinc(1,6)-dtinc(1,8)) ... - k(1)*f(tinc(1,8)-tinc(1,6)-5*pi/18+0.576); OMG = [1/mH zeros(1,13); zeros(1,7) 1/mH zeros(1,6); -LH2*sin(tinc(1,1))/IH zeros(1,6) -LH2*cos(tinc(1,1))/IH zeros(1,6); (1/mp)*[-1 1 1] zeros(1,11); zeros(1,7) (1/mp)*[-1 1 1] 0 0 0 0; (-Lp*sin(tinc(1,2))/Ip)*ones(1,3) 0 0 0 0 (-Lp*cos(tinc(1,2))/Ip)*ones(1,3) 0 0 0 0; 0 -1/mt 0 1/mt zeros(1,10); zeros(1,8) -1/mt 0 1/mt 0 0 0; 0 -Lt*sin(tinc(1,3))/It 0 -Lt*sin(tinc(1,3))/It 0 0 0 0 -Lt*cos(tinc(1,3))/It 0 -Lt*cos(tinc(1,3))/It 0 0 0; 0 0 -1/mt 0 1/mt zeros(1,9); zeros(1,9) -1/mt 0 1/mt 0 0; 0 0 -Lt*sin(tinc(1,4))/It 0 -Lt*sin(tinc(1,4))/It 0 0 0 0 -Lt*cos(tinc(1,4))/It 0 -Lt*cos(tinc(1,4))/It 0 0; 0 0 0 -1/ms 0 1/ms zeros(1,8); zeros(1,10) -1/ms 0 1/ms 0; 0 0 0 -Ls*sin(tinc(1,5))/Is 0 -Ls*sin(tinc(1,5))/Is 0 0 0 0 -Ls*cos(tinc(1,5))/Is 0 -Ls*cos(tinc(1,5))/Is 0; 0 0 0 0 -1/ms 0 1/ms zeros(1,7); zeros(1,11) -1/ms 0 1/ms; 0 0 0 0 -Ls*sin(tinc(1,6))/Is 0 -Ls*sin(tinc(1,6))/Is 0 0 0 0 -Ls*cos(tinc(1,6))/Is 0 -Ls*cos(tinc(1,6))/Is; 0 0 0 0 0 -1/mf zeros(1,8); zeros(1,12) -1/mf 0; 0 0 0 0 0 -Lf1*sin(tinc(1,7))/If zeros(1,6) -Lf1*cos(tinc(1,7))/If 0; zeros(1,6) -1/mf zeros(1,7); zeros(1,13) -1/mf; zeros(1,6) -Lf1*sin(tinc(1,8))/If zeros(1,6) -Lf1*cos(tinc(1,8))/If]; PHI = [1 0 -LH2*sin(tinc(1,1)) -1 0 -Lp*sin(tinc(1,2)) zeros(1,18); 0 1 -LH2*cos(tinc(1,1)) 0 -1 -Lp*cos(tinc(1,2)) zeros(1,18); 0 0 0 1 0 -Lp*sin(tinc(1,2)) -1 0 -Lt*sin(tinc(1,3)) zeros(1,15); 0 0 0 0 1 -Lp*cos(tinc(1,2)) 0 -1 -Lt*cos(tinc(1,3)) zeros(1,15); 0 0 0 1 0 -Lp*sin(tinc(1,2)) 0 0 0 -1 0 -Lt*sin(tinc(1,4)) zeros(1,12); 0 0 0 0 1 -Lp*cos(tinc(1,2)) 0 0 0 0 -1 -Lt*cos(tinc(1,4)) zeros(1,12); zeros(1,6) 1 0 -Lt*sin(tinc(1,3)) 0 0 0 -1 0 -Ls*sin(tinc(1,5)) zeros(1,9); zeros(1,6) 0 1 -Lt*cos(tinc(1,3)) 0 0 0 0 -1 -Ls*cos(tinc(1,5)) zeros(1,9); zeros(1,9) 1 0 -Lt*sin(tinc(1,4)) 0 0 0 -1 0 -Ls*sin(tinc(1,6)) zeros(1,6); zeros(1,9) 0 1 -Lt*cos(tinc(1,4)) 0 0 0 0 -1 -Ls*cos(tinc(1,6)) zeros(1,6); zeros(1,12) 1 0 -Ls*sin(tinc(1,5)) 0 0 0 -1 0 -Lf1*sin(tinc(1,7)) 0 0 0; zeros(1,12) 0 1 -Ls*cos(tinc(1,5)) 0 0 0 0 -1 -Lf1*cos(tinc(1,7)) 0 0 0; zeros(1,15) 1 0 -Ls*sin(tinc(1,6)) 0 0 0 -1 0 -Lf1*sin(tinc(1,8)); zeros(1,15) 0 1 -Ls*cos(tinc(1,6)) 0 0 0 0 -1 -Lf1*cos(tinc(1,8))]; % Calculate PSI PSI = [0; -g; -(Tp(1)+Ta(1))/IH; 0; -g; (Tp(1)-Tp(2)-Tp(3)+Ta(1)-Ta(2)-Ta(3))/Ip; 0; -g; (Tp(2)+Tp(4)+Ta(2)+Ta(4))/It; 0; -g; (Tp(3)+Tp(5)+Ta(3)+Ta(5))/It; 0; -g; -(Tp(4)+Tp(6)+Ta(4)+Ta(6))/Is; 0; -g; -(Tp(5)+Tp(7)+Ta(5)+Ta(7))/Is; (Fgx(1)+Fgx(3))/mf; (Fgy(1)+Fgy(3))/mf - g; (Lf2/If)*(-Fgx(1)*sin(alp1-tinc(1,7))+Fgy(1)*cos(alp1-tinc(1,7))) + (Lf3/If)*(Fgx(3)*sin(alp2+tinc(1,7))+Fgy(3)*cos(alp2+tinc(1,7))) + (Tp(6)+Ta(6))/If; (Fgx(2)+Fgx(4))/mf; (Fgy(2)+Fgy(4))/mf - g; (Lf2/If)*(-Fgx(2)*sin(alp1-tinc(1,8))+Fgy(2)*cos(alp1-tinc(1,8))) + (Lf3/If)*(Fgx(4)*sin(alp2+tinc(1,8))+Fgy(4)*cos(alp2+tinc(1,8))) + (Tp(7)+Ta(7))/If]; eta = [LH2*cos(tinc(1,1))*(dtinc(1,1))^2 + Lp*cos(tinc(1,2))*(dtinc(1,2))^2; -LH2*sin(tinc(1,1))*(dtinc(1,1))^2 - Lp*sin(tinc(1,2))*(dtinc(1,2))^2; Lp*cos(tinc(1,2))*(dtinc(1,2))^2 + Lt*cos(tinc(1,3))*(dtinc(1,3))^2; -Lp*sin(tinc(1,2))*(dtinc(1,2))^2 - Lt*sin(tinc(1,3))*(dtinc(1,3))^2; Lp*cos(tinc(1,2))*(dtinc(1,2))^2 + Lt*cos(tinc(1,4))*(dtinc(1,4))^2; -Lp*sin(tinc(1,2))*(dtinc(1,2))^2 - Lt*sin(tinc(1,4))*(dtinc(1,4))^2; Lt*cos(tinc(1,3))*(dtinc(1,3))^2 + Ls*cos(tinc(1,5))*(dtinc(1,5))^2; -Lt*sin(tinc(1,3))*(dtinc(1,3))^2 - Ls*sin(tinc(1,5))*(dtinc(1,5))^2; Lt*cos(tinc(1,4))*(dtinc(1,4))^2 + Ls*cos(tinc(1,6))*(dtinc(1,6))^2; -Lt*sin(tinc(1,4))*(dtinc(1,4))^2 - Ls*sin(tinc(1,6))*(dtinc(1,6))^2; Ls*cos(tinc(1,5))*(dtinc(1,5))^2 + Lf1*cos(tinc(1,7))*(dtinc(1,7))^2; -Ls*sin(tinc(1,5))*(dtinc(1,5))^2 - Lf1*sin(tinc(1,7))*(dtinc(1,7))^2; Ls*cos(tinc(1,6))*(dtinc(1,6))^2 + Lf1*cos(tinc(1,8))*(dtinc(1,8))^2; -Ls*sin(tinc(1,6))*(dtinc(1,6))^2 - Lf1*sin(tinc(1,8))*(dtinc(1,8))^2]; ddphi = OMG*((PHI*OMG)\(eta-PHI*PSI)) + PSI; K(:,idxavg) = ddphi; %%%%%%%%%%%%%%%%% % neural output % %%%%%%%%%%%%%%%%% %%% ipsilateral inhibition %%% Q(1) = sg(2)*w(1)*f(uinc(1,4)) + sg(5)*w(1)*f(uinc(1,6)); Q(2) = sg(3)*w(1)*f(uinc(1,3)) + sg(6)*w(1)*f(uinc(1,5)); Q(3) = (sg(4)+sg(5)-sg(6))*w(1)*f(uinc(1,7)) + (-sg(1)-sg(2)+sg(3))*w(1)*f(uinc(1,8)) + (-sg(1)-sg(2)+sg(5))*w(1)*f(uinc(1,11)) + (sg(3)+sg(4)-sg(6))*w(1)*f(uinc(1,12)); Q(4) = -Q(3); Q(5) = (sg(1)+sg(2)-sg(3))*w(1)*f(uinc(1,9)) + (-sg(4)-sg(5)+sg(6))*w(1)*f(uinc(1,10)) + (-sg(4)-sg(5)+sg(2))*w(1)*f(uinc(1,13)) + (sg(6)+sg(1)-sg(3))*w(1)*f(uinc(1,14)); Q(6) = -Q(5); Q(7) = (-sg(3)+sg(4)+sg(5))*w(2)*f(uinc(1,3)) + (-sg(1)-sg(2)+sg(6))*w(2)*f(uinc(1,4)) + (-sg(1)-sg(2)+sg(5))*w(1)*f(uinc(1,11)) + (-sg(3)+sg(4)+sg(6))*w(1)*f(uinc(1,12)); Q(8) = -Q(7); Q(9) = (-sg(6)+sg(1)+sg(2))*w(2)*f(uinc(1,5)) + (-sg(4)-sg(5)+sg(3))*w(2)*f(uinc(1,6)) + (-sg(4)-sg(5)+sg(2))*w(1)*f(uinc(1,13)) + (-sg(6)+sg(1)+sg(3))*w(1)*f(uinc(1,14)); Q(10) = -Q(9); Q(11) = (-sg(3)-sg(4)+sg(5))*w(2)*f(uinc(1,3)) + (sg(1)+sg(2)-sg(6))*w(2)*f(uinc(1,4)) + (-sg(4)+sg(5)-sg(6))*w(1)*f(uinc(1,7)) + (sg(1)+sg(2)-sg(3))*w(1)*f(uinc(1,8)); Q(12) = -Q(11); Q(13) = (-sg(6)-sg(1)+sg(2))*w(2)*f(uinc(1,5)) + (sg(4)+sg(5)-sg(3))*w(2)*f(uinc(1,6)) + (-sg(1)+sg(2)-sg(3))*w(1)*f(uinc(1,9)) + (sg(4)+sg(5)-sg(6))*w(1)*f(uinc(1,10)); Q(14) = -Q(13); %%% sensory signals %%% S(1) = -q(1)*(tinc(1,1)-0.55*pi) - q(2)*dtinc(1,1); S(2) = -S(1); S(3) = q(3)*(tinc(1,3)-pi/2) + slst*q(4)*(tinc(1,5)-pi/2) + (srst-slst)*q(5)*(GA-pi/2); S(4) = -S(3); S(5) = q(3)*(tinc(1,4)-pi/2) + srst*q(4)*(tinc(1,6)-pi/2) + (slst-srst)*q(5)*(GA-pi/2); S(6) = -S(5); S(7) = q(4)*(tinc(1,5)-pi/2) + (-srst+slst)*q(5)*f(pi/2-GA); S(8) = -S(7); S(9) = q(4)*(tinc(1,6)-pi/2) + (-slst+srst)*q(5)*f(pi/2-GA); S(10) = -S(9); S(11) = sroff*q(6)*(tinc(1,7)-0.9948) - srst*q(4)*(tinc(1,5)-pi/2) - (srst+sg(5)+sg(6))*q(5)*(GA-pi/2) - sg(4)*q(5)*f(pi/2-GA) - (sg(1)*q(7)+sg(3)*q(8))*dGA; S(12) = -S(11); S(13) = sloff*q(6)*(tinc(1,8)-0.9948) - slst*q(4)*(tinc(1,6)-pi/2) - (slst+sg(2)+sg(3))*q(5)*(GA-pi/2) - sg(1)*q(5)*f(pi/2-GA) - (sg(4)*q(7)+sg(6)*q(8))*dGA; S(14) = -S(13); for idxnrn1=1:size(u,2) sig = 0; for idxnrn2=1:size(u,2) sig = sig + wo(idxnrn1,idxnrn2)*f(uinc(1,idxnrn2)); end du(1,idxnrn1) = (-uinc(1,idxnrn1)-beta*f(vinc(1,idxnrn1))+SI*(sig+uomm(idxts)+Q(idxnrn1)+S(idxnrn1)))/tau(idxnrn1); dv(1,idxnrn1) = (-vinc(1,idxnrn1)+f(uinc(1,idxnrn1)))/dtau(idxnrn1); end stack_dudv = zeros(2*size(u,2),1); for idxnrn=1:size(u,2) stack_dudv(idxnrn*2-1:idxnrn*2) = [du(1,idxnrn);dv(1,idxnrn)]; end H(:,idxavg) = [dphiinc;stack_dudv]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % storing output for checking % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if idxavg==1 Fgy1(idxts,:) = Fgy; Fgx1(idxts,:) = Fgx; COF1(idxts,:) = COF; Fgxsum(:,1) = Fgx1(:,1) + Fgx1(:,3); Fgxsum(:,2) = Fgx1(:,2) + Fgx1(:,4); Fgysum(:,1) = Fgy1(:,1) + Fgy1(:,3); Fgysum(:,2) = Fgy1(:,2) + Fgy1(:,4); COFsum(:,1) = COF1(:,1) + COF1(:,3); COFsum(:,2) = COF1(:,2) + COF1(:,4); xf1(idxts,:) = xf; yf1(idxts,:) = yf; Tmr1(idxts,:) = Tmr; Tmi1(idxts,:) = Tmi; Ta1(idxts,:) = Ta; Tp1(idxts,:) = Tp; Q_store(idxts,:) = Q; S_store(idxts,:) = S; du_store(idxts,:) = du(1,:); dv_store(idxts,:) = dv(1,:); end %%% GA and dGA %%% GA_store(idxts,idxavg) = GA; dGA_store(idxts,idxavg) = dGA; %%% sg %%% sg1_store(idxts,idxavg) = sg(1); sg2_store(idxts,idxavg) = sg(2); sg3_store(idxts,idxavg) = sg(3); sg4_store(idxts,idxavg) = sg(4); sg5_store(idxts,idxavg) = sg(5); sg6_store(idxts,idxavg) = sg(6); % end % four averages in numerical integration %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % calculate the increment for dphi, phi, u, and v % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for idxmtn=1:size(K,1) deldq(idxts,idxmtn) = (stpsz/6)*(K(idxmtn,1)+(2-sqrt(2))*K(idxmtn,2)+(2+sqrt(2))*K(idxmtn,3)+K(idxmtn,4)); delq(idxts,idxmtn) = (stpsz/6)*(H(idxmtn,1)+(2-sqrt(2))*H(idxmtn,2)+(2+sqrt(2))*H(idxmtn,3)+H(idxmtn,4)); end for idxuv=size(K,1)+1:size(H,1) delq(idxts,idxuv) = (stpsz/6)*(H(idxuv,1)+(2-sqrt(2))*H(idxuv,2)+(2+sqrt(2))*H(idxuv,3)+H(idxuv,4)); end end if y(idxts,2) < 0.7 fall = 1; else fall = 0; end end % time steps %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % calculate fall probability % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Find end time of simulation (Fall-time or end-sim-time) if fall == 1 falltime = find(y(:,2) < 0.7); % pelvis height < 0.70 m endtime = falltime(1)-1; else if maxtoe(:,2) >= endofsimx endtime = find(any([maxtoe(:,2) >= (endofsimx-0.5), maxtoe(:,3) >= (endofsimx-0.5)])); endtime = endtime(1); else endtime = zzzzzz; end end % dall determination if maxtoe(endtime,2) < groundchangex aafallingbefore = aafallingbefore + 1; counter = counter + 1; success = 0; else % Find time of changing ground-condition timeofchangee = find(maxtoe(:,2) >= groundchangex); timeofchange = timeofchangee(1); %end %%% From ground-change-time to end-time if y(timeofchange:zzzzzz,2) >= 0.7 success = 1; aanofalling = aanofalling + 1; else success = -1; aafalling = aafalling + 1; end end if not(success == 0) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % MOS (margin of stability) % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for ct1= timeofchange:endtime aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx) = ct1; aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+1) = zzFgyFgx(ct1,1); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+2) = zzFgyFgx(ct1,2); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+3) = zzFgyFgx(ct1,3); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+4) = zzFgyFgx(ct1,4); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+5) = zzFgyFgx(ct1,5); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+6) = zzFgyFgx(ct1,6); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+7) = zzFgyFgx(ct1,7); aaFgyFgx(ct1-timeofchange+1,zcountfgyfgx+8) = zzFgyFgx(ct1,8); aaaFgyFgx(ct1-timeofchange+1,zcountafgyfgx) = ct1; if any([zzFgyFgx(ct1,3) == 2, zzFgyFgx(ct1,3) == 3]) aaaFgyFgx(ct1-timeofchange+1,zcountafgyfgx+1) = 0; else aaaFgyFgx(ct1-timeofchange+1,zcountafgyfgx+1) = zzFgyFgx(ct1,1); end if any([zzFgyFgx(ct1,4) == 2, zzFgyFgx(ct1,4) == 3]) aaaFgyFgx(ct1-timeofchange+1,zcountafgyfgx+2) = 0; else aaaFgyFgx(ct1-timeofchange+1,zcountafgyfgx+2) = zzFgyFgx(ct1,2); end end %%%%%%%%%%%%%%%%%%%%%% % calculation of MOS % %%%%%%%%%%%%%%%%%%%%%% for ct1= 1:endtime % COM COM(ct1,1) = ((mH*x(ct1,1))+(mp*x(ct1,2))+(mt*x(ct1,3))+(mt*x(ct1,4))+(ms*x(ct1,5))+(ms*x(ct1,6))+(mf*x(ct1,7))+(mf*x(ct1,8)))/70; COM(ct1,2) = ((mH*y(ct1,1))+(mp*y(ct1,2))+(mt*y(ct1,3))+(mt*y(ct1,4))+(ms*y(ct1,5))+(ms*y(ct1,6))+(mf*y(ct1,7))+(mf*y(ct1,8)))/70; % VCOM if ct1 ~= 1 vCOM(ct1,1)=(COM(ct1,1)-COM(ct1-1,1))*4000; end aaMoI(ct1,zcountmoi) = ct1; % anterior BOS boundary aaMoI(ct1,zcountmoi+1) = maxtoe(ct1,2); % posterior BOS boundary aaMoI(ct1,zcountmoi+2) = min(xf1(ct1,1),xf1(ct1,2)); end % XCOM aaMoI(:,zcountmoi+3) = COM(:,1)+(vCOM(:,1)./sqrt(g/0.9)); % anterior MOS aaMoI(:,zcountmoi+4) = aaMoI(:,zcountmoi+1)-aaMoI(:,zcountmoi+3); % posterior MOS aaMoI(:,zcountmoi+5) = aaMoI(:,zcountmoi+3)-aaMoI(:,zcountmoi+2); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % calculation of gait parameters % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%when the right heel makes a landing ii = 2; iii = 1; while ii < zzzzzz if yf1(ii,1) < 0 && yf1(ii-1,1) > 0 rightfootontheground(iii) = ii; ii = ii + 3000; iii = iii + 1; else ii = ii + 1; end end %%Calculating the gait parameters ii = length(rightfootontheground); iii =1; while iii < ii-1 if rightfootontheground(iii) < endtime aaGaitParameters(iii,zcountgaitparameters) = iii; % Stridelength aaGaitParameters(iii,zcountgaitparameters+1) = abs(xf1(rightfootontheground(iii+1),3)-xf1(rightfootontheground(iii),3)); % Walkingspeed aaGaitParameters(iii,zcountgaitparameters+2) = aaGaitParameters(iii,zcountgaitparameters+1) /((rightfootontheground(iii+1)-rightfootontheground(iii))/4000); % Footclearance aaGaitParameters(iii,zcountgaitparameters+4) = max(yf1(rightfootontheground(iii):rightfootontheground(iii+1),3)); % Step-Frequency aaGaitParameters(iii,zcountgaitparameters+3) = 1/((rightfootontheground(iii+1)-rightfootontheground(iii))/4000); end iii = iii + 1; end % Memorize the fall-time and changeground-time aafalltime(zcountfalltime,1) = endtime; aafalltime(zcountfalltime,2) = maxtoe(endtime,2); if not(success == 0) aafalltime(zcountfalltime,3) = timeofchange; aafalltime(zcountfalltime,4) = maxtoe(timeofchange,2); end aafalltime(zcountfalltime,5) = fall; if not(success == 0) zcountfalltime = zcountfalltime + 1; zcountgaitparameters = zcountgaitparameters + 6; zcountfgyfgx = zcountfgyfgx + 9; zcountmoi = zcountmoi + 6; zcountafgyfgx = zcountafgyfgx +3; end dt = datetime('now'); disp(dt) end % While-counter ends %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % summary for fall probility at each muS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% aasummary(si,1) = bgx2; aasummary(si,2) = kgx2; aasummary(si,3) = sf; aasummary(si,4) = muS; aasummary(si,5) = aanofalling; % success aasummary(si,6) = aafalling; % fall aasummary(si,7) = aafallingbefore; % fall before groundchangex aasummary(si,8) = TrialNumber; % trial numbers aasummary(si,9) = (aasummary(si,6)/TrialNumber); % fall probability si = si+1; fc = fc+1; aafallingbefore = 0; aafalling = 0; aanofalling = 0; end % Ends the SF-for-counter dt = datetime('now'); disp(dt) si = si+1; fc = fc+1; aafallingbefore = 0; aafalling = 0; aanofalling = 0; zcountfalltime = zcountfalltime + 1; zcountgaitparameters = zcountgaitparameters + 1; zcountfgyfgx = zcountfgyfgx + 1; zcountmoi = zcountmoi + 1; end % end of script ---------------------------------------------------------8x