% dynamic model of PR planar robot under gravity
% using a Lagrangian formulation in symbolic form
%
% A. De Luca
% distributed on March 14, 2020; updated March 27, 2024

clear all
close all
clc

syms m1 m2 d I2xx I2yy I2zz real  %symbolic variables explicitly defined as real
syms q1 q2 dq1 dq2 ddq1 ddq2 u1 u2 g0 real

disp('**** dynamic model of PR planar robot in a vertical plane ****')
disp(' ')
disp('[press return to proceed at every pause]')

pause

disp(' ')
disp('*kinetic energy of link 1*')

T1=(1/2)*m1*dq1^2

pause

disp('*kinetic energy of link 2 - linear part*')

pc2=[q1+d*cos(q2) d*sin(q2) 0]'
vc2=diff(pc2,q1)*dq1+diff(pc2,q2)*dq2
T2c=simplify((1/2)*m2*vc2'*vc2)

pause

disp('*kinetic energy of link 2 - angular part*')

om2=[0 0 dq2]'
T2a=(1/2)*om2'*diag([I2xx I2yy I2zz])*om2

T2=T2c+T2a;
pause

disp('***robot kinetic energy***')

T=simplify(T1+T2)

pause

disp('*simplifying (by collecting squared terms)*')

T=collect(T,dq1^2);
T=collect(T,dq2^2)

pause

disp('***robot inertia matrix***')

M(1,1)=diff(T,dq1,2);
M(2,2)=diff(T,dq2,2);
TempB12=diff(T,dq1);
M(1,2)=diff(TempB12,dq2);
M(2,1)=M(1,2);
M=simplify(M)

pause

disp('*Christoffel matrices*')

q=[q1;q2];
M1=M(:,1);
C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1))
M2=M(:,2);
C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2))

pause

disp('***robot centrifugal terms (no Coriolis here!)***')

dq=[dq1;dq2];
c1=dq'*C1*dq;
c2=dq'*C2*dq;
c=[c1;c2]

pause 

disp('*potential energy of link 1*')

U1=0

pause

disp('*potential energy of link 2*')

g=[0;-g0;0];
    
U2=-m2*g'*pc2

pause

disp('***robot potential energy (due to gravity)***')

U=simplify(U1+U2)

pause

disp('***robot gravity term***')

G=jacobian(U,q)'

pause

disp('***complete dynamic equations***')

M*[ddq1; ddq2]+c+G==[u1 u2]'

disp('***end***')

% end
