clear all;
clc;
syms M m1 m2 g l1 l2 t F %常量
syms x(t) dx(t) ddx(t) y(t) dy(t) ddy(t) %0级摆
syms thx1(t) dthx1(t) ddthx1(t) thy1(t) dthy1(t) ddthy1(t) %一级摆
syms thx2(t) dthx2(t) ddthx2(t) thy2(t) dthy2(t) ddthy2(t) %二级摆
syms Cx1 Cx2 Sx1 Sx2 Cy1 Cy2 Sy1 Sy2
%% 列写拉格朗日函数
x_1 = x+l1*sin(thx1)*cos(thy1);
y_1 =y+l1*sin(thy1);
z_1 =-l1*cos(thx1)*cos(thy1);
V_1 = sqrt(diff(x_1,t)^2+diff(y_1,t)^2+diff(z_1,t)^2);
K_1 = 1/2*m1*V_1^2;
% P_1 =-m1*g*l*cos(theta);
x_2 = x_1+l2*sin(thx2)*cos(thy2);
y_2 =y_1+l2*sin(thy2);
z_2 =z_1-l2*cos(thx2)*cos(thy2);
V_2 = sqrt(diff(x_2,t)^2+diff(y_2,t)^2+diff(z_2,t)^2);
K_2 = 1/2*m2*V_2^2;
V_0 = sqrt(diff(x,t)^2+diff(y,t)^2);
K_0 = 1/2*M*V_0^2;
K = K_1+K_2+K_0;
P =-(m1+m2)*g*l1*cos(thx1)*cos(thy1)-m2*g*l2*cos(thx2)*cos(thy2);
L = K-P;
L = subs(L,diff(x,t),dx);%0000
L = subs(L,diff(dx,t),ddx);
L = subs(L,diff(y,t),dy);
L = subs(L,diff(dy,t),ddy);
L = subs(L,diff(thx1,t),dthx1); %1111
L = subs(L,diff(dthx1,t),ddthx1);
L = subs(L,diff(thy1,t),dthy1);
L = subs(L,diff(dthy1,t),ddthy1);
L = subs(L,diff(thx2,t),dthx2); %2222
L = subs(L,diff(dthx2,t),ddthx2);
L = subs(L,diff(thy2,t),dthy2);
L = subs(L,diff(dthy2,t),ddthy2);
L = simplify(L);
%% 列些动力学方程
L_x=functionalDerivative(L,x);%0000
L_dx=functionalDerivative(L,dx);
L_dx_t = diff(L_dx,t);
L_y=functionalDerivative(L,y);
L_dy=functionalDerivative(L,dy);
L_dy_t = diff(L_dy,t);
L_thx1=functionalDerivative(L,thx1);%1111
L_dthx1=functionalDerivative(L,dthx1);
L_dthx1_t = diff(L_dthx1,t);
L_thy1=functionalDerivative(L,thy1);
L_dthy1=functionalDerivative(L,dthy1);
L_dthy1_t = diff(L_dthy1,t);
L_thx2=functionalDerivative(L,thx2);%2222
L_dthx2=functionalDerivative(L,dthx2);
L_dthx2_t = diff(L_dthx2,t);
L_thy2=functionalDerivative(L,thy2);
L_dthy2=functionalDerivative(L,dthy2);
L_dthy2_t = diff(L_dthy2,t);
%等式项 1111
right_1 = simplify(L_dx_t - L_x);
right_1 = subs(right_1,diff(x,t),dx);
right_1 = subs(right_1,diff(dx,t),ddx);
right_1 = subs(right_1,diff(y,t),dy);
right_1 = subs(right_1,diff(dy,t),ddy);
right_1 = subs(right_1,diff(thx1,t),dthx1);
right_1 = subs(right_1,diff(dthx1,t),ddthx1);
right_1 = subs(right_1,diff(thy1,t),dthy1);
right_1 = subs(right_1,diff(dthy1,t),ddthy1);
right_1 = subs(right_1,diff(thx2,t),dthx2);
right_1 = subs(right_1,diff(dthx2,t),ddthx2);
right_1 = subs(right_1,diff(thy2,t),dthy2);
right_1 = subs(right_1,diff(dthy2,t),ddthy2);
right_1 = subs(right_1,cos(thx1),Cx1);right_1 = subs(right_1,sin(thx1),Sx1);
right_1 = subs(right_1,cos(thy1),Cy1);right_1 = subs(right_1,sin(thy1),Sy1);
right_1 = subs(right_1,cos(thx2),Cx2);right_1 = subs(right_1,sin(thx2),Sx2);
right_1 = subs(right_1,cos(thy2),Cy2);right_1 = subs(right_1,sin(thy2),Sy2);
%等式项 2222
right_2 = simplify(L_dy_t - L_y);
right_2 = subs(right_2,diff(x,t),dx);
right_2 = subs(right_2,diff(dx,t),ddx);
right_2 = subs(right_2,diff(y,t),dy);
right_2 = subs(right_2,diff(dy,t),ddy);
right_2 = subs(right_2,diff(thx1,t),dthx1);
right_2 = subs(right_2,diff(dthx1,t),ddthx1);
right_2 = subs(right_2,diff(thy1,t),dthy1);
right_2 = subs(right_2,diff(dthy1,t),ddthy1);
right_2 = subs(right_2,diff(thx2,t),dthx2);
right_2 = subs(right_2,diff(dthx2,t),ddthx2);
right_2 = subs(right_2,diff(thy2,t),dthy2);
right_2 = subs(right_2,diff(dthy2,t),ddthy2);
right_2 = subs(right_2,cos(thx1),Cx1);right_2 = subs(right_2,sin(thx1),Sx1);
right_2 = subs(right_2,cos(thy1),Cy1);right_2 = subs(right_2,sin(thy1),Sy1);
right_2 = subs(right_2,cos(thx2),Cx2);right_2 = subs(right_2,sin(thx2),Sx2);
right_2 = subs(right_2,cos(thy2),Cy2);right_2 = subs(right_2,sin(thy2),Sy2);
%等式项 3333
right_3 = simplify(L_dthx1_t - L_thx1);
right_3 = subs(right_3,diff(x,t),dx);
right_3 = subs(right_3,diff(dx,t),ddx);
right_3 = subs(right_3,diff(y,t),dy);
right_3 = subs(right_3,diff(dy,t),ddy);
right_3 = subs(right_3,diff(thx1,t),dthx1);
right_3 = subs(right_3,diff(dthx1,t),ddthx1);
right_3 = subs(right_3,diff(thy1,t),dthy1);
right_3 = subs(right_3,diff(dthy1,t),ddthy1);
right_3 = subs(right_3,diff(thx2,t),dthx2);
right_3 = subs(right_3,diff(dthx2,t),ddthx2);
right_3 = subs(right_3,diff(thy2,t),dthy2);
right_3 = subs(right_3,diff(dthy2,t),ddthy2);
right_3 = subs(right_3,cos(thx1),Cx1);right_3 = subs(right_3,sin(thx1),Sx1);
right_3 = subs(right_3,cos(thy1),Cy1);right_3 = subs(right_3,sin(thy1),Sy1);
right_3 = subs(right_3,cos(thx2),Cx2);right_3 = subs(right_3,sin(thx2),Sx2);
right_3 = subs(right_3,cos(thy2),Cy2);right_3 = subs(right_3,sin(thy2),Sy2);
%等式项 4444
right_4 = simplify(L_dthy1_t - L_thy1);
right_4 = subs(right_4,diff(x,t),dx);
right_4 = subs(right_4,diff(dx,t),ddx);
right_4 = subs(right_4,diff(y,t),dy);
right_4 = subs(right_4,diff(dy,t),ddy);
right_4 = subs(right_4,diff(thx1,t),dthx1);
right_4 = subs(right_4,diff(dthx1,t),ddthx1);
right_4 = subs(right_4,diff(thy1,t),dthy1);
right_4 = subs(right_4,diff(dthy1,t),ddthy1);
right_4 = subs(right_4,diff(thx2,t),dthx2);
right_4 = subs(right_4,diff(dthx2,t),ddthx2);
right_4 = subs(right_4,diff(thy2,t),dthy2);
right_4 = subs(right_4,diff(dthy2,t),ddthy2);
right_4 = subs(right_4,cos(thx1),Cx1);right_4 = subs(right_4,sin(thx1),Sx1);
right_4 = subs(right_4,cos(thy1),Cy1);right_4 = subs(right_4,sin(thy1),Sy1);
right_4 = subs(right_4,cos(thx2),Cx2);right_4 = subs(right_4,sin(thx2),Sx2);
right_4 = subs(right_4,cos(thy2),Cy2);right_4 = subs(right_4,sin(thy2),Sy2);
%等式项 5555
right_5 = simplify(L_dthx2_t - L_thx2);
right_5 = subs(right_5,diff(x,t),dx);
right_5 = subs(right_5,diff(dx,t),ddx);
right_5 = subs(right_5,diff(y,t),dy);
right_5 = subs(right_5,diff(dy,t),ddy);
right_5 = subs(right_5,diff(thx1,t),dthx1);
right_5 = subs(right_5,diff(dthx1,t),ddthx1);
right_5 = subs(right_5,diff(thy1,t),dthy1);
right_5 = subs(right_5,diff(dthy1,t),ddthy1);
right_5 = subs(right_5,diff(thx2,t),dthx2);
right_5 = subs(right_5,diff(dthx2,t),ddthx2);
right_5 = subs(right_5,diff(thy2,t),dthy2);
right_5 = subs(right_5,diff(dthy2,t),ddthy2);
right_5 = subs(right_5,cos(thx1),Cx1);right_5 = subs(right_5,sin(thx1),Sx1);
right_5 = subs(right_5,cos(thy1),Cy1);right_5 = subs(right_5,sin(thy1),Sy1);
right_5 = subs(right_5,cos(thx2),Cx2);right_5 = subs(right_5,sin(thx2),Sx2);
right_5 = subs(right_5,cos(thy2),Cy2);right_5 = subs(right_5,sin(thy2),Sy2);
%等式项 6666
right_6 = simplify(L_dthy2_t - L_thy2);
right_6 = subs(right_6,diff(x,t),dx);
right_6 = subs(right_6,diff(dx,t),ddx);
right_6 = subs(right_6,diff(y,t),dy);
right_6 = subs(right_6,diff(dy,t),ddy);
right_6 = subs(right_6,diff(thx1,t),dthx1);
right_6 = subs(right_6,diff(dthx1,t),ddthx1);
right_6 = subs(right_6,diff(thy1,t),dthy1);
right_6 = subs(right_6,diff(dthy1,t),ddthy1);
right_6 = subs(right_6,diff(thx2,t),dthx2);
right_6 = subs(right_6,diff(dthx2,t),ddthx2);
right_6 = subs(right_6,diff(thy2,t),dthy2);
right_6 = subs(right_6,diff(dthy2,t),ddthy2);
right_6 = subs(right_6,cos(thx1),Cx1);right_6 = subs(right_6,sin(thx1),Sx1);
right_6 = subs(right_6,cos(thy1),Cy1);right_6 = subs(right_6,sin(thy1),Sy1);
right_6 = subs(right_6,cos(thx2),Cx2);right_6 = subs(right_6,sin(thx2),Sx2);
right_6 = subs(right_6,cos(thy2),Cy2);right_6 = subs(right_6,sin(thy2),Sy2);
% right_1 = collect(right_1,[dx ddx dy ddy dthx1 ddthx1 dthy1 ddthy1 dthx2 ddthx2 dthy2 ddthy2 ]);
% right_2 = collect(right_2,[dx ddx dy ddy dthx1 ddthx1 dthy1 ddthy1 dthx2 ddthx2 dthy2 ddthy2 ]);
% right_3 = collect(right_3,[dx ddx dy ddy dthx1 ddthx1 dthy1 ddthy1 dthx2 ddthx2 dthy2 ddthy2 ]);
% right_4 = collect(right_4,[dx ddx dy ddy dthx1 ddthx1 dthy1 ddthy1 dthx2 ddthx2 dthy2 ddthy2 ]);
% right_5 = collect(right_5,[dx ddx dy ddy dthx1 ddthx1 dthy1 ddthy1 dthx2 ddthx2 dthy2 ddthy2 ]);
% right_6 = collect(right_6,[dx ddx dy ddy dthx1 ddthx1 dthy1 ddthy1 dthx2 ddthx2 dthy2 ddthy2 ]);
% right_2 = simplify(L_dth_t - L_th);
% right_2 = subs(right_2,diff(theta,t),dtheta);
% right_2 = subs(right_2,di