1function mt = ligamentscalctorque(q, dq, cons)
2
3
4
5
6
7mt = zeros(6,1);
8
9for i = 1:6
10 mt(i) = - cons.CM_min(i) * exp(-cons.CP_min(i)*(-q(i)-cons.q_min(i)))...
11 + cons.CM_max(i) * exp(-cons.CP_max(i)*(cons.q_max(i)+q(i)))...
12 - cons.C_damp(i) * dq(i);
13end;
14
15end
16
17