%% Modelado dinámico de un tiovivo (2GL, Euler Lagrange)
% © 2024, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Este código funciónó con Matlab R2022b
%
% *Image from jana_lsb0 at Pixabay , Pixabay License 2022 (free even for commercial use)
%
% Presentaciones en vídeo:
% https://personales.upv.es/asala/YT/V/tiovEL.html
% https://personales.upv.es/asala/YT/V/tiovEL2.html
% https://personales.upv.es/asala/YT/V/tiovELsim.html
%
% Objetivos: Comprender el modelado físico de un tiovivo como la figura, y su simulación. En concreto, abordaremos el caso 2 grados de libertad con ecuaciones Euler-Lagrange de la dinámica.
% Tabla de Contenidos
% Cinemática
% Energías y ecs. movimiento Euler-Lagrange
%     Casos particulares
%         1.- Longitud suspendida cero
%         2.- R0=0, I0=0, tau=0, péndulo articulado esférico
%         3.- Oscilación columpio con velocidad tiovivo "forzada"
%     Forma Normalizada (ecs. en variables de estado)
%

syms phi theta real %grados de libertad para describir el movimiento
syms R_0 L M g I_0 real
assume(M>0), assume(g>0), assume(I_0>0), assume(L>0), assume(R_0>0) %ayuda para symbolic toolbox

%% Cinemática

R=R_0+L*sin(theta); %Distancia masa al eje de rotación central
x=R*cos(phi);
y=R*sin(phi);
z=L*(1-cos(theta)); %z=0 es el equilibrio vertical inferior cuando no se mueve
r=[x;y;z]
syms omega_0 omega_1 real %vels ang. dot_phi dot_theta
syms alpha_0 alpha_1 real %acels. ddot_phi ddot_theta
q=[phi; theta]; v_q=[omega_0;omega_1]; a_q=[alpha_0;alpha_1];

% v(q,\dot q)=\frac{dr}{dt}=\frac{\partial r}{\partial q}\cdot \frac{dq}{dt}=\frac{\partial r}{\partial q}\cdot \dot q

veloc = jacobian(r,q)*v_q %dr/dt

% Si quisiéramos calcular la aceleración de la masa suspendida en función de posición, velocidad y aceleraciones angulares, saldría:
% a(q,\dot q,\ddot q)=\frac{d^2r}{dt^2}=\frac{dv}{dt}=\frac{\partial v}{\partial q}\cdot \dot q+\frac{\partial v}{\partial \dot q}\cdot \ddot q=\left(\begin{array}{cc}\frac{\partial v}{\partial q} &\frac{\partial v}{\partial \dot q}\end{array}\right)\cdot \left(\begin{array}{c}\dot q \\ \ddot q\end{array}\right)

acel = simplify( jacobian(veloc,[q; v_q])*[v_q; a_q] ) %d2r/dt^2

% *Nota: esta aceleración NO es necesaria para obtener las ecuaciones con la metodología Euler-Lagrange.
%%
%% Energías y ecs. movimiento Euler-Lagrange

T=simplify( 0.5*M*(veloc'*veloc) + 0.5*I_0*omega_0^2, 100) %kinetic energy
V=M*g*z %potential energy
Lagrangian=T-V;

% Calculemos p(q,\dot q)=\frac{\partial L}{\partial \dot q}:

p = simplify( jacobian(Lagrangian,v_q) ) %momentum generalizado

% Las ecuaciones Euler Lagrange para cada coordenada q_i son:
% \frac{dp_i}{dt}-\frac{\partial L}{\partial q_i}=\tau_i
% donde, aplicando regla de la cadena:
% \frac{dp_i}{dt}=\frac{\partial p_i}{\partial q}\cdot \dot q+\frac{\partial p_i}{\partial \dot q}\cdot \ddot q=\left(\begin{array}{cc}\frac{\partial p_i}{\partial q} &\frac{\partial p_i}{\partial \dot q}\end{array}\right)\cdot \left(\begin{array}{c}\dot q \\ \ddot q\end{array}\right)

dpdt = simplify(jacobian(p,[q;v_q])*[v_q;a_q]);
syms tau real %par motor en tiovivo
%pongo en "columna" cada ec. movimiento q_i
EcsMovimiento = (dpdt - jacobian(Lagrangian,q)'==[tau;0]) %Euler-Lagrange

% Que podemos reescribir como M(q)\ddot q=\tau-C(q,\dot q)\dot q+G(q)
% (I_0+M(R_0+L\sin\theta)^2)\cdot \alpha_0=\tau - 2M(R_0+L\sin\theta)L\cos\theta\cdot \omega_0\omega_1 %plataforma
% ML^2\cdot \alpha_1=-MgL\sin\theta + M\omega_0^2(R_0+L\sin\theta)L\cos\theta     %masa suspendida

MatrizDeMasa = simplify(jacobian(lhs(EcsMovimiento),a_q))

%%
%% Casos particulares
%% 1.- Longitud suspendida cero

simplify(subs(EcsMovimiento,L,0))

% queda un modelo de 1GL con inercia equivalente I_0+MR_0^2.
%%
%
%% 2.- R0=0, I0=0, tau=0, péndulo articulado esférico

simplify(subs(EcsMovimiento,{R_0,I_0,tau},{0,0,0}))

%%
%% 3.- Oscilación columpio con velocidad tiovivo "forzada"
% Es el modelo visto en otros vídeos:
% ML^2\alpha_1=-MgL\sin\theta + M\omega_0^2(R_0+L\sin\theta)L\cos\theta,
%  par causado por peso y fuerza centrífuga, mirando sólo la segunda ecuación de movimiento. Si fijamos \omega_0(t) forzándolo a valer la trayectoria de velocidad de referencia que se desee, la primera ecuación de movimiento daría, sustituyendo \alpha_0=\frac{d\omega_0}{dt}, el par en función del tiempo necesario para mantener el perfil de velocidad deseado.
% Este tipo de cálculo está detras de metodologías de "par calculado" en control dinámico de robots.
%%
%%
%% Forma Normalizada (ecs. en variables de estado)

Estado=[phi; theta; omega_0; omega_1];
acels=solve(EcsMovimiento,a_q); %despejar aceleraciones
EcEstado4=simplify([omega_0;omega_1;acels.alpha_0;acels.alpha_1])

%
%