clc;
clear;
clearvars;
syms X(t) Y(t) alpha(t) beta(t) theta(t);
syms phi(t) [2 2];
syms chi m_0 g M_z s_0 gamma_0 J_t_0 J_p_0 real;
syms c l [1 3] real;
syms h r J_t J_p s gamma m M [1 2] real;
syms R_Et R_E [3 2];
syms R_Bt R_B [3 4];
syms J_E [3 3 2]
syms d [1 5] real;
syms R_G [3 1];
J_G = diag([J_t_0, J_t_0, J_p_0]);
J_E(:,:,1) = diag([J_t1, J_t1, J_p1]);
J_E(:,:,2) = diag([J_t2, J_t2, J_p2]);
R_C = [X;Y;0];
R_Gt = [s_0*cos(gamma_0);s_0*sin(gamma_0);0];
R_Bt(:,1) = [r1*cos(phi1_1);r1*sin(phi1_1);0];
R_Bt(:,2) = [r1*cos(phi2_1);r1*sin(phi2_1);0];
R_Bt(:,3) = [r2*cos(phi1_2);r2*sin(phi1_2);0];
R_Bt(:,4) = [r2*cos(phi2_2);r2*sin(phi2_2);0];
R_Et(:,1) = [s1*cos(gamma1);s1*sin(gamma1);h1];
R_Et(:,2) = [s2*cos(gamma2);s2*sin(gamma2);h2];
P_alpha = [[1 0 0];
[0 cos(alpha) sin(alpha)];
[0 -sin(alpha) cos(alpha)]];
P_beta = [[cos(beta) 0 -sin(beta)];
[0 1 0];
[sin(beta) 0 cos(beta)]];
P_theta = [[cos(theta) sin(theta) 0];
[-sin(theta) cos(theta) 0];
[0 0 1]];
A = P_theta*P_beta*P_alpha;
for_assume = [alpha, beta];
for_assume = transpose(symToVector(for_assume, 2));
A = assumeSmallTrig(A, for_assume);
A = assumeSmall(A,for_assume, 0);
P_chi = [[cos(chi) 0 -sin(chi)];
[0 1 0];
[sin(chi) 0 cos(chi)]];
Omega = P_chi*(P_theta*P_beta*[diff(alpha);0;0] + ...
P_theta*[0;diff(beta);0] + [0;0;diff(theta)]);
for_assume = [alpha, beta, chi];
for_assume = transpose(symToVector(for_assume, 3));
Omega = assumeSmallTrig(Omega, for_assume);
R_G = R_C + transpose(A)*R_Gt;
R_G = simplify(R_G, "Steps",30);
for j = 1:2
R_E(:,j) = R_C + transpose(A)*R_Et(:,j);
R_E(:,j) = simplify(R_E(:,j), "Steps",30);
end
for j = 1:2
R_B(:,j) = R_E(:,1) + transpose(A)*R_Bt(:,j);
R_B(:,j+2) = R_E(:,2) + transpose(A)*R_Bt(:,j+2);
R_B(:,j) = simplify(R_B(:,j), "Steps",30);
R_B(:,j+2) = simplify(R_B(:,j+2), "Steps",30);
end
v_r = symToVector(diff(R_G, 't'),3);
syms T V D;
T = 0; V =0; D = 0;
temp = [phi1_1(t), phi2_1(t), phi1_2(t), phi2_2(t)];
for j = 1:2
v_o = diff(R_E(:,j), 't');
v_b_1 = diff(R_B(:,j), 't');
v_b_2 = diff(R_B(:,j+2), 't');
T = T + M(j)*(v_o(1)^2+v_o(2)^2) + transpose(Omega)*(J_E(:,:,j)*Omega) + ...
m(1)*(v_b_1(1)^2+v_b_1(2)^2) + m(2)*(v_b_2(1)^2+v_b_2(2)^2);
V = V + M(j)*g*R_E(1,j) + m(1)*g*R_B(1,j) + m(2)*g*R_B(1,j+2);
D = D + d4/2*diff(temp(j))^2 + d5/2*diff(temp(j+2))^2;
end
T = T/2 + 1/2*(m_0*(v_r(1)^2+v_r(2)^2)+transpose(Omega)*(J_G*Omega));
R_G = symToVector(R_G,3);
V = V + c1/2*((X-l1*beta)^2+(Y+l1*alpha)^2)+...
c2/2*((X+l2*beta)^2+(Y-l2*alpha)^2) + m_0*g*R_G(1);
D = D + d1/2*((diff(X)-l1*diff(beta))^2+(diff(Y)+l1*diff(alpha))^2)+...
d2/2*((diff(X)+l2*diff(beta))^2+(diff(Y)-l2*diff(alpha))^2)+...
d3/2*(diff(theta)^2);
L = T - V;
Lagr = sym(zeros(9,1));
Lagr_r = [0; 0; M_z*beta; 0; M_z; 0; 0; 0; 0];
Lagr_r = symToVector(Lagr_r, 9);
temp_1 = [X(t), Y(t), alpha(t), beta(t), theta(t), ...
phi1_1(t), phi2_1(t), phi1_2(t), phi2_2(t)];
for_assume = [alpha, beta, chi];
for_assume = transpose(symToVector(for_assume, 3));
for i = 1:9
Lagr(i) = diff(diff(L,diff(temp_1(i),t)),t) - diff(L,temp_1(i)) + diff(D,diff(temp_1(i),t));
Lagr(i) = assumeSmall(Lagr(i),for_assume,2);
end
Lagr = simplify(Lagr);
Lagr = Lagr - Lagr_r;
s_0 = 0.01; gamma_0 = 0.01; chi = 0.02; s1 = 0.001;
s2 = 0.001; gamma1 = 1; gamma2 = 1+pi/2; m_0 = 10; l0 = 1;
r1 = l0/10; r2 = l0/10;
l1 = 0.5; l2 = l0-l1; J_p_0 = m_0*(r2)^2/8;
J_t_0 = J_p/2 + m_0*l0^2/12; m1=0.375;m2 = 0.375;
d1 = 100; d2 = 100; d3 = 2; d4 = 10; d5= 10;
h1 = 0.45; h2 = -0.45; c1 = 2*10^5; c2 = 2*10^5;
M_z = 2000; omega = 600; g = 9.81;
J_p1 = m1*(r1)^2/8; J_t1 = J_p1/2; M1 = 0.5;
J_p2 = m2*(r2)^2/8; J_t2 = J_p2/2; M2 = 0.5;
lagr_subs = subs(Lagr);
