%% Modelado de sistema bola-carrito con aceleración carrito forzada (reducción de dinámica, sistema reonómico)
% © 2021, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/cpforced.html
% Este código ejecutó sin errores en Matlab R2021a
% Objetivo: Modelar las ecuaciones dinámicas de cuarto orden de un sistema bola-carrito de 2 grados de libertad, si uno de los grados pasa a ser forzado a seguir un movimiento f(t) prefijado.
% Tabla de Contenidos
% Cinemática
% Preliminares: dinámica con 2GL (objetivo de otros vídeos)
%     Ecuaciones de la Dinámica mediante la formulación Euler-Lagrange
% Dinámica con posición x (+ velocidad, aceleración) del carro forzada
%     Método 1: fuerza/par calculado, reducción de la dinámica
%     Método 2: Euler-Lagrange con Lagrangiano explícitamente dependiente del tiempo (sistema reonómico)
%
%% Cinemática

syms x theta real %señales (funciones del tiempo)
syms L M m g b_x real %parámetros constantes del sistema
%L: distancia carro-bola
%M: masa carro
%m: masa bola
%g: cte. gravedad
%b_x: rozamiento deslizamiento x
pos_bola=[x+L*sin(theta); L*cos(theta)]; %theta=0, equilibrio superior (inestable), positivo en sentido horario

% Como los sistemas mecánicos suelen ser sistemas algebraico-diferenciales de "índice superior", debemos derivar dos veces las ecuaciones que relacionan las "posiciones" para obtener relaciones que involucren a velocidades y aceleraciones.

syms v_x v_theta a_x a_theta real %velocidades y aceleraciones
vel_bola=jacobian(pos_bola,[theta x])*[v_theta; v_x] %Regla de la cadena
%acel_bola=jacobian(vel_bola,[theta x v_theta v_x])*[v_theta; v_x; a_theta; a_x] %Regla de la cadena

%% Preliminares: dinámica con 2GL (objetivo de otros vídeos)
%% Ecuaciones de la Dinámica mediante la formulación Euler-Lagrange
% *No consideraremos inercia rotacional de la masa que cuelga del péndulo.

T=0.5*m*(vel_bola'*vel_bola) + 0.5*M*v_x^2 %E. cinética
V=m*g*pos_bola(2) %E. potencial
Lagrangiano=simplify(T-V);

% Estas son las ecuaciones Euler Lagrange: \frac{d}{dt}\left(\frac{\partial L}{\partial \dot q}\right)-\frac{\partial L}{\partial q} = Q
% Con Jacobian, haremos los 2 grados de libertad simultáneamente en Matlab.
% Denominaremos p:=\frac{\partial L}{\partial \dot q}

p=jacobian(Lagrangiano,[v_x;v_theta]); %momentum generalizado

% Fuerzas externas y rozamiento conforman el término Q no conservativo. Supondremos sólamente rozamiento carro-vía en el deslizamiento horizontal.

syms F real
Q=[F-b_x*v_x; 0];

% Con lo que las ecuaciones del movimiento finales quedan: \frac{dp}{dt}-\frac{\partial L}{\partial q} = Q

EcsMovimientoEL = ...
    jacobian(p,[x;theta; v_x;v_theta])*[v_x;v_theta; a_x;a_theta] ...
                    - jacobian(Lagrangiano,[x;theta])'==Q

% Despejamos aceleraciones para poder simular.

sol1=solve(EcsMovimientoEL,a_x,a_theta);
a_xEL=simplify(sol1.a_x)
a_thetaEL=simplify(sol1.a_theta)

%
%% Dinámica con posición x (+ velocidad, aceleración) del carro forzada
%% Método 1: fuerza/par calculado, reducción de la dinámica
% Si calculamos la fuerza necesaria para mover el carro con una cierta trayectoria

syms t real
caso_generico=false;
if caso_generico 
    syms x_forzada(t) %función del tiempo genérica
else
    x_forzada(t)=t^2+4*t;
end
v_x_forzada=diff(x_forzada) %no se hace jacobian, porque sólo depende de 1 vble (t).
a_x_forzada=diff(v_x_forzada)
solForzada=solve(EcsMovimientoEL,F,a_theta)
F_forzada=simplify(solForzada.F)
a_th_forzada=simplify(solForzada.a_theta)

% Y sustituriríamos las posiciones, velocidades y aceleraciones prefijadas:

subs(F_forzada,{v_x,a_x},{v_x_forzada,a_x_forzada})
subs(a_th_forzada,a_x,a_x_forzada)

% La idea de calcular fuerzas para que determinados elementos de un sistema mecánico tengan aceleraciones prefijadas se puede entender como solución de un problema de control; de hecho, en robótica, se denomina "método de par calculado" (porque los actuadores son motores que giran, pero la idea básica es la aquí presentada).
%
%%
%
%% Método 2: Euler-Lagrange con Lagrangiano explícitamente dependiente del tiempo (sistema reonómico)
% No es necesario calcular las ecuaciones 2GL y luego la F_forzada.
% Euler-Lagrange también funciona con un Lagrangiano dependiente del tiempo:

x=x_forzada %para poder copiar-pegar lo de arriba donde la pos. del carro era "x".

% La cinemática de posiciones y velocidades es:

pos_bola=[x+L*sin(theta); L*cos(theta)] %theta=0, equilibrio superior (inestable), positivo en sentido horario
vel_bola=jacobian(pos_bola,[theta t])*[v_theta; 1] %Regla de la cadena, derivada total.
assume(diff(x_forzada(t), t),'Real') % si no, Matlab se "raya" por si resultase ser un número complejo.

% La energía cinética (bueno, la que depende del grado de libertad)

T=0.5*m*(vel_bola'*vel_bola)% +0.5*M*diff(x)^2 %no hace falta, porque ese grado de libertad ya no es "libre".

% La energía potencial es:

%pos_bola(2) No está correcto
tmp=pos_bola(t);%necesario para que no se "raye" el symbolic toolbox, tmp ya no es una función, sino un vector.
V=m*g*tmp(2)
Lagr_forzado=simplify(T-V)

% Ahora ya hacemos Euler-Lagrange con el grado de libertad theta, como si nada hubiese cambiado:

p_forz=diff(Lagr_forzado,v_theta) %momentum generalizado
d_p_forz_dt = ...
   jacobian(p_forz,[theta v_theta t])*[v_theta; a_theta; 1];%Regla de la cadena, derivada total.

% Y estas son las ecuaciones Euler-Lagrange del movimiento forzado: \frac{dp}{dt}-\frac{\partial L}{\partial q} = Q

ecmov_forz= simplify(d_p_forz_dt-diff(Lagr_forzado,theta) ==0) %ecuación del movimiento en coordenada theta
a_th_forzEL=simplify(solve(ecmov_forz,a_theta)) 

% La ventaja de esta última manipulación es que sólo tenemos que manejar UNA única ecuación del movimiento, evitando tener que calcular las ecuaciones de todos los grados de libertad, y luego "las fuerzas que consiguen forzar una aceleración prefijada dada". También, como en todo Euler-Lagrange, podemos plantear el Lagrangiano sólamente con la cinemática de las velocidades. A cambio, no calculamos esas fuerzas de control que causarían el movimiento prefijado.