1function [tm, ds, rec_out] = calcreflex(t, N, pm, q, A, l_CE, s_filt, rec, rec_out, c, r, fcglobal)
2
3
4
5
6lc = length(fcglobal);
7contacts = [any(fcglobal(1:(lc/2))) any(fcglobal((lc/2+1):end))];
8refi= [1 2 3; 4 5 6];
9
10theta_f = -r.q_ref_f(3);
11dtheta_f = -r.dq_ref_f(3);
12
13if N == 1
14 qtheta_pto = [-q(3); -q(3)];
15else
16 qtheta_pto = rec([29; 30]);
17end
18
19
20
21contactForce = [sum(fcglobal((2:3:(lc/2))))/800, ...
22 sum(fcglobal(lc/2 + (2:3:(lc/2))))/800];
23ds_Force = [c.filt.A*s_filt(1) + c.filt.B*contactForce(1); ...
24 c.filt.A*s_filt(2) + c.filt.B*contactForce(2)];
25contactForceFilt = [c.filt.C*s_filt(1) + c.filt.D*contactForce(1); ...
26 c.filt.C*s_filt(2) + c.filt.D*contactForce(2)];
27
28
29
30nMuscles = 7;
31S = zeros(14, 1);
32
33for i = 1:2
34
35 iS = (i-1) * nMuscles;
36
37 if contacts(i)
38
39 qtheta_pto(i) = -q(3);
40
41
42 not_i = 3-i;
43
44 if contacts(not_i) && (pm(refi(i,1)) < pm(refi(not_i,1)))
45 DSup = 1;
46 else
47 DSup = 0;
48 end
49
50 Fleg_f = contactForceFilt(i);
51 Fleg_contra_f = contactForceFilt(not_i);
52
53
54 S(iS + 1) = c.S_0 ...
55 + c.m(1).G * r.refForce_s(iS+1);
56
57
58
59
60 S(iS + 2) = c.S_0 ...
61 + max(0, c.m(2).G * (r.l_CE_s(iS+2)-c.m(2).l_off)) ...
62 - max(0, c.m(2).G2 * (r.refForce_s(iS+1) + r.refForce_s(iS+3))/2);
63
64
65
66 S(iS + 3) = c.S_0 ...
67 + c.m(3).G * r.refForce_s(iS+3);
68 S(iS + 4) = c.m(4).S_0 ...
69 + c.m(4).G * r.refForce_m(iS+4) ...
70 - c.k_phi ...
71 * (r.qrel_ref_m(refi(i,2)) - c.phi_knee_off)...
72 * (r.qrel_ref_m(refi(i,2)) > c.phi_knee_off)...
73 * (r.dqrel_ref_m(refi(i,2)) > 0) ...
74 - c.k_bw * abs(Fleg_contra_f) * DSup;
75 S(iS + 5) = c.S_0_bal + (...
76 c.k_p * (theta_f - c.theta_ref) ...
77 + c.k_d * dtheta_f) ...
78 * c.k_bw * abs(Fleg_f);
79 S(iS + 6) = c.S_0_bal + ...
80 max(0, ...
81 0.68 * c.k_p * (theta_f - c.theta_ref) ...
82 + c.k_d * dtheta_f) ...
83 * c.k_bw * abs(Fleg_f) ...
84 - c.dS * DSup;
85 S(iS + 7) = (c.S_0_bal ...
86 - min(0, c.k_p * (theta_f - c.theta_ref) ...
87 + c.k_d * dtheta_f)) ...
88 * c.k_bw * abs(Fleg_f) ...
89 + c.dS * DSup;
90 else
91
92 S(iS + 1) = c.S_0;
93 S(iS + 2) = c.S_0 ...
94 + c.m(2).G * (r.l_CE_s(iS+2)-c.m(2).l_off);
95 S(iS + 3) = c.S_0;
96 S(iS + 4) = c.S_0;
97 S(iS + 5) = c.S_0 ...
98 + c.m(5).G * r.refForce_f(iS+5);
99 S(iS + 6) = c.S_0 ...
100 + c.m(6).G * r.refForce_f(iS+6);
101 S(iS + 7) = c.S_0 ...
102 + max(0, c.m(7).G * (r.l_CE_f(iS+7)-c.m(7).l_off)) ...
103 - max(0, c.m(7).G2 * (r.l_CE_f(iS+5)-c.m(5).l_off)) ...
104 + c.k_lean * (qtheta_pto(i) - c.theta_ref);
105 end
106end
107
108musclenoise = rec_out(31:44) * c.maxnoise;
109S = S + S .* musclenoise;
110
111S = min(1, max(0.01, S));
112if t <= c.dt_f
113 S(:) = 0.01;
114elseif t <= c.dt_m
115 S([1 2 3 4 8 9 10 11]) = 0.01;
116elseif t <= c.dt_s
117 S([1 2 3 8 9 10]) = 0.01;
118end
119
120[v_CE, dA, F_m_out, F_CE, tm] = loopMuscles(c.m, pm(refi), l_CE, A, S);
121tm = -tm;
122
123
124ds = [dA; v_CE; ds_Force];
125
126rec_out(1:30) = [F_m_out; F_CE; qtheta_pto];
127rec_out(45:50) = tm;
128
129end
130