-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAngleAccelN.m
44 lines (42 loc) · 9.33 KB
/
AngleAccelN.m
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
37
38
39
40
41
42
43
44
function ddq_modN = AngleAccelN(b1,b2,b3,dth1,dth2,dth3,l1,l3,m1,m3,th1,th2,th3,u1,u2,u3)
%ANGLEACCELN
% DDQ_MODN = ANGLEACCELN(B1,B2,B3,DTH1,DTH2,DTH3,L1,L3,M1,M3,TH1,TH2,TH3,U1,U2,U3)
% This function was generated by the Symbolic Math Toolbox version 7.2.
% 08-Dec-2017 22:59:24
t2 = cos(th2);
t3 = cos(th3);
t4 = sin(th2);
t5 = sin(th1);
t6 = l3.^2;
t7 = cos(th1);
t8 = sin(th3);
t9 = dth1.^2;
t10 = t2.^2;
t11 = t3.^2;
t12 = t7.^2;
t13 = dth2.^2;
t14 = l1.^2;
t15 = 1.0./l1.^2;
t16 = 1.0./l3;
t17 = 1.0./t3;
t18 = m3.*t10.*t11.*4.0;
t19 = m1+t18;
t20 = 1.0./t19;
t21 = dth3.^2;
t22 = t8.^2;
t23 = t4.^2;
t24 = 1.0./l3.^2;
t25 = t22-1.0;
t26 = 1.0./t25;
t27 = m3.*4.0;
t28 = m3.*t22.*t23.*4.0;
t29 = m1+t27+t28-m3.*t22.*4.0-m3.*t23.*4.0;
t30 = 1.0./t29;
t31 = 1.0./m3;
t32 = m3.^2;
t33 = t10.^2;
t34 = t6.^2;
t35 = t11.^2;
t36 = th2.*2.0;
t37 = sin(t36);
ddq_modN = [t15.*t16.*t17.*t20.*(l1.*t4.*u2.*2.0e2-l3.*t3.*u1.*2.0e2+b1.*dth1.*l3.*t3.*2.0e2-b2.*dth2.*l1.*t4.*2.0e2+l3.*t2.*t3.*u3.*2.0e2+l3.*t4.*t5.*u2.*2.0e2-dth1.*dth3.*l1.*m3.*t6.*2.0e2-b3.*dth3.*l3.*t2.*t3.*2.0e2-b2.*dth2.*l3.*t4.*t5.*2.0e2-l1.*l3.*m1.*t3.*t7.*9.81e2+l1.*m3.*t2.*t6.*t9.*4.0e2+l1.*t2.*t3.*t8.*u3.*2.0e2+m3.*t3.*t5.*t6.*t7.*1.962e3-m3.*t3.*t6.*t7.*t8.*1.962e3-l1.*m3.*t2.*t6.*t9.*t10.*4.0e2-l1.*m3.*t2.*t6.*t9.*t12.*6.0e2+l3.*m3.*t2.*t5.*t9.*t14.*4.0e2-l3.*m3.*t2.*t8.*t9.*t14.*4.0e2+l1.*m3.*t2.*t6.*t11.*t21.*2.0e2+l1.*m3.*t2.*t6.*t13.*t35.*2.0e2-m3.*t3.*t5.*t6.*t7.*t10.*1.962e3+m3.*t3.*t6.*t7.*t8.*t10.*1.962e3-dth1.*dth3.*l3.*m3.*t5.*t6.*2.0e2+dth1.*dth3.*l1.*m3.*t6.*t10.*2.0e2+dth1.*dth3.*l3.*m3.*t6.*t8.*2.0e2-b3.*dth3.*l1.*t2.*t3.*t8.*2.0e2-dth2.*dth3.*l3.*m3.*t3.*t4.*t6.*2.0e2+dth1.*dth3.*l1.*m3.*t5.*t6.*t8.*2.0e2+dth1.*dth3.*l3.*m3.*t5.*t6.*t10.*2.0e2-dth1.*dth3.*l3.*m3.*t6.*t8.*t10.*2.0e2+dth1.*dth3.*l1.*m3.*t6.*t10.*t11.*4.0e2-dth1.*dth3.*l3.*m3.*t6.*t8.*t12.*2.0e2+l1.*l3.*m3.*t2.*t5.*t8.*t11.*1.962e3-l1.*l3.*m3.*t3.*t7.*t10.*t11.*1.962e3-l1.*m3.*t2.*t5.*t6.*t8.*t9.*4.0e2-l3.*m3.*t2.*t5.*t6.*t9.*t12.*2.0e2+l1.*m3.*t2.*t6.*t9.*t10.*t11.*2.0e2+l1.*m3.*t2.*t6.*t9.*t10.*t12.*6.0e2+l3.*m3.*t2.*t5.*t6.*t11.*t13.*2.0e2-l3.*m3.*t2.*t5.*t9.*t10.*t14.*4.0e2-l3.*m3.*t2.*t6.*t8.*t11.*t13.*2.0e2+l3.*m3.*t2.*t8.*t9.*t10.*t14.*4.0e2+dth1.*dth2.*l3.*m3.*t2.*t3.*t4.*t6.*2.0e2-dth1.*dth3.*l1.*m3.*t5.*t6.*t8.*t10.*2.0e2+dth1.*dth3.*l3.*m3.*t6.*t8.*t10.*t12.*2.0e2+l1.*m3.*t2.*t5.*t6.*t8.*t9.*t10.*4.0e2+l3.*m3.*t2.*t5.*t6.*t9.*t10.*t12.*2.0e2+l3.*m3.*t2.*t8.*t9.*t10.*t11.*t14.*2.0e2+dth1.*dth2.*l1.*m3.*t2.*t3.*t4.*t5.*t6.*6.0e2-dth1.*dth2.*l1.*m3.*t2.*t3.*t4.*t6.*t8.*6.0e2-dth1.*dth2.*l3.*m3.*t2.*t3.*t4.*t6.*t12.*4.0e2+dth2.*dth3.*l3.*m3.*t3.*t4.*t5.*t6.*t8.*2.0e2-dth1.*dth2.*l3.*m3.*t2.*t3.*t4.*t5.*t6.*t8.*2.0e2+dth1.*dth2.*l1.*m3.*t2.*t3.*t4.*t5.*t6.*t11.*2.0e2).*(-1.0./5.0e1);t15.*t16.*t17.*t20.*(l1.*t4.*u1.*2.0e2-b1.*dth1.*l1.*t4.*2.0e2+l3.*t4.*t5.*u1.*2.0e2+m1.*t4.*t7.*t14.*9.81e2+m3.*t4.*t7.*t14.*1.962e3+dth2.*dth3.*l1.*m3.*t6.*2.0e2-b1.*dth1.*l3.*t4.*t5.*2.0e2+m3.*t2.*t3.*t4.*t6.*1.962e3+l1.*l3.*m1.*t4.*t5.*t7.*9.81e2+l1.*l3.*m3.*t4.*t5.*t7.*1.962e3+l1.*l3.*m3.*t4.*t7.*t8.*1.962e3-l3.*m3.*t4.*t6.*t7.*t9.*2.0e2-l3.*m3.*t4.*t7.*t9.*t14.*2.0e2-m3.*t2.*t3.*t4.*t6.*t12.*1.962e3+m3.*t4.*t5.*t6.*t7.*t8.*1.962e3-dth1.*dth2.*l1.*m3.*t2.*t6.*8.0e2+dth2.*dth3.*l3.*m3.*t5.*t6.*2.0e2-dth2.*dth3.*l1.*m3.*t6.*t10.*2.0e2+dth2.*dth3.*l3.*m3.*t6.*t8.*2.0e2+dth2.*dth3.*l3.*m3.*t8.*t14.*4.0e2+dth1.*dth2.*l1.*m3.*t2.*t6.*t10.*8.0e2+dth1.*dth2.*l1.*m3.*t2.*t6.*t12.*1.2e3+dth2.*dth3.*l1.*m3.*t5.*t6.*t8.*6.0e2-dth1.*dth2.*l3.*m3.*t2.*t5.*t14.*8.0e2-dth2.*dth3.*l3.*m3.*t5.*t6.*t10.*2.0e2+dth1.*dth2.*l3.*m3.*t2.*t8.*t14.*8.0e2-dth2.*dth3.*l3.*m3.*t6.*t8.*t10.*2.0e2-dth2.*dth3.*l3.*m3.*t6.*t8.*t12.*2.0e2-dth2.*dth3.*l3.*m3.*t8.*t10.*t14.*4.0e2+l1.*l3.*m3.*t2.*t3.*t4.*t5.*1.962e3-l3.*m3.*t2.*t3.*t4.*t6.*t13.*2.0e2-l1.*m3.*t4.*t5.*t6.*t7.*t9.*4.0e2-l3.*m3.*t2.*t3.*t4.*t13.*t14.*2.0e2+l3.*m3.*t4.*t6.*t7.*t9.*t10.*2.0e2+l3.*m3.*t4.*t6.*t7.*t9.*t12.*2.0e2-l3.*m3.*t2.*t3.*t4.*t14.*t21.*2.0e2+l3.*m3.*t4.*t7.*t9.*t10.*t14.*2.0e2+dth1.*dth2.*l1.*m3.*t2.*t5.*t6.*t8.*8.0e2+dth1.*dth2.*l3.*m3.*t2.*t5.*t6.*t12.*4.0e2-dth1.*dth2.*l1.*m3.*t2.*t6.*t10.*t12.*1.2e3-dth2.*dth3.*l1.*m3.*t5.*t6.*t8.*t10.*6.0e2+dth1.*dth2.*l3.*m3.*t2.*t5.*t10.*t14.*8.0e2-dth1.*dth3.*l3.*m3.*t3.*t4.*t10.*t14.*4.0e2-dth1.*dth2.*l3.*m3.*t2.*t8.*t10.*t14.*8.0e2+dth2.*dth3.*l3.*m3.*t6.*t8.*t10.*t12.*2.0e2-l1.*m3.*t2.*t3.*t4.*t5.*t6.*t13.*4.0e2-l1.*m3.*t2.*t3.*t4.*t5.*t6.*t21.*2.0e2+l1.*m3.*t4.*t5.*t6.*t7.*t9.*t10.*4.0e2+l3.*m3.*t2.*t3.*t4.*t6.*t12.*t13.*2.0e2-l3.*m3.*t4.*t6.*t7.*t9.*t10.*t12.*2.0e2-dth1.*dth3.*l1.*m3.*t3.*t4.*t5.*t6.*t10.*4.0e2-dth1.*dth2.*l1.*m3.*t2.*t5.*t6.*t8.*t10.*8.0e2-dth1.*dth2.*l3.*m3.*t2.*t5.*t6.*t10.*t12.*4.0e2).*(-1.0./5.0e1)-t15.*t24.*t26.*t30.*t31.*(m1.*t14+m3.*t14.*4.0-m3.*t14.*t22.*4.0+m3.*t14.*t22.*t23.*4.0+m3.*t5.^2.*t6.*t23.*4.0+l1.*l3.*m3.*t5.*t23.*8.0).*(u2-b2.*dth2+dth2.*dth3.*m3.*t6.*sin(th3.*2.0)-dth1.*dth3.*m3.*t4.*t6+l3.*m3.*t3.*t4.*t7.*(9.81e2./1.0e2)-m3.*t3.*t4.*t6.*t7.*t9-m3.*t2.*t4.*t6.*t9.*t12+dth1.*dth3.*m3.*t4.*t5.*t6.*t8+l1.*l3.*m3.*t2.*t4.*t5.*t9.*2.0-l1.*l3.*m3.*t2.*t4.*t8.*t9.*2.0)-t2.*t3.*t4.*t15.*t24.*t26.*t30.*(l1+l3.*t5).*(l3+l1.*t8).*(u3-b3.*dth3+l3.*m3.*t3.*t5.*(9.81e2./1.0e2)+dth1.*dth2.*m3.*t4.*t6+l3.*m3.*t2.*t7.*t8.*(9.81e2./1.0e2)-m3.*t3.*t6.*t8.*t13+l1.*l3.*m3.*t3.*t9.*t10-dth1.*dth2.*m3.*t4.*t5.*t6.*t8).*4.0;t15.*t17.*t20.*t24.*t31.*(m1.*t3.*t14.*u3.*1.0e2-b3.*dth3.*m1.*t3.*t14.*1.0e2+l1.*l3.*m3.*t37.*u2.*2.0e2-m3.*t2.*t3.*t6.*u1.*4.0e2+m3.*t3.*t6.*t10.*u3.*4.0e2+m3.*t3.*t10.*t14.*u3.*4.0e2+l3.*m1.*m3.*t5.*t11.*t14.*9.81e2+l1.*l3.*t6.*t9.*t10.*t32.*8.0e2-l1.*l3.*t9.*t10.*t14.*t32.*8.0e2-l1.*l3.*t6.*t9.*t32.*t33.*8.0e2+l1.*l3.*t9.*t14.*t32.*t33.*8.0e2-l1.*t2.*t3.*t6.*t7.*t32.*3.924e3+l3.*t5.*t10.*t11.*t14.*t32.*3.924e3+m3.*t2.*t4.*t5.*t6.*u2.*4.0e2+m3.*t2.*t4.*t8.*t14.*u2.*4.0e2-t5.*t9.*t10.*t12.*t32.*t34.*4.0e2+t5.*t10.*t11.*t13.*t32.*t34.*4.0e2-t8.*t10.*t11.*t13.*t32.*t34.*4.0e2+t5.*t9.*t12.*t32.*t33.*t34.*4.0e2-m1.*m3.*t6.*t13.*t14.*(t8-t8.*t22).*1.0e2-b2.*dth2.*l1.*l3.*m3.*t37.*2.0e2+b1.*dth1.*m3.*t2.*t3.*t6.*4.0e2-b3.*dth3.*m3.*t3.*t6.*t10.*4.0e2-b3.*dth3.*m3.*t3.*t10.*t14.*4.0e2-dth1.*dth3.*t2.*t5.*t32.*t34.*4.0e2+dth1.*dth3.*t2.*t8.*t32.*t34.*4.0e2-b2.*dth2.*m3.*t2.*t4.*t5.*t6.*4.0e2-b2.*dth2.*m3.*t2.*t4.*t8.*t14.*4.0e2+dth1.*dth3.*t2.*t5.*t6.*t14.*t32.*4.0e2-dth1.*dth3.*t2.*t6.*t8.*t14.*t32.*4.0e2-dth2.*dth3.*t2.*t3.*t4.*t32.*t34.*4.0e2+dth1.*dth2.*t3.*t4.*t10.*t32.*t34.*4.0e2+dth1.*dth3.*t2.*t5.*t10.*t32.*t34.*4.0e2-dth1.*dth3.*t2.*t8.*t10.*t32.*t34.*4.0e2-dth1.*dth3.*t2.*t8.*t12.*t32.*t34.*4.0e2-l1.*l3.*m3.*t2.*t3.*t8.*u1.*4.0e2-l1.*m1.*m3.*t2.*t3.*t6.*t7.*1.962e3+l1.*l3.*m3.*t3.*t8.*t10.*u3.*8.0e2-l1.*l3.*t6.*t9.*t10.*t12.*t32.*1.2e3-l1.*l3.*t6.*t10.*t11.*t13.*t32.*4.0e2+l1.*l3.*t9.*t10.*t11.*t14.*t32.*8.0e2+l1.*l3.*t6.*t10.*t11.*t21.*t32.*4.0e2+l1.*l3.*t6.*t9.*t11.*t32.*t33.*4.0e2+l1.*l3.*t6.*t9.*t12.*t32.*t33.*1.2e3+l1.*l3.*t6.*t10.*t13.*t32.*t35.*8.0e2-l1.*l3.*t9.*t11.*t14.*t32.*t33.*4.0e2+l3.*t2.*t3.*t5.*t6.*t7.*t32.*3.924e3+l1.*t2.*t3.*t6.*t7.*t10.*t32.*3.924e3-l3.*t2.*t3.*t6.*t7.*t8.*t32.*3.924e3+l1.*t2.*t3.*t6.*t7.*t11.*t32.*3.924e3+l1.*t5.*t6.*t8.*t10.*t11.*t32.*3.924e3+t5.*t6.*t9.*t10.*t11.*t14.*t32.*8.0e2-t6.*t8.*t9.*t10.*t12.*t14.*t32.*1.2e3+t6.*t8.*t10.*t11.*t14.*t21.*t32.*4.0e2-t5.*t6.*t9.*t11.*t14.*t32.*t33.*8.0e2+t6.*t8.*t9.*t11.*t14.*t32.*t33.*8.0e2+t6.*t8.*t9.*t12.*t14.*t32.*t33.*1.2e3+b1.*dth1.*l1.*l3.*m3.*t2.*t3.*t8.*4.0e2-b3.*dth3.*l1.*l3.*m3.*t3.*t8.*t10.*8.0e2-dth1.*dth3.*l1.*l3.*t2.*t6.*t11.*t32.*4.0e2-dth1.*dth3.*l1.*l3.*t2.*t6.*t12.*t32.*4.0e2+dth1.*dth2.*m1.*m3.*t3.*t4.*t6.*t14.*1.0e2-dth1.*dth2.*t3.*t4.*t6.*t10.*t14.*t32.*1.2e3-dth1.*dth3.*t2.*t5.*t6.*t10.*t14.*t32.*4.0e2-dth1.*dth3.*t2.*t5.*t6.*t11.*t14.*t32.*4.0e2+dth1.*dth3.*t2.*t6.*t8.*t10.*t14.*t32.*4.0e2-dth1.*dth2.*t3.*t4.*t10.*t12.*t32.*t34.*8.0e2+dth1.*dth3.*t2.*t8.*t10.*t12.*t32.*t34.*4.0e2+l1.*l3.*m1.*m3.*t9.*t10.*t11.*t14.*1.0e2+l1.*l3.*m3.*t2.*t4.*t5.*t8.*u2.*4.0e2-l3.*m1.*m3.*t2.*t3.*t7.*t8.*t14.*9.81e2-l1.*l3.*t5.*t6.*t8.*t9.*t10.*t32.*8.0e2+l1.*l3.*t5.*t8.*t9.*t10.*t14.*t32.*8.0e2+l1.*l3.*t5.*t6.*t8.*t9.*t32.*t33.*8.0e2-l1.*l3.*t5.*t8.*t9.*t14.*t32.*t33.*8.0e2+l1.*t2.*t3.*t5.*t6.*t7.*t8.*t32.*3.924e3-l3.*t2.*t3.*t5.*t6.*t7.*t10.*t32.*3.924e3+l3.*t2.*t3.*t6.*t7.*t8.*t10.*t32.*3.924e3-l1.*t2.*t3.*t6.*t7.*t10.*t11.*t32.*7.848e3-b2.*dth2.*l1.*l3.*m3.*t2.*t4.*t5.*t8.*4.0e2+dth1.*dth3.*l1.*l3.*t2.*t6.*t10.*t11.*t32.*1.2e3+dth1.*dth3.*l1.*l3.*t2.*t6.*t10.*t12.*t32.*4.0e2+dth1.*dth3.*l1.*l3.*t2.*t6.*t11.*t12.*t32.*4.0e2+dth1.*dth2.*t3.*t4.*t6.*t10.*t11.*t14.*t32.*1.6e3+dth1.*dth3.*t2.*t5.*t6.*t10.*t11.*t14.*t32.*4.0e2+dth1.*dth3.*t2.*t6.*t8.*t10.*t11.*t14.*t32.*8.0e2+dth2.*dth3.*t2.*t3.*t4.*t5.*t8.*t32.*t34.*4.0e2-dth1.*dth2.*t3.*t4.*t5.*t8.*t10.*t32.*t34.*4.0e2-l1.*l3.*t5.*t6.*t8.*t9.*t10.*t12.*t32.*4.0e2+l1.*l3.*t5.*t6.*t8.*t10.*t11.*t13.*t32.*4.0e2+l1.*l3.*t5.*t6.*t8.*t9.*t12.*t32.*t33.*4.0e2-l1.*t2.*t3.*t5.*t6.*t7.*t8.*t10.*t32.*3.924e3+dth2.*dth3.*l1.*l3.*t2.*t3.*t4.*t5.*t6.*t32.*4.0e2-dth2.*dth3.*l1.*l3.*t2.*t3.*t4.*t6.*t8.*t32.*4.0e2+dth1.*dth2.*l1.*l3.*t3.*t4.*t5.*t6.*t10.*t32.*8.0e2-dth1.*dth2.*l1.*l3.*t3.*t4.*t6.*t8.*t10.*t32.*8.0e2-dth1.*dth3.*l1.*l3.*t2.*t6.*t10.*t11.*t12.*t32.*4.0e2-dth1.*dth2.*m1.*m3.*t3.*t4.*t5.*t6.*t8.*t14.*1.0e2+dth1.*dth2.*t3.*t4.*t5.*t6.*t8.*t10.*t14.*t32.*1.2e3-dth2.*dth3.*l1.*l3.*t2.*t3.*t4.*t5.*t6.*t11.*t32.*4.0e2+dth1.*dth2.*l1.*l3.*t3.*t4.*t5.*t6.*t10.*t11.*t32.*8.0e2-dth1.*dth2.*l1.*l3.*t3.*t4.*t6.*t8.*t10.*t12.*t32.*8.0e2).*(1.0./1.0e2)];