Added EulerLagrange models

README.md

@@ -13,6 +13,7 @@ The paper can be seen at https://gitlab.fel.cvut.cz/lehkysim/fbab-thesis.
 #### DONE
 * Hardware modification
 * Camera homography
+* Motor friction identification
 * Mathematical models of the hybrid system
     * Balancing mode
     * Revolving mode
@@ -27,10 +28,4 @@ The paper can be seen at https://gitlab.fel.cvut.cz/lehkysim/fbab-thesis.
 * Review and completion of the system documentation
 
 #### WIP
-* Motor friction identification
 * Demonstration of the ball throw
-
-#### TODO
-* SHDN RPi button repair
-
-#### LONG-TERM GOALS

docs/README.md

@@ -44,7 +44,11 @@
 ## Switching on
 
 * **PC**: First, run *load_params.m* script to initialize all necessary variables
-* **PC**: Before starting any Simulink file, be sure to add *dependencies* file located in the same folder as .slx file to path (including their subfolders)
+* **PC**: Before starting any Simulink file, be sure to add *dependencies* file located in the same folder as .slx file to Matlab path (including their subfolders)
     * If you want to use Moteus motor controlled from Simulink, it is recommended to copy *dependencies* folder from moteus folder in main directory to directory where you create new Simulink file 
+* **PC**: In Simulink file, check if Raspberry IP address and name are correct
+* **Raspberry**: Launch RaspiCam with `python3 vision.py -vc config.json` and calibrate camera in RaspiBallPos interface
+* **PC**: Build and deploy Simulink file
+* **PC**: Start simulating
 
 <br/>
\ No newline at end of file

m/control/projecile/create_FF_wav.m

+clear simin;
 
 Ts = 0.005;
-ks = [1 2 5 10];
-dur = [3 1 0.5 2];
+ks = [0.23 0 -0.3 0];
+dur = [0.65 0.2 1 2];
 
 t = @(start,dur) start:Ts:(start+dur);
 lin_fcn = @(k,t,q) k*t+q;
@@ -32,3 +33,6 @@ end
 
 plot(wav_t,wav);
 xlim([0 wav_t(end)]);
+
+simin.time = wav_t';
+simin.signals.values = wav';

m/eqn/balancing_mode/bal_lin_el.m

+%% LINEARIZATION OF MODEL GENERATED BY EULER-LAGRANGE TOOLBOX
+
+syms theta Dtheta d Dd tau
+
+theta_dot = Dtheta;
+d_dot = Dd;
+Dtheta_dot = (30.5812*tau - 0.0486*Dtheta - 0.0034*Dd + 0.3233*sin(theta) - 0.0057*Dtheta^2*d - 9.8069*d*cos(theta) - 2*Dd*Dtheta*d)/(d^2 + 0.0272);
+Dd_dot = (1.2494e-04*Dtheta - 0.0153*Dd - 0.1747*tau - 0.1924*sin(theta) - 7.0053*d^2*sin(theta) - 0.5623*Dd*d^2 - 0.0056*Dtheta*d^2 + 0.0195*Dtheta^2*d + 0.0560*d*cos(theta) + 0.7143*Dtheta^2*d^3 + 0.0114*Dd*Dtheta*d)/(d^2 + 0.0272);
+
+f = [theta_dot; 
+     Dtheta_dot; 
+     d_dot; 
+     Dd_dot];
+
+% operating point
+xp = [0; 0; 0; 0];
+taup = 0;
+
+% matrix A
+A = jacobian(f, [theta, Dtheta, d, Dd]);
+A = double(subs(A,{'theta','Dtheta','d','Dd'},{xp(1),xp(2),xp(3),xp(4)}));
+A = round(A,3);
+
+% matrix B
+B = diff(f, tau);
+B = double(subs(B,{'theta','Dtheta','d','Dd','tau'},{xp(1),xp(2),xp(3),xp(4),taup}));
+
+% matrix C
+C = [1 0 0 0;
+     0 0 1 0];
+
+% matrix D
+D = 0;
+
+bm_sys = ss(A,B,C,D);
+% discrete state-space
+bm_sys_d = c2d(bm_sys,prms.Ts,'zoh');
+A_d = bm_sys_d.A;
+B_d = bm_sys_d.B;
+C_d = bm_sys_d.C;
+D_d = bm_sys_d.D;
+

m/eqn/balancing_mode/bal_vec_el.m

+%% STATE-SPACE MODEL VECTOR GENERATED BY EULER-LAGRANGE TOOLBOX
+
+function dYdt = Balancing_enum(t, Y)    %#ok
+
+tau = 0;
+
+theta = Y(1);
+Dtheta = Y(2);
+d = Y(3);
+Dd = Y(4);
+
+dYdt = ...
+    [Dtheta;
+    (30.5812*tau - 0.0486*Dtheta - 0.0034*Dd + 0.3233*sin(theta) - 0.0057*Dtheta^2*d - 9.8069*d*cos(theta) - 2*Dd*Dtheta*d)/(d^2 + 0.0272);
+    Dd;
+    (1.2494e-04*Dtheta - 0.0153*Dd - 0.1747*tau - 0.1924*sin(theta) - 7.0053*d^2*sin(theta) - 0.5623*Dd*d^2 - 0.0056*Dtheta*d^2 + 0.0195*Dtheta^2*d + 0.0560*d*cos(theta) + 0.7143*Dtheta^2*d^3 + 0.0114*Dd*Dtheta*d)/(d^2 + 0.0272)];
+
+end
+

m/eqn/projectile_mode/pro_vec_el.m

+%% STATE-SPACE MODEL VECTOR GENERATED BY EULER-LAGRANGE TOOLBOX
+
+function dYdt = Projectile_sys(t, Y, prms)  %#ok
+
+g  = prms.g;
+
+dYdt = [Y(2);
+        0;
+        Y(4);
+        -g;
+        Y(6);
+        0];
+
+end

m/eqn/revolving_mode/rev_lin_el.m

+%% LINEARIZATION OF MODEL GENERATED BY EULER-LAGRANGE TOOLBOX
+
+syms theta Dtheta alpha Dalpha tau
+
+theta_dot = Dtheta;
+d_alpha = Dalpha;
+Dtheta_dot = (474.4086*Dtheta - 1.0125*Dalpha - 2.9888e+05*tau - 57.5183*cos(alpha + theta) + 8.1707e+03*cos(theta) - 6.1719*Dalpha*cos(alpha) - 3.5780*Dtheta*cos(alpha) - 7.0417e+03*cos(alpha + theta)*cos(alpha) - 22.7029*Dalpha^2*sin(alpha) - 23.2030*Dtheta^2*sin(alpha) - 61.2125*Dtheta^2*cos(alpha)*sin(alpha) - 45.4068*Dalpha*Dtheta*sin(alpha))/(cos(alpha) + 61.2125*cos(alpha)^2 - 335.8439);
+Dd_alpha = (92.3358*Dalpha - 431.9110*Dtheta + 3.0545e+05*tau + 1.0423e+05*cos(alpha + theta) - 8.3505e+03*cos(theta) + 8.7659*Dalpha*cos(alpha) - 1.2756e+03*Dtheta*cos(alpha) + 8.0581e+05*tau*cos(alpha) + 7.0419e+03*cos(alpha + theta)*cos(alpha) + 23.2033*Dalpha^2*sin(alpha) + 929.2325*Dtheta^2*sin(alpha) - 2.2030e+04*cos(alpha)*cos(theta) + 61.2125*Dalpha^2*cos(alpha)*sin(alpha) + 122.4263*Dtheta^2*cos(alpha)*sin(alpha) + 46.4053*Dalpha*Dtheta*sin(alpha) + 122.4263*Dalpha*Dtheta*cos(alpha)*sin(alpha))/(cos(alpha) + 61.2125*cos(alpha)^2 - 335.8439);
+
+f = [theta_dot; 
+     Dtheta_dot; 
+     d_alpha; 
+     Dd_alpha];
+
+% operating point
+xp = [pi/2; 0; 0; 0];
+taup = 0;
+
+% matrix A
+A = jacobian(f, [theta, Dtheta, alpha, Dalpha]);
+A = double(subs(A,{'theta','Dtheta','alpha','Dalpha'},{xp(1),xp(2),xp(3),xp(4)}));
+A = round(A,3);
+
+% matrix B
+B = diff(f, tau);
+B = double(subs(B,{'theta','Dtheta','alpha','Dalpha','tau'},{xp(1),xp(2),xp(3),xp(4),taup}));
+
+% matrix C
+C = [1 0 0 0;
+     0 0 1 0];
+
+% matrix D
+D = 0;
+
+rm_sys = ss(A,B,C,D);
+% discrete state-space
+rm_sys_d = c2d(rm_sys,prms.Ts,'zoh');
+A_d = rm_sys_d.A;
+B_d = rm_sys_d.B;
+C_d = rm_sys_d.C;
+D_d = rm_sys_d.D;
+

m/eqn/revolving_mode/rev_vec_el.m

+%% STATE-SPACE MODEL VECTOR GENERATED BY EULER-LAGRANGE TOOLBOX
+
+
+function dYdt = Balancing_enum(t, Y)    %#ok
+
+tau = 0;
+
+theta = Y(1);
+Dtheta = Y(2);
+alpha = Y(3);
+Dalpha = Y(4);
+
+dYdt = ...
+    [Dtheta;
+    (474.4086*Dtheta - 1.0125*Dalpha - 2.9888e+05*tau - 57.5183*cos(alpha + theta) + 8.1707e+03*cos(theta) - 6.1719*Dalpha*cos(alpha) - 3.5780*Dtheta*cos(alpha) - 7.0417e+03*cos(alpha + theta)*cos(alpha) - 22.7029*Dalpha^2*sin(alpha) - 23.2030*Dtheta^2*sin(alpha) - 61.2125*Dtheta^2*cos(alpha)*sin(alpha) - 45.4068*Dalpha*Dtheta*sin(alpha))/(cos(alpha) + 61.2125*cos(alpha)^2 - 335.8439);
+    Dalpha;
+    (92.3358*Dalpha - 431.9110*Dtheta + 3.0545e+05*tau + 1.0423e+05*cos(alpha + theta) - 8.3505e+03*cos(theta) + 8.7659*Dalpha*cos(alpha) - 1.2756e+03*Dtheta*cos(alpha) + 8.0581e+05*tau*cos(alpha) + 7.0419e+03*cos(alpha + theta)*cos(alpha) + 23.2033*Dalpha^2*sin(alpha) + 929.2325*Dtheta^2*sin(alpha) - 2.2030e+04*cos(alpha)*cos(theta) + 61.2125*Dalpha^2*cos(alpha)*sin(alpha) + 122.4263*Dtheta^2*cos(alpha)*sin(alpha) + 46.4053*Dalpha*Dtheta*sin(alpha) + 122.4263*Dalpha*Dtheta*cos(alpha)*sin(alpha))/(cos(alpha) + 61.2125*cos(alpha)^2 - 335.8439)];
+
+end
+

m/ident/model_verify/create_input.m

+
+Ts = 0.005;
+ks = [1 2 5 10];
+dur = [3 1 0.5 2];
+
+t = @(start,dur) start:Ts:(start+dur);
+lin_fcn = @(k,t,q) k*t+q;
+
+wav_t = zeros(1,sum(dur)/Ts);
+wav = zeros(1,sum(dur)/Ts);
+t_last = 0;
+fcn_last = 0;
+N = 1;
+
+t_all = 0;
+
+for i = 1:numel(dur)
+
+    time = t(t_last,dur(i));
+    func = lin_fcn(ks(i),time-t_last,fcn_last);
+
+    for j = 1:(numel(time))
+        wav_t(N+j) = time(j);
+        wav(N+j) = func(j);
+    end
+
+    N = N + numel(time);
+    t_last = time(end);
+    fcn_last = func(end);
+
+end
+
+plot(wav_t,wav);
+xlim([0 wav_t(end)]);

model/euler_lagrange/balancing/Balancing_enum.m

+function dYdt = Balancing_enum(t, Y)    %#ok
+
+tau = 0;
+
+theta = Y(1);
+Dtheta = Y(2);
+d = Y(3);
+Dd = Y(4);
+
+dYdt = ...
+    [Dtheta;
+    (30.5812*tau - 0.0486*Dtheta - 0.0034*Dd + 0.3233*sin(theta) - 0.0057*Dtheta^2*d - 9.8069*d*cos(theta) - 2*Dd*Dtheta*d)/(d^2 + 0.0272);
+    Dd;
+    (1.2494e-04*Dtheta - 0.0153*Dd - 0.1747*tau - 0.1924*sin(theta) - 7.0053*d^2*sin(theta) - 0.5623*Dd*d^2 - 0.0056*Dtheta*d^2 + 0.0195*Dtheta^2*d + 0.0560*d*cos(theta) + 0.7143*Dtheta^2*d^3 + 0.0114*Dd*Dtheta*d)/(d^2 + 0.0272)];
+
+end
+

model/euler_lagrange/Balancing_sys.m → model/euler_lagrange/balancing/Balancing_sys.m

-function dYdt = Balancing_sys(t,Y,prms)
-%Balancing_sys
-%    dYdt = Balancing_sys(T,Y,D1,M1,R1,B1,I1,M2,R2,B2,I2,G)
-
-%    This function was generated by the Symbolic Math Toolbox version 9.1.
-%    23-May-2023 17:51:07
-
-d1 = prms.d1;
-m1 = prms.m1;
-r1 = prms.r1;
-b1 = prms.b1;
-I1 = prms.I1;
-m2 = prms.m2;
-r2 = prms.r2;
-b2 = prms.b2;
-I2 = prms.I2;
-g  = prms.g;
-
-t2 = Y(1);
-t3 = Y(2);
-t4 = Y(3);
-t5 = Y(4);
-t6 = m2.^2;
-t7 = r2.^2;
-t8 = r2.^3;
-t9 = I1.*I2;
-t10 = cos(t2);
-t11 = sin(t2);
-t12 = t3.^2;
-t13 = t4.^2;
-t14 = I1.*m2.*t7;
-t15 = I2.*m2.*t7.*4.0;
-t16 = I2.*m2.*t13;
-t17 = t6.*t7.*t13;
-t18 = t9+t14+t15+t16+t17;
-t19 = 1.0./t18;
-mt1 = [t3;-(t19.*(-I2.*b2.*t5+b1.*m2.*t3.*t8+b2.*m2.*t3.*t8+b2.*m2.*t5.*t7-g.*t6.*t7.^2.*t11+I2.*b1.*r2.*t3-I2.*b2.*r2.*t3-I2.*g.*m2.*t7.*t11.*3.0+I2.*m2.*t4.*t7.*t12.*2.0-g.*r1.*t6.*t8.*t11+g.*t4.*t6.*t8.*t10+t3.*t4.*t5.*t6.*t8.*2.0-I2.*g.*m2.*r1.*r2.*t11+I2.*g.*m2.*r2.*t4.*t10+I2.*m2.*r2.*t3.*t4.*t5.*2.0))./r2;t5];
-mt2 = [-t19.*(I1.*b2.*t5+I2.*b2.*t5.*2.0+g.*t11.*t14+g.*t11.*t17-t4.*t12.*t14+b2.*m2.*t5.*t13-t4.^3.*t6.*t7.*t12+I1.*b2.*r2.*t3-I2.*b1.*r2.*t3.*2.0+I2.*b2.*r2.*t3.*2.0+I2.*g.*m2.*t7.*t11.*6.0-I2.*m2.*t4.*t7.*t12.*4.0+b2.*m2.*r2.*t3.*t13+I2.*g.*m2.*r1.*r2.*t11.*2.0-I2.*g.*m2.*r2.*t4.*t10.*2.0-I2.*m2.*r2.*t3.*t4.*t5.*4.0)];
-dYdt = [mt1;mt2];
+function dYdt = Balancing_sys(t, Y, prms)   %#ok
+
+tau = 0;
+
+d1 = prms.d1;
+m1 = prms.m1;
+r1 = prms.r1;
+b1 = prms.b1;
+I1 = prms.I1;
+m2 = prms.m2;
+r2 = prms.r2;
+b2 = prms.b2;
+I2 = prms.I2;
+g  = prms.g;
+
+t2 = Y(1);
+t3 = Y(2);
+t4 = Y(3);
+t5 = Y(4);
+t6 = m2.^2;
+t7 = r2.^2;
+t8 = r2.^3;
+t9 = I1.*I2;
+t10 = cos(t2);
+t11 = sin(t2);
+t12 = t3.^2;
+t13 = t4.^2;
+t14 = I1.*m2.*t7;
+t15 = I2.*m2.*t7.*4.0;
+t16 = I2.*m2.*t13;
+t17 = t6.*t7.*t13;
+t18 = t9+t14+t15+t16+t17;
+t19 = 1.0./t18;
+mt1 = [t3;-(t19.*(-I2.*b2.*t5-I2.*r2.*tau-m2.*t8.*tau+b1.*m2.*t3.*t8+b2.*m2.*t3.*t8+b2.*m2.*t5.*t7-g.*t6.*t7.^2.*t11+I2.*b1.*r2.*t3-I2.*b2.*r2.*t3-I2.*g.*m2.*t7.*t11.*3.0+I2.*m2.*t4.*t7.*t12.*2.0-g.*r1.*t6.*t8.*t11+g.*t4.*t6.*t8.*t10+t3.*t4.*t5.*t6.*t8.*2.0-I2.*g.*m2.*r1.*r2.*t11+I2.*g.*m2.*r2.*t4.*t10+I2.*m2.*r2.*t3.*t4.*t5.*2.0))./r2;t5];
+mt2 = [-t19.*(I1.*b2.*t5+I2.*b2.*t5.*2.0+I2.*r2.*tau.*2.0+g.*t11.*t14+g.*t11.*t17-t4.*t12.*t14+b2.*m2.*t5.*t13-t4.^3.*t6.*t7.*t12+I1.*b2.*r2.*t3-I2.*b1.*r2.*t3.*2.0+I2.*b2.*r2.*t3.*2.0+I2.*g.*m2.*t7.*t11.*6.0-I2.*m2.*t4.*t7.*t12.*4.0+b2.*m2.*r2.*t3.*t13+I2.*g.*m2.*r1.*r2.*t11.*2.0-I2.*g.*m2.*r2.*t4.*t10.*2.0-I2.*m2.*r2.*t3.*t4.*t5.*4.0)];
+dYdt = [mt1;mt2];
+
+end

model/euler_lagrange/balancing.m → model/euler_lagrange/balancing/balancing.m

 %% CREATE MODEL
 
-% syms theta(t) d(t) tau(t)
 syms theta Dtheta d Dd
+syms tau
 syms d1 m1 r1 b1 I1 m2 r2 b2 I2 g
 
-% first derivatives
-% Dtheta(t) = diff(theta(t),t);
-% Dd(t) = diff(d(t),t);
-
 phi = d/r2 + theta;
 Dphi = Dd/r2 + Dtheta;
 
+% kinetic energy 
 T = 1/2*I1*Dtheta^2 + 1/2*m2*(d^2*Dtheta^2+Dd^2) + ...
     1/2*I2*(Dtheta+Dphi)^2; 
 
+% potential energy
 h = d*sin(theta) + (r1+r2)*cos(theta);
 V = m2*g*h;
 
+% Lagrangian
 L = T - V;
 X = {theta Dtheta d Dd};
 Q_i = {0 0};
-Q_e = {0 0};
+Q_e = {tau 0};
 
+% dissipation function  
 R = 1/2*b1*Dtheta^2 + 1/2*b2*Dphi^2;
 
 par = {d1 m1 r1 b1 I1 m2 r2 b2 I2 g};
 
 % VF = EulerLagrange(L,X,Q_i,Q_e,R,par,'m','Balancing_sys');
-
-%% SIMULATE
-
-tspan = [0 10];
-Y0 = [0 deg2rad(30) 0 0.03];
-options = odeset('RelTol',1e-8);
-
-% dYdt = Balancing_sys(T,Y,D1,M1,R1,B1,I1,M2,R2,B2,I2,G)
-[t, Y] = ode45(@Balancing_sys,tspan,Y0,options,prms);
-plot(t,Y)
-legend("theta","Dtheta","d","Dd");

model/euler_lagrange/balancing/simulate.m

+%% SIMULATE
+
+tspan = [0 0.4];
+Y0 = [0 0 0.03 0];
+options = odeset('RelTol',1e-8);
+ 
+% [t1, Y1] = ode45(@Balancing_sys,tspan,Y0,optionss);
+[t2, Y2] = ode45(@Balancing_enum,tspan,Y0,options);
+
+plot(t2, Y2);
\ No newline at end of file

model/euler_lagrange/balancing/tmp.m

+%% VARIABLES
+
+syms theta Dtheta d Dd
+syms tau
+Y = [theta Dtheta d Dd];
+
+d1 = prms.d1;
+m1 = prms.m1;
+r1 = prms.r1;
+b1 = prms.b1;
+I1 = prms.I1;
+m2 = prms.m2;
+r2 = prms.r2;
+b2 = prms.b2;
+I2 = prms.I2;
+g  = prms.g;
+
+%% ENUMERATE VECTOR
+
+t2 = Y(1);
+t3 = Y(2);
+t4 = Y(3);
+t5 = Y(4);
+t6 = m2.^2;
+t7 = r2.^2;
+t8 = r2.^3;
+t9 = I1.*I2;
+t10 = cos(t2);
+t11 = sin(t2);
+t12 = t3.^2;
+t13 = t4.^2;
+t14 = I1.*m2.*t7;
+t15 = I2.*m2.*t7.*4.0;
+t16 = I2.*m2.*t13;
+t17 = t6.*t7.*t13;
+t18 = t9+t14+t15+t16+t17;
+t19 = 1.0./t18;
+mt1 = [t3;-(t19.*(-I2.*b2.*t5-I2.*r2.*tau-m2.*t8.*tau+b1.*m2.*t3.*t8+b2.*m2.*t3.*t8+b2.*m2.*t5.*t7-g.*t6.*t7.^2.*t11+I2.*b1.*r2.*t3-I2.*b2.*r2.*t3-I2.*g.*m2.*t7.*t11.*3.0+I2.*m2.*t4.*t7.*t12.*2.0-g.*r1.*t6.*t8.*t11+g.*t4.*t6.*t8.*t10+t3.*t4.*t5.*t6.*t8.*2.0-I2.*g.*m2.*r1.*r2.*t11+I2.*g.*m2.*r2.*t4.*t10+I2.*m2.*r2.*t3.*t4.*t5.*2.0))./r2;t5];
+mt2 = [-t19.*(I1.*b2.*t5+I2.*b2.*t5.*2.0+I2.*r2.*tau.*2.0+g.*t11.*t14+g.*t11.*t17-t4.*t12.*t14+b2.*m2.*t5.*t13-t4.^3.*t6.*t7.*t12+I1.*b2.*r2.*t3-I2.*b1.*r2.*t3.*2.0+I2.*b2.*r2.*t3.*2.0+I2.*g.*m2.*t7.*t11.*6.0-I2.*m2.*t4.*t7.*t12.*4.0+b2.*m2.*r2.*t3.*t13+I2.*g.*m2.*r1.*r2.*t11.*2.0-I2.*g.*m2.*r2.*t4.*t10.*2.0-I2.*m2.*r2.*t3.*t4.*t5.*4.0)];
+dYdt = [mt1;mt2];
+
+%% STATE VECTOR FOR BALANCING MODE
+
+dYdt = ...
+    [Dtheta;
+    -(100*(5.0502e-12*Dd + 7.2809e-11*Dtheta - 4.5780e-08*tau - 4.8395e-10*sin(theta) + 8.5543e-12*Dtheta^2*d + 1.4681e-08*d*cos(theta) + 2.9940e-09*Dd*Dtheta*d))/(1.4970e-07*d^2 + 4.0732e-09);
+    Dd
+    -(2.2873e-09*Dd - 1.8703e-11*Dtheta + 2.6160e-08*tau + 2.8809e-08*sin(theta) + 1.0487e-06*d^2*sin(theta) + 8.4170e-08*Dd*d^2 + 8.4170e-10*Dtheta*d^2 - 2.9143e-09*Dtheta^2*d - 8.3892e-09*d*cos(theta) - 1.0693e-07*Dtheta^2*d^3 - 1.7109e-09*Dd*Dtheta*d)/(1.4970e-07*d^2 + 4.0732e-09)];
+ 
+%% SIMPLIFY VECTOR
+
+dYdt = ...
+    [Dtheta;
+    (30.5812*tau - 0.0486*Dtheta - 0.0034*Dd + 0.3233*sin(theta) - 0.0057*Dtheta^2*d - 9.8069*d*cos(theta) - 2*Dd*Dtheta*d)/(d^2 + 0.0272);
+    Dd;
+    (1.2494e-04*Dtheta - 0.0153*Dd - 0.1747*tau - 0.1924*sin(theta) - 7.0053*d^2*sin(theta) - 0.5623*Dd*d^2 - 0.0056*Dtheta*d^2 + 0.0195*Dtheta^2*d + 0.0560*d*cos(theta) + 0.7143*Dtheta^2*d^3 + 0.0114*Dd*Dtheta*d)/(d^2 + 0.0272)];

model/euler_lagrange/projectile/Projectile_sys.m

+function dYdt = Projectile_sys(t, Y, prms)  %#ok
+
+g  = prms.g;
+
+dYdt = [Y(2);
+        0;
+        Y(4);
+        -g;
+        Y(6);
+        0];
+
+end