%% Modelling of carousel dynamics (2 DoF, Euler Lagrange): SIMULATION and ANIMATION
% © 2024, Antonio Sala Piqueras, Universitat Politècnica de València. All rights reserved.
% This code worked with Matlab R2023a
%
% *Image from jana_lsb0 at Pixabay , Pixabay License 2022 (free even for commercial use).
% Presentations in video:
% https://personales.upv.es/asala/YT/V/tiovELEN.html
% https://personales.upv.es/asala/YT/V/tiovEL2EN.html
% https://personales.upv.es/asala/YT/V/tiovELsimEN.html
%
% Objectives: Understand the physical modeling of a carousel like the one in the figure, and its simulation. Specifically, we will address the case of 2 degrees of freedom with Euler-Lagrange equations of dynamics.
%
% Table of Contents
% Kinematics
% Energies and Euler-Lagrange equations of motion
%     Particular cases
%         Zero suspended length
%         R0=0, I0=0, tau=0: spherical pendulum
%         Lateral oscillations with forced angular rotation speed of the platform
%     Normalised state-space equation
%

syms phi theta real %degrees of freedom to describe motion
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) %we help symbolic toolbox

%% Kinematics

R=R_0+L*sin(theta); %Distance from mass to central axis of rotation
x=R*cos(phi);
y=R*sin(phi);
z=L*(1-cos(theta)); %z=0 is downwards equilibrium point (when not rotating)
r=[x;y;z]
q=[phi; theta];
syms omega_0 omega_1 real %angular velocities dot_phi dot_theta
syms alpha_0 alpha_1 real %angular accelerations ddot_phi ddot_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

% If we wished to express the suspended mass acceleration as a function of position, velocity and angular accelerations, it would come out:
% 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

% *Note: this acceleration is NOT needed in the Euler-Lagrange methodology.
%%
%% Energies and Euler-Lagrange equations of motion

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;

% Let us define p(q,\dot q)=\frac{\partial L}{\partial \dot q}:

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

% Euler-Lagrange equations of motion for each coordinate q_i are:
% \frac{dp_i}{dt}-\frac{\partial L}{\partial q_i}=\tau_i
% where, of course, we may need chain rule:
% \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 %torque in carousel top
%we arrange in "column" form each equation of motion
EcsMotion = (dpdt - jacobian(Lagrangian,q)'==[tau;0]) %Euler-Lagrange

% We can rewrite as 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 %top platform
% ML^2\cdot \alpha_1=-MgL\sin\theta + M\omega_0^2(R_0+L\sin\theta)L\cos\theta     %suspended mass

MassMatrix = simplify(jacobian(lhs(EcsMotion),a_q))

%%
%%
%% Particular cases
%%
%% Zero suspended length

simplify(subs(EcsMotion,L,0))

% We have a 1 DoF model with equivalent moment of inertia I_0+MR_0^2.
%%
%%
%%
%% R0=0, I0=0, tau=0: spherical pendulum

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

%%
%
%% Lateral oscillations with forced angular rotation speed of the platform
% We would need to look just at the second equation of motion:ML^2\alpha_1=-MgL\sin\theta + M\omega_0^2(R_0+L\sin\theta)L\cos\theta,
% We have a torque caused by weight and by centrifugal force.
% Fixing \omega_0(t) to any desired trajectory, the first equation of motion, replacing \alpha_0=\frac{d\omega_0}{dt}, would give the motor torque needed to keep the desired velocity profile.
% Variations of the above idea are behind the so-called "computed torque" methodologies in robotics.
%
%% Normalised state-space equation

State=[phi; theta; omega_0; omega_1];
accels=solve(EcsMotion,a_q);
EcEstado4=simplify([omega_0;omega_1;accels.alpha_0;accels.alpha_1])

%