clc; clear; close all; try load('analytics_reduction.mat') catch syms x y th1 th2 dx dy dbeta dth1 dth2 ddx ddy ddbeta ddth1 ddth2 ... % beta L m mx my mt I k c real beta = sym('beta','real'); % All bodies are equal L0 = L; L1 = L; L2 = L; m0 = m; m1 = m; m2 = m; I0 = I; I1 = I; I2 = I; th1_0 = 0; delta_mi = 1; M_i = delta_mi*diag([mx , my , mt]); %added mass matrix M0 = diag([m0 m0 I0]); M1 = diag([m1 m1 I1]); M2 = diag([m2 m2 I2]); %mass matrix % velocity: % velocity In link center of mass coordinate system V0_w = [dx , dy , dbeta].'; V1_w = [dx+L0*sin(beta)*dbeta+L1*sin(beta+th1)*(dbeta+dth1) , dy-L0*cos(beta)*dbeta-L1*cos(beta+th1)*(dbeta+dth1) , dbeta+dth1].'; V2_w = [dx-L0*sin(beta)*dbeta-L2*sin(beta+th2)*(dbeta+dth2) , dy+L0*cos(beta)*dbeta+L2*cos(beta+th2)*(dbeta+dth2) , dbeta+dth2].'; Rotation_mat = @(angle) [cos(angle) sin(angle) 0 ; -sin(angle) cos(angle) 0 ; 0 0 1]; %Rotation matrix % velocity In link fixed coordinate system V0 = Rotation_mat(beta)*V0_w; V1 = Rotation_mat(beta+th1)*V1_w; V2 = Rotation_mat(beta+th2)*V2_w; U = 0.5*k*(th1-th1_0)^2; %Potential energy R = 0.5*c*(dth1)^2; %dissipation energy T_tot = (0.5*(V0.'*(M_i+M0)*V0 + V1.'*(M_i+M1)*V1 + V2.'*(M_i+M2)*V2)); %kinetic energy Lagrangian = T_tot - U; X = {x dx y dy beta dbeta th1 dth1 th2 dth2}; DDX = {ddx ddy ddbeta ddth1 ddth2}; [H,B] = MY_EulerLagrange_H(Lagrangian,R,X,DDX); %% ODE reduction to be dependent only by (th1,th2,t) R_rot = [cos(beta) -sin(beta) 0 0 0 sin(beta) cos(beta) 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1]; dR_rot = [-dbeta*sin(beta) -dbeta*cos(beta) 0 0 0 dbeta*cos(beta) -dbeta*sin(beta) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]; %% body system is removing the dependence on beta V_b = [V0;dth1;dth2]; H_0 = simplify(R_rot'*H*R_rot); B_0 = simplify(R_rot'*(H*dR_rot*V_b+B)); %% switching from dx dy to vx vy - bidy system velocity syms vx vy real ss = solve([vx==V0(1) , vy==V0(2)],[dx,dy]); B_0 = simplify(subs(B_0,[dx,dy],[ss.dx,ss.dy])); Bb = B_0(1:3); Bs = B_0(4:5); Hbb = H_0(1:3,1:3); Hbs = H_0(1:3,4:5); Hss = H_0(4:5,4:5); %% Zero momentum conservation eq is removing the dependence on vx vy dbeta invHbb = inv(Hbb); Vb_ZMC = simplify(-invHbb*Hbs*[dth1 dth2]'); %Zero momentum conservation Bb = subs(Bb,[vx,vy,dbeta],Vb_ZMC'); dVb_temp = Hbs*[ddth1;ddth2] + Bb; dVb = -invHbb*dVb_temp; % dVb = (-inv(Hbb)*(Hbs*[ddth1;ddth2]+subs(Bb,[vx,vy,dbeta],ZMC'))) Bs = subs(Bs,[vx,vy,dbeta],Vb_ZMC'); eq4 = Hbs'*dVb+Hss*[ddth1;ddth2]+Bs; [cxy, txy] = coeffs(eq4(1),[ddth1 ddth2]); H_t = cxy(1:2); B_t = cxy(3); [cxy, txy] = coeffs(eq4(2),[ddth1 ddth2]); H_t = [H_t;cxy(1:2)]; B_t = [B_t ; cxy(3)]; %% save('analytics_reduction.mat','H_t','B_t','th1','th2','dth1','dth2','ddth1','ddth2','k','c')%,'m11','m12') end %% Perturbation and approximate solution %%% if ~isfile('f_ddth1_ana.m')||~isfile('f_dth1_ana.m')||~isfile('f_th1_ana.m')||~isfile('f_a_th1_ana.m')||~isfile('f_b_th1_ana.m') M_11 = simplify(subs(H_t(1,1),[th1,th2,dth1,dth2,ddth1,ddth2],[0,0,0,0,0,0])); M_12 = simplify(subs(H_t(1,2),[th1,th2,dth1,dth2,ddth1,ddth2],[0,0,0,0,0,0])); matlabFunction(M_11,'File','M_11_in0'); matlabFunction(M_12,'File','M_12_in0'); %%% Steady periodic solution of ODE syms A B m11 m12 w t epsilon real odesol = A*sin(w*t)+B*cos(w*t); fs = odesol; dfs = diff(odesol,t); ddfs = diff(odesol,t,2); ode = m11*ddfs+c*dfs+k*fs-m12*w^2*cos(w*t); %%% ode steady periodic solution [csc, tsc] = coeffs(ode,[sin(w*t) cos(w*t)]); eq4 = csc; [solA, solB] = solve(eq4,[A,B]); f1_th1 = subs(odesol,[A,B],[solA, solB]); %% th1 = ep*f1_th1 df1_th1 = simplify(diff(f1_th1,t)); %% dth1 = ep*df1_th1 ddf1_th1 = simplify(diff(f1_th1,t,2)); th1_ana = epsilon*subs(f1_th1,[m11,m12],[M_11,M_12]); dth1_ana = epsilon*subs(df1_th1,[m11,m12],[M_11,M_12]); ddth1_ana = epsilon*subs(ddf1_th1,[m11,m12],[M_11,M_12]); matlabFunction(th1_ana,'File','f_th1_ana'); matlabFunction(dth1_ana,'File','f_dth1_ana'); matlabFunction(ddth1_ana,'File','f_ddth1_ana'); %%% epsilon*(a_th*sin(w*t)+b_th*cos(w*t) [csc, tsc] = coeffs(f1_th1,[sin(w*t) cos(w*t)]); a_th1_ana = subs(csc(1),[m11,m12],[M_11,M_12]); b_th1_ana = subs(csc(2),[m11,m12],[M_11,M_12]); matlabFunction(a_th1_ana,'File','f_a_th1_ana'); matlabFunction(b_th1_ana,'File','f_b_th1_ana'); end %% Variationla equation ode = H_t(1,1)*ddth1 + H_t(1,2)*ddth2 + B_t(1); [csc, tsc] = coeffs(ode,[ddth1 dth1 ddth2 dth2]); coeff_ddth1 = csc(1); coeff_dth1dth1 = csc(2); coeff_dth1dth2 = csc(3); coeff_dth1 = csc(4); coeff_ddth2 = csc(5); coeff_dth2dth2 = csc(6); coeff_th1 = csc(7); m11_tilde = coeff_ddth1; m12_tilde = coeff_ddth2; b11_tilde = coeff_dth1dth1; b12_tilde = coeff_dth1dth2; b22_tilde = coeff_dth2dth2; syms th1_bar dth1_bar ddth1_bar del ddel dddel real b2_second_order = simplify(subs(m11_tilde,[th1,th2],[0,0]) + subs(diff(m11_tilde,th1),[th1,th2],[0,0])*th1_bar + subs(diff(m11_tilde,th2),[th1,th2],[0,0])*th2 + 0.5*subs(diff(m11_tilde,th1,2),[th1,th2],[0,0])*th1_bar^2 + 0.5*subs(diff(m11_tilde,th2,2),[th1,th2],[0,0])*th2^2 +subs(diff(diff(m11_tilde,th1),th2),[th1,th2],[0,0])*th1_bar*th2); b1_second_order = simplify(2*((subs(b11_tilde,[th1,th2],[0,0])) + subs(diff(b11_tilde,th1),[th1,th2],[0,0])*th1_bar + subs(diff(b11_tilde,th2),[th1,th2],[0,0])*th2)*dth1_bar + (subs(b12_tilde,[th1,th2],[0,0]) + subs(diff(b12_tilde,th1),[th1,th2],[0,0])*th1_bar + subs(diff(b12_tilde,th2),[th1,th2],[0,0])*th2)*dth2 + c); b0_second_order = simplify((subs(diff(m11_tilde,th1),[th1,th2],[0,0]) + subs(diff(m11_tilde,th1,2),[th1,th2],[0,0])*th1_bar + subs(diff(diff(m11_tilde,th1),th2),[th1,th2],[0,0])*th2)*ddth1_bar + (subs(diff(m12_tilde,th1),[th1,th2],[0,0]) + subs(diff(m12_tilde,th1,2),[th1,th2],[0,0])*th1_bar + subs(diff(diff(m12_tilde,th1),th2),[th1,th2],[0,0])*th2)*ddth2 + subs(diff(b11_tilde,th1),[th1,th2],[0,0])*dth1_bar^2 + subs(diff(b12_tilde,th1),[th1,th2],[0,0])*dth1_bar*dth2 + subs(diff(b22_tilde,th1),[th1,th2],[0,0])*dth2^2 + k); % matlabFunction(b2_second_order,'File','f_variational_b2_second_order','Var',[th1_bar, dth1_bar , ddth1_bar , th2 , dth2 , ddth2 , I, L , c , k, m, mt, mx, my]); % matlabFunction(b1_second_order,'File','f_variational_b1_second_order','Var',[th1_bar, dth1_bar , ddth1_bar , th2 , dth2 , ddth2 , I, L , c , k, m, mt, mx, my]); % matlabFunction(b0_second_order,'File','f_variational_b0_second_order','Var',[th1_bar, dth1_bar , ddth1_bar , th2 , dth2 , ddth2 , I, L , c , k, m, mt, mx, my]); %% Hill equation %In the paper, we use the notation \tilde{b}_0, \tilde{b}_1, and %\tilde{b_2}, instead of b0, b1, and b2, respectively. b2 = simplify(b2_second_order,'Steps', 100); b1 = simplify(b1_second_order,'Steps', 100); b0 = simplify(b0_second_order,'Steps', 100); [N_b2, D_b2] = numden(b2); [Ncb2, Ntb2] = coeffs(N_b2,[th1_bar th2]); [Ncb2_simp] = simplify_vec(Ncb2); D_b2=simplify(D_b2); [N_b1, D_b1] = numden(b1); [Ncb1, Ntb1] = coeffs(N_b1,[th1_bar th2 dth1_bar dth2]); [Ncb1_simp] = simplify_vec(Ncb1); D_b1=simplify(D_b1); [N_b0, D_b0] = numden(b0); [Ncb0, Ntb0] = coeffs(N_b0,[th1_bar th2 dth1_bar dth2 ddth1_bar ddth2]); [Ncb0_simp] = simplify_vec(Ncb0); D_b0=simplify(D_b0); charNtb2 = char(Ntb2); charNtb2([1:8, end-1:end]) = []; charNtb1 = char(Ntb1); charNtb1([1:8, end-1:end]) = []; charNtb0 = char(Ntb0); charNtb0([1:8, end-1:end]) = []; comm = ["b2*dddel + b1*ddel + b0*del = 0" " alpha_i*" charNtb2 " beta_i*" charNtb1 " gamma_i*" charNtb0]; matlabFunction(Ncb2_simp./D_b2,'File','f_hill_b2_alpha_i','Comments',comm); matlabFunction(Ncb1_simp./D_b1,'File','f_hill_b1_beta_i','Comments',comm); matlabFunction(Ncb0_simp./D_b0,'File','f_hill_b0_gamma_i','Comments',comm); %% Hill's Determinante syms th1_bar dth1_bar ddth1_bar th2 dth2 ddth2 ... alpha_1 alpha_2 alpha_3 alpha_4 ... beta_1 beta_2 beta_3 beta_4 beta_5 ... gamma_1 gamma_2 gamma_3 gamma_4 gamma_5 gamma_6 gamma_7 gamma_8... a_th b_th w epsilon t real b2 = alpha_1*th1_bar^2 + alpha_2*th2*th1_bar + alpha_3*th2^2 + alpha_4; b1 = beta_1*dth1_bar*th1_bar + beta_2*dth2*th1_bar + beta_3*dth1_bar*th2 + beta_4*dth2*th2 + beta_5; b0 = gamma_1*ddth1_bar*th1_bar + gamma_2*ddth2*th1_bar + gamma_3*ddth1_bar*th2 + gamma_4*ddth2*th2 + gamma_5*dth1_bar^2 + gamma_6*dth2*dth1_bar + gamma_7*dth2^2 + gamma_8; b2_subs = subs(b2,[th1_bar dth1_bar ddth1_bar th2 dth2 ddth2] , [epsilon*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*w*(a_th*cos(w*t)-b_th*sin(w*t)) , -epsilon*w^2*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*cos(w*t) , -epsilon*w*sin(w*t) , -epsilon*w^2*cos(w*t)]); b1_subs = subs(b1,[th1_bar dth1_bar ddth1_bar th2 dth2 ddth2] , [epsilon*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*w*(a_th*cos(w*t)-b_th*sin(w*t)) , -epsilon*w^2*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*cos(w*t) , -epsilon*w*sin(w*t) , -epsilon*w^2*cos(w*t)]); b0_subs = subs(b0,[th1_bar dth1_bar ddth1_bar th2 dth2 ddth2] , [epsilon*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*w*(a_th*cos(w*t)-b_th*sin(w*t)) , -epsilon*w^2*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*cos(w*t) , -epsilon*w*sin(w*t) , -epsilon*w^2*cos(w*t)]); %% b2_simplify = sin2_cossin_cos2_to_sincos2(b2_subs); b1_simplify = sin2_cossin_cos2_to_sincos2(b1_subs); b0_simplify = sin2_cossin_cos2_to_sincos2(b0_subs); %% order 2 syms M0 M2 N2 real del_bar2 = M0+M2*cos(2*w*t)+N2*sin(2*w*t); ddel_bar2 = 2*w*(-M2*sin(2*w*t)+N2*cos(2*w*t)); dddel_bar2 = -4*w^2*(M2*cos(2*w*t)+N2*sin(2*w*t)); eq2 = b2_simplify*dddel_bar2 + b1_simplify*ddel_bar2 + b0_simplify*del_bar2; % [csc, tsc] = coeffs(eq,[sin(2*w*t),cos(2*w*t)]); eq_simp2 = sin2_cossin_cos2_to_sincos4(eq2); [csc, tsc] = coeffs(eq_simp2,[sin(2*w*t),cos(2*w*t) sin(4*w*t) cos(4*w*t)]); [csc1, tsc1] = coeffs(csc(1)+(M0+M2+N2),[M0 M2 N2]); [csc2, tsc2] = coeffs(csc(2)+(M0+M2+N2),[M0 M2 N2]); [csc3, tsc3] = coeffs(csc(3)+(M0+M2+N2),[M0 M2 N2]); [csc4, tsc4] = coeffs(csc(4)+(M0+M2+N2),[M0 M2 N2]); [csc5, tsc5] = coeffs(csc(5)+(M0+M2+N2),[M0 M2 N2]); Hill_H3x3 = simplify([csc5(1)-1 , csc5(2)-1 csc5(3)-1 csc1(1)-1 , csc1(2)-1 csc1(3)-1 csc2(1)-1 , csc2(2)-1 csc2(3)-1]); det_Hill_H3x3 = simplify(det(Hill_H3x3)); det_Hill_H3x3_f = matlabFunction(det_Hill_H3x3,'File','f_det_Hill_H_dellbarorder_2'); %% syms m11 m12 real % form_a_th = -(c*w^3*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 4*L^4*m^2 + 8*L^4*m*my + 4*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(k^2 + c^2*w^2 + (4*w^4*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2)^2)/(9*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)^2) - (4*k*w^2*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)))*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)); form_a_th = (c*m12*w^3)/(c^2*w^2+k^2-2*k*m11*w^2+m11^2*w^4); % form_b_th = -(w^2*(k - (2*w^2*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)))*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 4*L^4*m^2 + 8*L^4*m*my + 4*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(k^2 + c^2*w^2 + (4*w^4*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2)^2)/(9*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)^2) - (4*k*w^2*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)))*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)); form_b_th = (m12*w^2*(k-m11*w^2))/(c^2*w^2+k^2-2*k*m11*w^2+m11^2*w^4); Hill_H3X3_a_b_th = subs(Hill_H3x3,[a_th,b_th],[form_a_th,form_b_th]); %% syms a_th b_th real det_Hill_H3X3_a_b_th = subs(det_Hill_H3x3,[a_th,b_th],[form_a_th,form_b_th]); [N , D] = numden(det_Hill_H3X3_a_b_th); [Nc_det_Hill_H3X3_a_b_th, Nt_det_Hill_H3X3_a_b_th] = coeffs(N,epsilon); [Dcwe, Dtwe] = coeffs(D,epsilon); matlabFunction(Nc_det_Hill_H3X3_a_b_th,'File','f_eq_zero_det_Hill_H3X3_N_1.m','Comments','output1*epsilon^6 + output2*epsilon^4 + output3*epsilon^2 + output4*1 = 0') %% % dimentions and propertis w = 20*pi; % [rad/sec] spring_k=0.002;%1; % [N*m/rad] spring coefficient Damping_c=1.9747*sqrt(spring_k); % [N*m*sec/rad] Critical Damping coefficient link_a = 0.1524; %[m] link length = 2a link_b = 0.0762; %[m] link width = 2b f_ro = 1000; %[kg/m^3] liquid density link_hight = 0.0254; %[m] link hight link_m = f_ro*pi*link_a*link_b*link_hight; % link 0 mass link_mx = link_hight*pi*f_ro*link_b^2; % added mass in direction x; link_my = link_hight*pi*f_ro*link_a^2; % added mass in direction y link_mt = link_hight*(1/8)*pi*f_ro*(link_a^2-link_b^2)^2; % added rotational mass link_I = (link_m*(link_a^2+link_b^2))/4; % link 0 inertia link_L = 0.198; %[m] Distance from center of link to axis m11 = M_11_in0(link_I,link_L,link_m,link_mt,link_my); m12 = M_12_in0(link_I,link_L,link_m,link_mt,link_my); a_th = f_a_th1_ana(link_I,link_L,Damping_c,spring_k,link_m,link_mt,link_my,w); b_th = f_b_th1_ana(link_I,link_L,Damping_c,spring_k,link_m,link_mt,link_my,w); alpha_i = f_hill_b2_alpha_i(link_I,link_L,link_m,link_mt,link_mx,link_my); alpha_1 = alpha_i(1); alpha_2 = alpha_i(2); alpha_3 = alpha_i(3); alpha_4 = alpha_i(4); %Note that in the paper we expressed beta_i, for i=1, 2, 3, and 4 in terms of alpha_j with the %corresponding j and beta_5=c. beta_i = f_hill_b1_beta_i(link_I,link_L,Damping_c,link_m,link_mt,link_mx,link_my); beta_1 = beta_i(1); beta_2 = beta_i(2); beta_3 = beta_i(3); beta_4 = beta_i(4); beta_5 = beta_i(5); gamma_i = f_hill_b0_gamma_i(link_I,link_L,spring_k,link_m,link_mt,link_mx,link_my); %The gamma_i's that appear here were denoted by mu_j in the Appendix of the paper (more %specifically: mu_1=gamma_1=gamma_5, mu_2=gamma_2=gamma_10, %mu_3=gamma_3=gamma_6=2*gamma_11, mu_4=gamma_4, mu_5=gamma_7, mu_6=gamma_8, %mu_7=gamma_9=0.5*gamma_12, mu_8=gamma_13, mu_9=gamma_14, %mu_10=gamma_15=-2*gamma_16. gamma_1 = gamma_i(1); gamma_2 = gamma_i(2); gamma_3 = gamma_i(3); gamma_4 = gamma_i(4); gamma_5 = gamma_i(5); gamma_6 = gamma_i(6); gamma_7 = gamma_i(7); gamma_8 = gamma_i(8); Si = f_eq_zero_det_Hill_H3X3_N_1(alpha_1,alpha_2,alpha_3,alpha_4,beta_1,beta_2,beta_3,beta_4,beta_5,Damping_c,gamma_1,gamma_2,gamma_3,gamma_4,gamma_5,gamma_6,gamma_7,gamma_8,spring_k,m11,m12,w); zero_det_Hill_H3X3_eq = Nt_det_Hill_H3X3_a_b_th.*Si %% function [exp_out] = sin2_cossin_cos2_to_sincos2(exp) %%% a*sin(w*t)^2 + b*cos(w*t)*sin(w*t) + c*cos(w*t)^2 + d %%% to e*sin(2*w*t) + f*cos(2*w*t) + g syms w t real [csc, tsc] = coeffs(exp,[sin(w*t),cos(w*t)],'All'); a = csc(1,3); b = csc(2,2); c = csc(3,1); d = csc(3,3); e = 0.5*b; f = 0.5*(c-a); g = 0.5*(a+c+2*d); exp_out = e*sin(2*w*t) + f*cos(2*w*t) + g; end function [exp_out] = sin2_cossin_cos2_to_sincos4(exp) %%% a*sin(2*t*w)^2 + b*cos(2*t*w)*sin(2*t*w) + c*sin(2*t*w) + d*cos(2*t*w)^2 + e*cos(2*t*w) + f %%% to g*sin(4*w*t) + h*cos(4*w*t) + i*sin(2*w*t) + j*cos(2*w*t) + k syms w t real [csc, tsc] = coeffs(exp,[sin(2*w*t),cos(2*w*t)],'All'); a = csc(1,3); b = csc(2,2); c = csc(2,3); d = csc(3,1); e = csc(3,2); f = csc(3,3); g = 0.5*b; h = 0.5*(d-a); i = c; j = e; k = 0.5*(a+d+2*f); exp_out = g*sin(4*w*t) + h*cos(4*w*t) + i*sin(2*w*t) + j*cos(2*w*t) + k; end function [H,B] = MY_EulerLagrange_H(L,R,X,DDX) % Euler - Lagrange syms t % Reshape coordinates/velocities state vector to help with substitution X2n = reshape(X,2,[]); X3n = [X2n;DDX]; numcoor = size(X2n,2); qt = arrayfun(@(ii) symfun(['q' int2str(ii) '(t)'],t),1:numcoor,'UniformOutput',false); Xt2n = [qt; arrayfun(@(ii) diff(qt{1,ii},t),1:numcoor,'UniformOutput',false)]; Xt3n = [Xt2n;arrayfun(@(ii) diff(qt{1,ii},t,t),1:numcoor,'UniformOutput',false)]; R = sym(R); % take care of numeric zero DE = []; for ii = 1:numcoor % Differentiate w.r.t. coordinate/velocity L_q = diff(L,X2n{1,ii}); L_qdot = diff(L,X2n{2,ii}); R_qdot = diff(R,X2n{2,ii}); % Substitute coordinates/velocities by corresponding symfuns L_qt(t) = subs(L_q, X2n,Xt2n); %#ok<*AGROW> L_qtdot(t) = subs(L_qdot,X2n,Xt2n); R_qtdot(t) = subs(R_qdot,X2n,Xt2n); % Differentiate "velocity" term w.r.t. time L_qdotdt = diff(L_qtdot,t); % Collect all Lagrange terms DE = [DE; (L_qdotdt - L_qt + R_qtdot)]; end % of for % Return to original coordinates DE = subs(DE,Xt3n,X3n); DE = formula(DE); %%%% DE is the eq system of the problem [DE]=[0] is the homogenos eq % finding all the coeffs of DDX and building H for ii = 1:numcoor [c, t] = coeffs(DE(ii),[X,DDX]); for jj = 1:numcoor [row,col]=find(t == DDX(jj)); if isempty(row) H(ii,jj) = 0; else H(ii,jj) = c(row,col); end end end H=simplify(H); B = simplify(DE-H*DDX'); % B is what's left end function [simp_vec] = simplify_vec(vec) for i = 1:length(vec) simp_vec(i) = simplify(vec(i),'Steps', 5000); % simp_vec(i) = simplify(simp_vec(i),'Steps', 100); end end function [str_n]=latex2(symbol) % example % CCC = latex2(L); % clipboard('copy',CCC); % % str = latex(symbol); str = strrep(str, '\mathrm{mx}', 'm_{x}'); str = strrep(str, '\mathrm{my}', 'm_{y}'); str = strrep(str, '\mathrm{link_mt}', 'm_{t}'); % str = strrep(str, 'beta', '\beta'); str = strrep(str, '\mathrm{th}_{1}', '\theta_{1}'); str = strrep(str, '\mathrm{th}_{2}', '\theta_{2}'); str = strrep(str, '\mathrm{dbeta}', '\dot{\beta}'); str = strrep(str, '\mathrm{dy}', '\dot{y}'); str = strrep(str, '\mathrm{dx}', '\dot{x}'); str = strrep(str, '\mathrm{dth}_{1}', '\dot{\theta}_{1}'); str = strrep(str, '\mathrm{dth}_{2}', '\dot{\theta}_{2}'); str = strrep(str, '\mathrm{ddth}_{1}', '\ddot{\theta}_{1}'); str = strrep(str, '\mathrm{ddth}_{2}', '\ddot{\theta}_{2}'); str = strrep(str, '\mathrm{th}_{\bar{1}}', '\overline{\theta_{1}}'); str = strrep(str, '\mathrm{dth}_{\bar{1}}', '\dot{\overline{\theta}}_{1}'); str = strrep(str, '\mathrm{ddth}_{\bar{1}}', '\ddot{\overline{\theta}}_{1}'); str = strrep(str, '\mathrm{del}', '\delta'); str = strrep(str, '\mathrm{ddel}', '\dot{\delta}'); str = strrep(str, '\mathrm{dddel}', '\ddot{\delta}'); str = strrep(str, 'f_{1,\mathrm{th}}', '{f}_{\theta}^{(1)}'); str = strrep(str, '\mathrm{df}_{1,\mathrm{th}}', '\dot{f}_{\theta}^{(1)}'); str = strrep(str, '\mathrm{ddf}_{1,\mathrm{th}}', '\ddot{f}_{\theta}^{(1)}'); str = strrep(str, 'f_{2,\mathrm{th}}', '{f}_{\theta}^{(2)}'); str = strrep(str, '\mathrm{df}_{2,\mathrm{th}}', '\dot{f}_{\theta}^{(2)}'); str = strrep(str, '\mathrm{ddf}_{2,\mathrm{th}}', '\ddot{f}_{\theta}^{(2)}'); str = strrep(str, 'b_{\mathrm{th}}', 'b_{\theta}'); str = strrep(str, 'a_{\mathrm{th}}', 'a_{\theta}'); str = strrep(str, 'w', '\omega'); str = strrep(str, '\delta _{\mathrm{mi}}', '\delta'); str = strrep(str, 'mathrm{ep}', '\varepsilon'); str = strrep(str, '\epsilon', '\varepsilon'); str = erase(str, '\,'); str = erase(str, '\left'); str = erase(str, '\right'); str_n=str; end