% SCARA ROBOT

%% DH PARAMETERS
clear all;
%Parameters
syms L1
assume(L1,'real')
syms L2
assume(L2,'real')
syms L3
assume(L3,'real')
syms L4
assume(L4,'real')
syms L5
assume(L5,'real')
L1=0.322;
L2=0.75;
L3=0.75;
L4=0.50;
L5=0.26;

%DH Parameters using serial link function
L(1)=Link([0 L1 0 0 0 ],'modified');
L(2)=Link([0 0 L2 0 0 ],'modified');
L(3)=Link([0 0 L3 0 1 ],'modified');% Como poner -d3
L(4)=Link([0 -(L4+L5) 0  0 0 ],'modified');
L(3).jointtype='P';
SCARA_robot = SerialLink(L, 'name', 'SCARA')
figure,SCARA_robot.plot([pi,pi/2,0.5,pi/2],'workspace', [-3,3,-3,3,-2,2]); %Da error al dibujar



%% FORDWARD KINEMATICS
syms q1
assume(q1,'real')
syms q2
assume(q2,'real')
syms q3
assume(q3,'real')
syms q4
assume(q4,'real')
% q1=0;
% q2=0;
% q3=0;
% q4=0;
A01=[cos(q1) -sin(q1) 0 0; sin(q1) cos(q1) 0 0; 0 0 1 L1;0 0 0 1];
A12=[cos(q2) -sin(q2) 0 L2; sin(q2) cos(q2) 0 0; 0 0 1  0; 0 0 0 1];
A23=[cos(0) 0 0 L3; 0 1 0 0; 0 0 1 q3; 0 0 0 1];
A34=[cos(q4) -sin(q4) 0 0; sin(q4) cos(q4) 0  0; 0 0 1 -(L4+L5); 0 0 0 1];
A4Tool=[1 0 0 0; 0 cos(pi) 0 0; 0 0 cos(pi) 0; 0 0 0 1];

A04=A01*A12*A23*A34;
A0Tool=A04*A4Tool;
%% GEOMETRICAL JACOBIAN

p0 = [0 0 0 1]';
p1 = A01*p0;
p2 = A01*A12*p0;
p3= A01*A12*A23*p0;
p4= A01*A12*A23*A34*p0;
z0 = [0 0 1]';
z1 = A01(1:3,1:3)*z0;
z2 = A01(1:3,1:3)*A12(1:3,1:3)*z0;
z3=  A01(1:3,1:3)*A12(1:3,1:3)*A23(1:3,1:3)*z0;
z=[0;0;0];

J = [ cross(z0,p4(1:3)-p0(1:3)),  cross(z1,p4(1:3)-p1(1:3)),    z2,        cross(z3,p4(1:3)-p3(1:3));
            z0                                z1                z                            z3]
        
J = simplify(J)

pe = p4
z4=  A01(1:3,1:3)*A12(1:3,1:3)*A23(1:3,1:3)*A34(1:3,1:3)*z0;

J2 = [ cross(z1,pe(1:3)-p1(1:3)),  cross(z2,pe(1:3)-p2(1:3)),    z3,        cross(z4,pe(1:3)-p4(1:3));
            z1                                z2                z                            z4]
        
J2 = simplify(J2)
% SCARA_robot.jacob0([0 0 0 0]);
% Analytical Jacobian

Pe = A04(:,4);
Phie = A04(1:4,1:4);
phi = atan2(Phie(2,1),Phie(1,1)); 
phi=simplify(phi);
phi=q1+q2+q4;
theta = atan2(-Phie(3,1),sqrt(Phie(3,2)^2+Phie(3,3)^2));
theta=simplify(theta);
psi = atan2(Phie(3,2), Phie(3,3));
psi=simplify(psi);

dPexdq1 = diff(Pe(1),q1);
dPexdq2 = diff(Pe(1),q2);
dPexdq3 = diff(Pe(1),q3);
dPexdq4 = diff(Pe(1),q4);
dPeydq1 = diff(Pe(2),q1);
dPeydq2 = diff(Pe(2),q2);
dPeydq3 = diff(Pe(2),q3);
dPeydq4 = diff(Pe(2),q4);
dPezdq1 = diff(Pe(3),q1);
dPezdq2 = diff(Pe(3),q2);
dPezdq3 = diff(Pe(3),q3);
dPezdq4 = diff(Pe(3),q4);
Jp = [dPexdq1 dPexdq2 dPexdq3 dPexdq4;
      dPeydq1 dPeydq2 dPeydq3 dPeydq4;
      dPezdq1 dPezdq2 dPezdq3 dPezdq4];
%Jp = simplify(Jp);


dphidq1 = diff(phi,q1);
dphidq2 = diff(phi,q2);
dphidq3 = diff(phi,q3);
dphidq4 = diff(phi,q4);
dthetadq1 = diff(theta,q1);
dthetadq2 = diff(theta,q2);
dthetadq3 = diff(theta,q3);
dthetadq4 = diff(theta,q4);
dpsidq1 = diff(psi,q1);
dpsidq2 = diff(psi,q2);
dpsidq3 = diff(psi,q3);
dpsidq4 = diff(psi,q4);
JPhi = [dphidq1 dphidq2 dphidq3 dphidq4;
      dthetadq1 dthetadq2 dthetadq3 dthetadq4;
       dpsidq1 dpsidq2 dpsidq3 dpsidq4];


JA = [Jp;JPhi];
%% Function of matrix TA
TA = rpy2jac(phi,theta,psi,'xyz');

%% Function T
T = [eye(3,3) zeros(3,3); zeros(3,3) TA];

%% From analytical to Jacobian
Jgeo1 = T*JA;
JgeoZYX=simplify(Jgeo1)
%% ZYZ
Pe = A04(:,4);
Phie = A04(1:4,1:4);
phi = 0; 
theta =0;
psi = atan2(Phie(1,1), Phie(2,1));
psi=simplify(psi);
psi= q1+q2+q4;

dPexdq1 = diff(Pe(1),q1);
dPexdq2 = diff(Pe(1),q2);
dPexdq3 = diff(Pe(1),q3);
dPexdq4 = diff(Pe(1),q4);
dPeydq1 = diff(Pe(2),q1);
dPeydq2 = diff(Pe(2),q2);
dPeydq3 = diff(Pe(2),q3);
dPeydq4 = diff(Pe(2),q4);
dPezdq1 = diff(Pe(3),q1);
dPezdq2 = diff(Pe(3),q2);
dPezdq3 = diff(Pe(3),q3);
dPezdq4 = diff(Pe(3),q4);
Jp = [dPexdq1 dPexdq2 dPexdq3 dPexdq4;
      dPeydq1 dPeydq2 dPeydq3 dPeydq4;
      dPezdq1 dPezdq2 dPezdq3 dPezdq4];
%Jp = simplify(Jp);


dphidq1 = diff(phi,q1);
dphidq2 = diff(phi,q2);
dphidq3 = diff(phi,q3);
dphidq4 = diff(phi,q4);
dthetadq1 = diff(theta,q1);
dthetadq2 = diff(theta,q2);
dthetadq3 = diff(theta,q3);
dthetadq4 = diff(theta,q4);
dpsidq1 = diff(psi,q1);
dpsidq2 = diff(psi,q2);
dpsidq3 = diff(psi,q3);
dpsidq4 = diff(psi,q4);
JPhi = [dphidq1 dphidq2 dphidq3 dphidq4;
      dthetadq1 dthetadq2 dthetadq3 dthetadq4;
       dpsidq1 dpsidq2 dpsidq3 dpsidq4];


JA = [Jp;JPhi];
%% Function of matrix TA
TA = eul2jac(phi,theta,psi);

%% Function T
T = [eye(3,3) zeros(3,3); zeros(3,3) TA];

%% From analytical to Jacobian
Jgeo = T*JA;
JgeoZYZ=simplify(Jgeo)