-
Notifications
You must be signed in to change notification settings - Fork 0
/
three_dof_arm_dyn2.m
22 lines (18 loc) · 12.8 KB
/
three_dof_arm_dyn2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
function dy = three_dof_arm_dyn2(y,u)
y1 = y(1);
y2 = y(2);
y3 = y(3);
y4 = y(4);
y5 = y(5);
y6 = y(6);
u1 = u(1);
u2 = u(2);
u3 = u(3);
three_dof_pen_parameters;
dy1 = y2;
dy2 = -(2*(32*I2*I3*k1*y2 - 8*I3*L2^2*M2*u1 - 8*I2*L3^2*M3*u1 - 32*I3*L2^2*M3*u1 - 32*I2*I3*u1 - 4*L2^2*L3^2*M3^2*u1 + 4*L2^2*L3^2*M3^2*k1*y2 + 4*L2^2*L3^2*M3^2*u1*cos(2*y3 - 2*y5) + 8*I3*L2^2*M2*k1*y2 + 8*I2*L3^2*M3*k1*y2 + 32*I3*L2^2*M3*k1*y2 - 2*L2^2*L3^2*M2*M3*u1 + 4*I3*L1^2*L2^2*M2^2*y2^2*sin(2*y1 - 2*y3) + 16*I3*L1^2*L2^2*M3^2*y2^2*sin(2*y1 - 2*y3) + 4*I2*L1^2*L3^2*M3^2*y2^2*sin(2*y1 - 2*y5) + 16*I3*L1*L2*M2*u2*cos(y1 - y3) + 32*I3*L1*L2*M3*u2*cos(y1 - y3) + 16*I2*L1*L3*M3*u3*cos(y1 - y5) + 2*L2^2*L3^2*M2*M3*k1*y2 + 4*I3*L1*L2^3*M2^2*y4^2*sin(y1 - y3) + 32*I3*L1*L2^3*M3^2*y4^2*sin(y1 - y3) + 4*I2*L1*L3^3*M3^2*y6^2*sin(y1 - y5) - 4*L1*L2*L3^2*M3^2*u2*cos(y1 + y3 - 2*y5) - 8*L1*L2^2*L3*M3^2*u3*cos(y1 - 2*y3 + y5) + 4*I3*L1*L2^2*M2^2*g*sin(y1) + 4*I2*L1*L3^2*M3^2*g*sin(y1) + 16*I3*L1*L2^2*M3^2*g*sin(y1) - 4*L2^2*L3^2*M3^2*k1*y2*cos(2*y3 - 2*y5) + 4*I3*L1*L2^2*M2^2*g*sin(y1 - 2*y3) + 16*I3*L1*L2^2*M3^2*g*sin(y1 - 2*y3) + 4*I2*L1*L3^2*M3^2*g*sin(y1 - 2*y5) + 4*L1*L2*L3^2*M3^2*u2*cos(y1 - y3) + 8*L1*L2^2*L3*M3^2*u3*cos(y1 - y5) + 16*I2*I3*L1*M1*g*sin(y1) + 32*I2*I3*L1*M2*g*sin(y1) + 32*I2*I3*L1*M3*g*sin(y1) + 4*I2*L1*L2*L3^2*M3^2*y4^2*sin(y1 + y3 - 2*y5) + 8*I3*L1*L2^2*L3*M3^2*y6^2*sin(y1 - 2*y3 + y5) + 24*I3*L1*L2^3*M2*M3*y4^2*sin(y1 - y3) - 4*L1*L2^2*L3*M2*M3*u3*cos(y1 - 2*y3 + y5) - 4*L1*L2*L3^2*M3^2*k2*y4*cos(y1 - y3) - 8*L1*L2^2*L3*M3^2*k3*y6*cos(y1 - y5) + 2*L1*L2^2*L3^2*M1*M3^2*g*sin(y1) + 3*L1*L2^2*L3^2*M2*M3^2*g*sin(y1) + L1*L2^2*L3^2*M2^2*M3*g*sin(y1) - L1*L2^2*L3^2*M1*M3^2*g*sin(y1 - 2*y3 + 2*y5) - L1*L2^2*L3^2*M1*M3^2*g*sin(y1 + 2*y3 - 2*y5) - L1*L2^2*L3^2*M2*M3^2*g*sin(y1 - 2*y3 + 2*y5) - L1*L2^2*L3^2*M2*M3^2*g*sin(y1 + 2*y3 - 2*y5) - L1*L2^3*L3^2*M2*M3^2*y4^2*sin(y1 + y3 - 2*y5) + L1*L2^2*L3^3*M2*M3^2*y6^2*sin(y1 - 2*y3 + y5) - 16*I3*L1*L2*M2*k2*y4*cos(y1 - y3) - 32*I3*L1*L2*M3*k2*y4*cos(y1 - y3) - 16*I2*L1*L3*M3*k3*y6*cos(y1 - y5) + 2*L1*L2^2*L3^2*M2*M3^2*g*sin(y1 - 2*y3) + L1*L2^2*L3^2*M2^2*M3*g*sin(y1 - 2*y3) - L1*L2^2*L3^2*M2*M3^2*g*sin(y1 - 2*y5) + 4*I3*L1*L2^2*M1*M2*g*sin(y1) + 4*I2*L1*L3^2*M1*M3*g*sin(y1) + 16*I3*L1*L2^2*M1*M3*g*sin(y1) + 8*I2*L1*L3^2*M2*M3*g*sin(y1) + 24*I3*L1*L2^2*M2*M3*g*sin(y1) + 4*I2*L1*L2*L3^2*M3^2*y4^2*sin(y1 - y3) + 8*I3*L1*L2^2*L3*M3^2*y6^2*sin(y1 - y5) + 2*L1^2*L2^2*L3^2*M2*M3^2*y2^2*sin(2*y1 - 2*y3) + L1^2*L2^2*L3^2*M2^2*M3*y2^2*sin(2*y1 - 2*y3) - L1^2*L2^2*L3^2*M2*M3^2*y2^2*sin(2*y1 - 2*y5) + 4*L1*L2*L3^2*M3^2*k2*y4*cos(y1 + y3 - 2*y5) + 8*L1*L2^2*L3*M3^2*k3*y6*cos(y1 - 2*y3 + y5) + 16*I3*L1*L2^2*M2*M3*g*sin(y1 - 2*y3) + 4*L1*L2*L3^2*M2*M3*u2*cos(y1 - y3) + 16*I2*I3*L1*L2*M2*y4^2*sin(y1 - y3) + 32*I2*I3*L1*L2*M3*y4^2*sin(y1 - y3) + 16*I2*I3*L1*L3*M3*y6^2*sin(y1 - y5) + 16*I3*L1^2*L2^2*M2*M3*y2^2*sin(2*y1 - 2*y3) + 3*L1*L2^3*L3^2*M2*M3^2*y4^2*sin(y1 - y3) + L1*L2^3*L3^2*M2^2*M3*y4^2*sin(y1 - y3) - 4*L1*L2*L3^2*M2*M3*k2*y4*cos(y1 - y3) + L1*L2^2*L3^2*M1*M2*M3*g*sin(y1) + 4*I2*L1*L2*L3^2*M2*M3*y4^2*sin(y1 - y3) + 4*L1*L2^2*L3*M2*M3*k3*y6*cos(y1 - 2*y3 + y5) + 4*I3*L1*L2^2*L3*M2*M3*y6^2*sin(y1 - 2*y3 + y5)))/(64*I1*I2*I3 + 8*I3*L1^2*L2^2*M2^2 + 8*I1*L2^2*L3^2*M3^2 + 8*I2*L1^2*L3^2*M3^2 + 32*I3*L1^2*L2^2*M3^2 + 16*I2*I3*L1^2*M1 + 16*I1*I3*L2^2*M2 + 64*I2*I3*L1^2*M2 + 16*I1*I2*L3^2*M3 + 64*I1*I3*L2^2*M3 + 64*I2*I3*L1^2*M3 - 8*I3*L1^2*L2^2*M2^2*cos(2*y1 - 2*y3) - 32*I3*L1^2*L2^2*M3^2*cos(2*y1 - 2*y3) - 8*I2*L1^2*L3^2*M3^2*cos(2*y1 - 2*y5) - 8*I1*L2^2*L3^2*M3^2*cos(2*y3 - 2*y5) + 2*L1^2*L2^2*L3^2*M1*M3^2 + 6*L1^2*L2^2*L3^2*M2*M3^2 + 2*L1^2*L2^2*L3^2*M2^2*M3 + 4*I3*L1^2*L2^2*M1*M2 + 4*I2*L1^2*L3^2*M1*M3 + 16*I3*L1^2*L2^2*M1*M3 + 4*I1*L2^2*L3^2*M2*M3 + 16*I2*L1^2*L3^2*M2*M3 + 48*I3*L1^2*L2^2*M2*M3 - 32*I3*L1^2*L2^2*M2*M3*cos(2*y1 - 2*y3) + L1^2*L2^2*L3^2*M1*M2*M3 - 4*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y1 - 2*y3) - 2*L1^2*L2^2*L3^2*M2^2*M3*cos(2*y1 - 2*y3) + 2*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y1 - 2*y5) - 2*L1^2*L2^2*L3^2*M1*M3^2*cos(2*y3 - 2*y5) - 4*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y3 - 2*y5));
dy3 = y4;
dy4 = -(2*(32*I1*I3*k2*y4 - 8*I3*L1^2*M1*u2 - 32*I3*L1^2*M2*u2 - 8*I1*L3^2*M3*u2 - 32*I3*L1^2*M3*u2 - 32*I1*I3*u2 - 4*L1^2*L3^2*M3^2*u2 + 4*L1^2*L3^2*M3^2*k2*y4 + 4*L1^2*L3^2*M3^2*u2*cos(2*y1 - 2*y5) + 8*I3*L1^2*M1*k2*y4 + 32*I3*L1^2*M2*k2*y4 + 8*I1*L3^2*M3*k2*y4 + 32*I3*L1^2*M3*k2*y4 - 2*L1^2*L3^2*M1*M3*u2 - 8*L1^2*L3^2*M2*M3*u2 - 4*I3*L1^2*L2^2*M2^2*y4^2*sin(2*y1 - 2*y3) - 16*I3*L1^2*L2^2*M3^2*y4^2*sin(2*y1 - 2*y3) + 4*I1*L2^2*L3^2*M3^2*y4^2*sin(2*y3 - 2*y5) + 16*I3*L1*L2*M2*u1*cos(y1 - y3) + 32*I3*L1*L2*M3*u1*cos(y1 - y3) + 16*I1*L2*L3*M3*u3*cos(y3 - y5) + 2*L1^2*L3^2*M1*M3*k2*y4 + 8*L1^2*L3^2*M2*M3*k2*y4 - 8*I3*L1^2*L2*M2^2*g*sin(2*y1 - y3) - 16*I3*L1^2*L2*M3^2*g*sin(2*y1 - y3) - 16*I3*L1^3*L2*M2^2*y2^2*sin(y1 - y3) - 32*I3*L1^3*L2*M3^2*y2^2*sin(y1 - y3) + 4*I1*L2*L3^3*M3^2*y6^2*sin(y3 - y5) - 4*L1*L2*L3^2*M3^2*u1*cos(y1 + y3 - 2*y5) - 8*L1^2*L2*L3*M3^2*u3*cos(y3 - 2*y1 + y5) + 8*I3*L1^2*L2*M2^2*g*sin(y3) + 4*I1*L2*L3^2*M3^2*g*sin(y3) + 16*I3*L1^2*L2*M3^2*g*sin(y3) - 4*L1^2*L3^2*M3^2*k2*y4*cos(2*y1 - 2*y5) + 4*I1*L2*L3^2*M3^2*g*sin(y3 - 2*y5) + 4*L1*L2*L3^2*M3^2*u1*cos(y1 - y3) + 8*L1^2*L2*L3*M3^2*u3*cos(y3 - y5) + 16*I1*I3*L2*M2*g*sin(y3) + 32*I1*I3*L2*M3*g*sin(y3) + 4*I1*L1*L2*L3^2*M3^2*y2^2*sin(y1 + y3 - 2*y5) + 8*I3*L1^2*L2*L3*M3^2*y6^2*sin(y3 - 2*y1 + y5) - 4*I3*L1^2*L2*M1*M2*g*sin(2*y1 - y3) - 8*I3*L1^2*L2*M1*M3*g*sin(2*y1 - y3) - 24*I3*L1^2*L2*M2*M3*g*sin(2*y1 - y3) - 4*I3*L1^3*L2*M1*M2*y2^2*sin(y1 - y3) - 8*I3*L1^3*L2*M1*M3*y2^2*sin(y1 - y3) - 48*I3*L1^3*L2*M2*M3*y2^2*sin(y1 - y3) - 4*L1^2*L2*L3*M2*M3*u3*cos(y3 - 2*y1 + y5) - 4*L1*L2*L3^2*M3^2*k1*y2*cos(y1 - y3) - 8*L1^2*L2*L3*M3^2*k3*y6*cos(y3 - y5) + 3*L1^2*L2*L3^2*M2*M3^2*g*sin(y3) + 2*L1^2*L2*L3^2*M2^2*M3*g*sin(y3) + L1^2*L2*L3^2*M1*M3^2*g*sin(2*y1 + y3 - 2*y5) + L1^2*L2*L3^2*M2*M3^2*g*sin(2*y1 + y3 - 2*y5) + L1^3*L2*L3^2*M1*M3^2*y2^2*sin(y1 + y3 - 2*y5) + 2*L1^3*L2*L3^2*M2*M3^2*y2^2*sin(y1 + y3 - 2*y5) + L1^2*L2*L3^3*M2*M3^2*y6^2*sin(y3 - 2*y1 + y5) - 16*I3*L1*L2*M2*k1*y2*cos(y1 - y3) - 32*I3*L1*L2*M3*k1*y2*cos(y1 - y3) - 16*I1*L2*L3*M3*k3*y6*cos(y3 - y5) + L1^2*L2*L3^2*M2*M3^2*g*sin(y3 - 2*y5) + 4*I1*L2*L3^2*M2*M3*g*sin(y3) + 24*I3*L1^2*L2*M2*M3*g*sin(y3) - 4*I1*L1*L2*L3^2*M3^2*y2^2*sin(y1 - y3) + 8*I3*L1^2*L2*L3*M3^2*y6^2*sin(y3 - y5) - 2*L1^2*L2^2*L3^2*M2*M3^2*y4^2*sin(2*y1 - 2*y3) - L1^2*L2^2*L3^2*M2^2*M3*y4^2*sin(2*y1 - 2*y3) + L1^2*L2^2*L3^2*M1*M3^2*y4^2*sin(2*y3 - 2*y5) + 2*L1^2*L2^2*L3^2*M2*M3^2*y4^2*sin(2*y3 - 2*y5) + 4*L1*L2*L3^2*M3^2*k1*y2*cos(y1 + y3 - 2*y5) + 8*L1^2*L2*L3*M3^2*k3*y6*cos(y3 - 2*y1 + y5) + 4*L1*L2*L3^2*M2*M3*u1*cos(y1 - y3) + 4*L1^2*L2*L3*M1*M3*u3*cos(y3 - y5) + 12*L1^2*L2*L3*M2*M3*u3*cos(y3 - y5) - 16*I1*I3*L1*L2*M2*y2^2*sin(y1 - y3) - 32*I1*I3*L1*L2*M3*y2^2*sin(y1 - y3) + 16*I1*I3*L2*L3*M3*y6^2*sin(y3 - y5) - L1^2*L2*L3^2*M1*M3^2*g*sin(2*y1 - y3) - 3*L1^2*L2*L3^2*M2*M3^2*g*sin(2*y1 - y3) - 2*L1^2*L2*L3^2*M2^2*M3*g*sin(2*y1 - y3) - 16*I3*L1^2*L2^2*M2*M3*y4^2*sin(2*y1 - 2*y3) - L1^3*L2*L3^2*M1*M3^2*y2^2*sin(y1 - y3) - 6*L1^3*L2*L3^2*M2*M3^2*y2^2*sin(y1 - y3) - 4*L1^3*L2*L3^2*M2^2*M3*y2^2*sin(y1 - y3) + L1^2*L2*L3^3*M1*M3^2*y6^2*sin(y3 - y5) + 3*L1^2*L2*L3^3*M2*M3^2*y6^2*sin(y3 - y5) - 4*L1*L2*L3^2*M2*M3*k1*y2*cos(y1 - y3) - 4*L1^2*L2*L3*M1*M3*k3*y6*cos(y3 - y5) - 12*L1^2*L2*L3*M2*M3*k3*y6*cos(y3 - y5) - 4*I1*L1*L2*L3^2*M2*M3*y2^2*sin(y1 - y3) + 4*I3*L1^2*L2*L3*M1*M3*y6^2*sin(y3 - y5) + 12*I3*L1^2*L2*L3*M2*M3*y6^2*sin(y3 - y5) + 4*L1^2*L2*L3*M2*M3*k3*y6*cos(y3 - 2*y1 + y5) - L1^2*L2*L3^2*M1*M2*M3*g*sin(2*y1 - y3) - L1^3*L2*L3^2*M1*M2*M3*y2^2*sin(y1 - y3) + 4*I3*L1^2*L2*L3*M2*M3*y6^2*sin(y3 - 2*y1 + y5)))/(64*I1*I2*I3 + 8*I3*L1^2*L2^2*M2^2 + 8*I1*L2^2*L3^2*M3^2 + 8*I2*L1^2*L3^2*M3^2 + 32*I3*L1^2*L2^2*M3^2 + 16*I2*I3*L1^2*M1 + 16*I1*I3*L2^2*M2 + 64*I2*I3*L1^2*M2 + 16*I1*I2*L3^2*M3 + 64*I1*I3*L2^2*M3 + 64*I2*I3*L1^2*M3 - 8*I3*L1^2*L2^2*M2^2*cos(2*y1 - 2*y3) - 32*I3*L1^2*L2^2*M3^2*cos(2*y1 - 2*y3) - 8*I2*L1^2*L3^2*M3^2*cos(2*y1 - 2*y5) - 8*I1*L2^2*L3^2*M3^2*cos(2*y3 - 2*y5) + 2*L1^2*L2^2*L3^2*M1*M3^2 + 6*L1^2*L2^2*L3^2*M2*M3^2 + 2*L1^2*L2^2*L3^2*M2^2*M3 + 4*I3*L1^2*L2^2*M1*M2 + 4*I2*L1^2*L3^2*M1*M3 + 16*I3*L1^2*L2^2*M1*M3 + 4*I1*L2^2*L3^2*M2*M3 + 16*I2*L1^2*L3^2*M2*M3 + 48*I3*L1^2*L2^2*M2*M3 - 32*I3*L1^2*L2^2*M2*M3*cos(2*y1 - 2*y3) + L1^2*L2^2*L3^2*M1*M2*M3 - 4*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y1 - 2*y3) - 2*L1^2*L2^2*L3^2*M2^2*M3*cos(2*y1 - 2*y3) + 2*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y1 - 2*y5) - 2*L1^2*L2^2*L3^2*M1*M3^2*cos(2*y3 - 2*y5) - 4*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y3 - 2*y5));
dy5 = y6;
dy6 = (2*(32*I1*I2*u3 + 8*I2*L1^2*M1*u3 + 8*I1*L2^2*M2*u3 + 32*I2*L1^2*M2*u3 + 32*I1*L2^2*M3*u3 + 32*I2*L1^2*M3*u3 - 32*I1*I2*k3*y6 + 4*L1^2*L2^2*M2^2*u3 + 16*L1^2*L2^2*M3^2*u3 - 4*L1^2*L2^2*M2^2*k3*y6 - 16*L1^2*L2^2*M3^2*k3*y6 - 4*L1^2*L2^2*M2^2*u3*cos(2*y1 - 2*y3) - 16*L1^2*L2^2*M3^2*u3*cos(2*y1 - 2*y3) - 8*I2*L1^2*M1*k3*y6 - 8*I1*L2^2*M2*k3*y6 - 32*I2*L1^2*M2*k3*y6 - 32*I1*L2^2*M3*k3*y6 - 32*I2*L1^2*M3*k3*y6 + 2*L1^2*L2^2*M1*M2*u3 + 8*L1^2*L2^2*M1*M3*u3 + 24*L1^2*L2^2*M2*M3*u3 + 4*I2*L1^2*L3^2*M3^2*y6^2*sin(2*y1 - 2*y5) + 4*I1*L2^2*L3^2*M3^2*y6^2*sin(2*y3 - 2*y5) - 16*I2*L1*L3*M3*u1*cos(y1 - y5) - 16*I1*L2*L3*M3*u2*cos(y3 - y5) - 2*L1^2*L2^2*M1*M2*k3*y6 - 8*L1^2*L2^2*M1*M3*k3*y6 - 24*L1^2*L2^2*M2*M3*k3*y6 + 8*I2*L1^2*L3*M3^2*g*sin(2*y1 - y5) + 8*I1*L2^2*L3*M3^2*g*sin(2*y3 - y5) - 16*L1^2*L2^2*M2*M3*u3*cos(2*y1 - 2*y3) + 16*I2*L1^3*L3*M3^2*y2^2*sin(y1 - y5) + 16*I1*L2^3*L3*M3^2*y4^2*sin(y3 - y5) + 8*L1*L2^2*L3*M3^2*u1*cos(y1 - 2*y3 + y5) + 8*L1^2*L2*L3*M3^2*u2*cos(y3 - 2*y1 + y5) - 8*I1*L2^2*L3*M3^2*g*sin(y5) - 8*I2*L1^2*L3*M3^2*g*sin(y5) + 4*L1^2*L2^2*M2^2*k3*y6*cos(2*y1 - 2*y3) + 16*L1^2*L2^2*M3^2*k3*y6*cos(2*y1 - 2*y3) - 8*L1*L2^2*L3*M3^2*u1*cos(y1 - y5) - 8*L1^2*L2*L3*M3^2*u2*cos(y3 - y5) - 16*I1*I2*L3*M3*g*sin(y5) - 8*I1*L1*L2^2*L3*M3^2*y2^2*sin(y1 - 2*y3 + y5) - 8*I2*L1^2*L2*L3*M3^2*y4^2*sin(y3 - 2*y1 + y5) + 4*I2*L1^2*L3*M1*M3*g*sin(2*y1 - y5) + 8*I2*L1^2*L3*M2*M3*g*sin(2*y1 - y5) + 4*I1*L2^2*L3*M2*M3*g*sin(2*y3 - y5) + 4*I2*L1^3*L3*M1*M3*y2^2*sin(y1 - y5) + 16*I2*L1^3*L3*M2*M3*y2^2*sin(y1 - y5) + 4*I1*L2^3*L3*M2*M3*y4^2*sin(y3 - y5) + 4*L1*L2^2*L3*M2*M3*u1*cos(y1 - 2*y3 + y5) + 4*L1^2*L2*L3*M2*M3*u2*cos(y3 - 2*y1 + y5) + 8*L1*L2^2*L3*M3^2*k1*y2*cos(y1 - y5) + 8*L1^2*L2*L3*M3^2*k2*y4*cos(y3 - y5) + L1^2*L2^2*L3*M2^2*M3*g*sin(y5) - 2*L1^2*L2^2*L3*M1*M3^2*g*sin(2*y1 - 2*y3 + y5) - 2*L1^2*L2^2*L3*M2*M3^2*g*sin(2*y1 - 2*y3 + y5) - L1^2*L2^2*L3*M2^2*M3*g*sin(2*y1 - 2*y3 + y5) - 2*L1^3*L2^2*L3*M1*M3^2*y2^2*sin(y1 - 2*y3 + y5) - 4*L1^3*L2^2*L3*M2*M3^2*y2^2*sin(y1 - 2*y3 + y5) - 2*L1^3*L2^2*L3*M2^2*M3*y2^2*sin(y1 - 2*y3 + y5) + 2*L1^2*L2^3*L3*M2*M3^2*y4^2*sin(y3 - 2*y1 + y5) + L1^2*L2^3*L3*M2^2*M3*y4^2*sin(y3 - 2*y1 + y5) + 16*I2*L1*L3*M3*k1*y2*cos(y1 - y5) + 16*I1*L2*L3*M3*k2*y4*cos(y3 - y5) - 8*I2*L1^2*L3*M2*M3*g*sin(y5) + 8*I1*L1*L2^2*L3*M3^2*y2^2*sin(y1 - y5) + 8*I2*L1^2*L2*L3*M3^2*y4^2*sin(y3 - y5) + 16*L1^2*L2^2*M2*M3*k3*y6*cos(2*y1 - 2*y3) - L1^2*L2^2*L3^2*M2*M3^2*y6^2*sin(2*y1 - 2*y5) + L1^2*L2^2*L3^2*M1*M3^2*y6^2*sin(2*y3 - 2*y5) + 2*L1^2*L2^2*L3^2*M2*M3^2*y6^2*sin(2*y3 - 2*y5) - 8*L1*L2^2*L3*M3^2*k1*y2*cos(y1 - 2*y3 + y5) - 8*L1^2*L2*L3*M3^2*k2*y4*cos(y3 - 2*y1 + y5) - 4*L1^2*L2*L3*M1*M3*u2*cos(y3 - y5) - 12*L1^2*L2*L3*M2*M3*u2*cos(y3 - y5) + 16*I1*I2*L1*L3*M3*y2^2*sin(y1 - y5) + 16*I1*I2*L2*L3*M3*y4^2*sin(y3 - y5) + 2*L1^2*L2^2*L3*M1*M3^2*g*sin(2*y1 - y5) - L1^2*L2^2*L3*M2^2*M3*g*sin(2*y1 - y5) + 2*L1^2*L2^2*L3*M2*M3^2*g*sin(2*y3 - y5) + L1^2*L2^2*L3*M2^2*M3*g*sin(2*y3 - y5) + 2*L1^3*L2^2*L3*M1*M3^2*y2^2*sin(y1 - y5) - 2*L1^3*L2^2*L3*M2^2*M3*y2^2*sin(y1 - y5) + 4*L1^2*L2^3*L3*M1*M3^2*y4^2*sin(y3 - y5) + 6*L1^2*L2^3*L3*M2*M3^2*y4^2*sin(y3 - y5) + L1^2*L2^3*L3*M2^2*M3*y4^2*sin(y3 - y5) + 4*L1^2*L2*L3*M1*M3*k2*y4*cos(y3 - y5) + 12*L1^2*L2*L3*M2*M3*k2*y4*cos(y3 - y5) - L1^2*L2^2*L3*M1*M2*M3*g*sin(2*y1 - 2*y3 + y5) - L1^3*L2^2*L3*M1*M2*M3*y2^2*sin(y1 - 2*y3 + y5) + 4*I2*L1^2*L2*L3*M1*M3*y4^2*sin(y3 - y5) + 12*I2*L1^2*L2*L3*M2*M3*y4^2*sin(y3 - y5) - 4*L1*L2^2*L3*M2*M3*k1*y2*cos(y1 - 2*y3 + y5) - 4*L1^2*L2*L3*M2*M3*k2*y4*cos(y3 - 2*y1 + y5) + L1^2*L2^3*L3*M1*M2*M3*y4^2*sin(y3 - y5) - 4*I1*L1*L2^2*L3*M2*M3*y2^2*sin(y1 - 2*y3 + y5) - 4*I2*L1^2*L2*L3*M2*M3*y4^2*sin(y3 - 2*y1 + y5)))/(64*I1*I2*I3 + 8*I3*L1^2*L2^2*M2^2 + 8*I1*L2^2*L3^2*M3^2 + 8*I2*L1^2*L3^2*M3^2 + 32*I3*L1^2*L2^2*M3^2 + 16*I2*I3*L1^2*M1 + 16*I1*I3*L2^2*M2 + 64*I2*I3*L1^2*M2 + 16*I1*I2*L3^2*M3 + 64*I1*I3*L2^2*M3 + 64*I2*I3*L1^2*M3 - 8*I3*L1^2*L2^2*M2^2*cos(2*y1 - 2*y3) - 32*I3*L1^2*L2^2*M3^2*cos(2*y1 - 2*y3) - 8*I2*L1^2*L3^2*M3^2*cos(2*y1 - 2*y5) - 8*I1*L2^2*L3^2*M3^2*cos(2*y3 - 2*y5) + 2*L1^2*L2^2*L3^2*M1*M3^2 + 6*L1^2*L2^2*L3^2*M2*M3^2 + 2*L1^2*L2^2*L3^2*M2^2*M3 + 4*I3*L1^2*L2^2*M1*M2 + 4*I2*L1^2*L3^2*M1*M3 + 16*I3*L1^2*L2^2*M1*M3 + 4*I1*L2^2*L3^2*M2*M3 + 16*I2*L1^2*L3^2*M2*M3 + 48*I3*L1^2*L2^2*M2*M3 - 32*I3*L1^2*L2^2*M2*M3*cos(2*y1 - 2*y3) + L1^2*L2^2*L3^2*M1*M2*M3 - 4*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y1 - 2*y3) - 2*L1^2*L2^2*L3^2*M2^2*M3*cos(2*y1 - 2*y3) + 2*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y1 - 2*y5) - 2*L1^2*L2^2*L3^2*M1*M3^2*cos(2*y3 - 2*y5) - 4*L1^2*L2^2*L3^2*M2*M3^2*cos(2*y3 - 2*y5));
dy = [dy1 dy2 dy3 dy4 dy5 dy6]';