1function [F ds rec_out] = contactforce_control(t, N, dt, q, dq, t_all, x_all, cons, s_in, rec_in, Tj, Tm, Tp, dTj, dTm, dTp, M)
 2%#codegen 
 3F = zeros(9, 1); 
 4pm = zeros(0,1); 
 5vm = zeros(0,1); 
 6dxy = dTp*dq; 
 7Fc = zeros(length(Tp), 1); 
 8y = min(0, Tp(2)); 
 9dx = dxy(1); 
10dy = dxy(2); 
11Fc(2) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
12Fx = -500 * dx; 
13FxMax = Fc(2) * 1.5; 
14Fc(1) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
15y = min(0, Tp(5)); 
16dx = dxy(4); 
17dy = dxy(5); 
18Fc(5) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
19Fx = -500 * dx; 
20FxMax = Fc(5) * 1.5; 
21Fc(4) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
22y = min(0, Tp(8)); 
23dx = dxy(7); 
24dy = dxy(8); 
25Fc(8) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
26Fx = -500 * dx; 
27FxMax = Fc(8) * 1.5; 
28Fc(7) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
29y = min(0, Tp(11)); 
30dx = dxy(10); 
31dy = dxy(11); 
32Fc(11) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
33Fx = -500 * dx; 
34FxMax = Fc(11) * 1.5; 
35Fc(10) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
36y = min(0, Tp(14)); 
37dx = dxy(13); 
38dy = dxy(14); 
39Fc(14) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
40Fx = -500 * dx; 
41FxMax = Fc(14) * 1.5; 
42Fc(13) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
43y = min(0, Tp(17)); 
44dx = dxy(16); 
45dy = dxy(17); 
46Fc(17) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
47Fx = -500 * dx; 
48FxMax = Fc(17) * 1.5; 
49Fc(16) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
50y = min(0, Tp(20)); 
51dx = dxy(19); 
52dy = dxy(20); 
53Fc(20) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
54Fx = -500 * dx; 
55FxMax = Fc(20) * 1.5; 
56Fc(19) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
57y = min(0, Tp(23)); 
58dx = dxy(22); 
59dy = dxy(23); 
60Fc(23) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
61Fx = -500 * dx; 
62FxMax = Fc(23) * 1.5; 
63Fc(22) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
64y = min(0, Tp(26)); 
65dx = dxy(25); 
66dy = dxy(26); 
67Fc(26) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
68Fx = -500 * dx; 
69FxMax = Fc(26) * 1.5; 
70Fc(25) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
71y = min(0, Tp(29)); 
72dx = dxy(28); 
73dy = dxy(29); 
74Fc(29) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
75Fx = -500 * dx; 
76FxMax = Fc(29) * 1.5; 
77Fc(28) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
78y = min(0, Tp(32)); 
79dx = dxy(31); 
80dy = dxy(32); 
81Fc(32) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
82Fx = -500 * dx; 
83FxMax = Fc(32) * 1.5; 
84Fc(31) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
85y = min(0, Tp(35)); 
86dx = dxy(34); 
87dy = dxy(35); 
88Fc(35) = max(0,  min(500000000 * (-(y^3) + dy * y^3), 5000)); 
89Fx = -500 * dx; 
90FxMax = Fc(35) * 1.5; 
91Fc(34) = sign(Fx)*min(abs(Fx),abs(FxMax)); 
92rec_out = Fc; 
93F = dTp' * Fc; 
94ds = [];