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
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 = [];