1function [F ds rec_out] = reflex_control(t, N, dt, q, dq, t_all, x_all, cons, s_in, rec_in, Tj, Tm, Tp, dTj, dTm, dTp, M)
2
3F = zeros(9, 1);
4pm = zeros(6,1);
5vm = zeros(6,1);
6pm(1) = q(4) - q(3);
7vm(1) = dq(4) - dq(3);
8pm(2) = q(5) - q(4);
9vm(2) = dq(5) - dq(4);
10pm(3) = q(6) - q(5);
11vm(3) = dq(6) - dq(5);
12pm(4) = q(7) - q(3);
13vm(4) = dq(7) - dq(3);
14pm(5) = q(8) - q(7);
15vm(5) = dq(8) - dq(7);
16pm(6) = q(9) - q(8);
17vm(6) = dq(9) - dq(8);
18rec_s = interpfast(t - cons.dt_s, dt, t_all, rec_in);
19rec_m = interpfast(t - cons.dt_m, dt, t_all, rec_in);
20rec_f = interpfast(t - cons.dt_f, dt, t_all, rec_in);
21x_s = interpfast(t - cons.dt_s, dt, t_all, x_all);
22x_m = interpfast(t - cons.dt_m, dt, t_all, x_all);
23x_f = interpfast(t - cons.dt_f, dt, t_all, x_all);
24r.q_ref_s = x_s([1 2 3 4 5 6 7 8 9]);
25r.dq_ref_s = x_s([10 11 12 13 14 15 16 17 18]);
26r.l_CE_s = x_s([33 34 35 36 37 38 39 40 41 42 43 44 45 46]);
27r.q_ref_m = x_s([1 2 3 4 5 6 7 8 9]);
28r.dq_ref_m = x_s([10 11 12 13 14 15 16 17 18]);
29r.l_CE_m = x_s([33 34 35 36 37 38 39 40 41 42 43 44 45 46]);
30r.q_ref_f = x_s([1 2 3 4 5 6 7 8 9]);
31r.dq_ref_f = x_s([10 11 12 13 14 15 16 17 18]);
32r.l_CE_f = x_s([33 34 35 36 37 38 39 40 41 42 43 44 45 46]);
33r.refForce_s = rec_s([43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92]);
34r.refForce_m = rec_m([43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92]);
35r.refForce_f = rec_f([43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92]);
36r.qrel_ref_m = zeros(6,1);
37r.dqrel_ref_m = zeros(6,1);
38r.qrel_ref_m(1) = r.q_ref_m(4) - r.q_ref_m(3);
39r.dqrel_ref_m(1) = r.dq_ref_m(4) - r.dq_ref_m(3);
40r.qrel_ref_m(2) = r.q_ref_m(5) - r.q_ref_m(4);
41r.dqrel_ref_m(2) = r.dq_ref_m(5) - r.dq_ref_m(4);
42r.qrel_ref_m(3) = r.q_ref_m(6) - r.q_ref_m(5);
43r.dqrel_ref_m(3) = r.dq_ref_m(6) - r.dq_ref_m(5);
44r.qrel_ref_m(4) = r.q_ref_m(7) - r.q_ref_m(3);
45r.dqrel_ref_m(4) = r.dq_ref_m(7) - r.dq_ref_m(3);
46r.qrel_ref_m(5) = r.q_ref_m(8) - r.q_ref_m(7);
47r.dqrel_ref_m(5) = r.dq_ref_m(8) - r.dq_ref_m(7);
48r.qrel_ref_m(6) = r.q_ref_m(9) - r.q_ref_m(8);
49r.dqrel_ref_m(6) = r.dq_ref_m(9) - r.dq_ref_m(8);
50A = s_in([1 2 3 4 5 6 7 8 9 10 11 12 13 14]);
51l_CE = s_in([15 16 17 18 19 20 21 22 23 24 25 26 27 28]);
52s_filt = s_in([29 30]);
53fcglobal = rec_in([1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36], max(1, N-1));
54rec = rec_in([43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92], max(1, N-1));
55rec_out = rec_in([43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92], N);
56[mt ds rec_out] = calcreflex(t, N, pm, q, A, l_CE, s_filt, rec, rec_out, cons, r, fcglobal);
57F(4) = F(4) + mt(1);
58F(3) = F(3) - mt(1);
59F(5) = F(5) + mt(2);
60F(4) = F(4) - mt(2);
61F(6) = F(6) + mt(3);
62F(5) = F(5) - mt(3);
63F(7) = F(7) + mt(4);
64F(3) = F(3) - mt(4);
65F(8) = F(8) + mt(5);
66F(7) = F(7) - mt(5);
67F(9) = F(9) + mt(6);
68F(8) = F(8) - mt(6);