clear
%
%joint variables
syms qi
syms q1
syms q2
syms d3
syms q4
assume(qi,'real')
assume(q1,'real')
assume(q2,'real')
assume(d3,'real')
assume(q4,'real')
%link lengths
syms ai
syms a1
syms a2
assume(ai,'real')
assume(a1,'real')
assume(a2,'real')
%link offsets
syms di
syms d4
assume(di,'real')
assume(d4,'real')
%link twists
syms alphai
assume(alphai,'real')
%link offsets


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% MODELLING
%A matrices
A01i = [cos(qi) -sin(qi)*cos(alphai)  sin(qi)*sin(alphai) ai*cos(qi);...
       sin(qi)  cos(qi)*cos(alphai) -cos(qi)*sin(alphai) ai*sin(qi);...
          0     sin(alphai)          cos(alphai)         di;...
          0    0     0    1]
    
%option a: z2 pointing down
A01 = subs(A01i,{ai,di,alphai,qi},{ a1,    0,   0, q1})
A12 = subs(A01i,{ai,di,alphai,qi},{ a2,    0,  pi, q2})
A23 = subs(A01i,{ai,di,alphai,qi},{  0,   d3,  pi,  0})
A34 = subs(A01i,{ai,di,alphai,qi},{  0,   -d4,  pi, q4})

%optin b: z2 pointing up
A01b = subs(A01i,{ai,di,alphai,qi},{ a1,    0,   0, q1})
A12b = subs(A01i,{ai,di,alphai,qi},{ a2,    0,   0, q2})
A23b = subs(A01i,{ai,di,alphai,qi},{  0,   d3,   0,  0})
A34b = subs(A01i,{ai,di,alphai,qi},{  0,  -d4,  pi, q4})

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% DIRECT KINEMATICS
A04  = A01*A12*A23*A34
A04b = A01b*A12b*A23b*A34b

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% GEOMETRIC JACOBIAN
%compute the origins and z vectors of the frames
%done with option a
origin = [0 0 0 1]'
p0 = origin
p1 = A01*origin
p2 = A01*A12*origin
p3 = A01*A12*A23*origin
p4 = A01*A12*A23*A34*origin
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

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   [0 0 0]'   z3 ]
        
J = simplify(J)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ANALYTIC JACOBIAN uisng rpy angles

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


dPexdq1 = diff(Pe(1),q1);
dPexdq2 = diff(Pe(1),q2);
dPexdd3 = diff(Pe(1),d3);
dPexdq4 = diff(Pe(1),q4);
dPeydq1 = diff(Pe(2),q1);
dPeydq2 = diff(Pe(2),q2);
dPeydd3 = diff(Pe(2),d3);
dPeydq4 = diff(Pe(2),q4);
dPezdq1 = diff(Pe(3),q1);
dPezdq2 = diff(Pe(3),q2);
dPezdd3 = diff(Pe(3),d3);
dPezdq4 = diff(Pe(3),q4);
Jp = [dPexdq1 dPexdq2 dPexdd3 dPexdq4;...
      dPeydq1 dPeydq2 dPeydd3 dPeydq4;...
      dPezdq1 dPezdq2 dPezdd3 dPezdq4]
Jp = simplify(Jp)


dphidq1 = diff(phi,q1);
dphidq2 = diff(phi,q2);
dphidd3 = diff(phi,d3);
dphidq4 = diff(phi,q4);
dthetadq1 = diff(theta,q1);
dthetadq2 = diff(theta,q2);
dthetadd3 = diff(theta,d3);
dthetadq4 = diff(theta,q4);
dpsidq1= diff(psi,q1);
dpsidq2 = diff(psi,q2);
dpsidd3 = diff(psi,d3);
dpsidq4 = diff(psi,q4);
JPhiRPY = [dphidq1 dphidq2 dphidd3 dphidq4;...
      dthetadq1 dthetadq2 dthetadd3 dthetadq4;...
      dpsidq1 dpsidq2 dpsidd3 dpsidq4]
  
JArpy = simplify([Jp;JPhiRPY])

%The T matrix relating omega and the derivative of the ZYZ Euler angles
%[ cos(psi)*cos(theta), -sin(psi), 0]
%[ cos(theta)*sin(psi),  cos(psi), 0]
%[         -sin(theta),         0, 1]
%This matrix relates J(3:6,:) and JPHi

Trpy = [cos(psi)*cos(theta) -sin(psi) 0; cos(theta)*sin(psi) cos(psi) 0; -sin(theta) 0 1]
Jang_1 = J(4:6,:)
Jang_2RPY = double(simplify(Trpy*JPhiRPY))



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ANALYTIC JACOBIAN using zyz Euler angles

Pe = A04(:,4)
Phie = A04(1:3,1:3)
phi = 0
theta = pi
psi = atan2(Phie(2,1),Phie(2,2))
%[-c_psi s_psi o]
%[s_psi  c_psi 0]
%[0       0   -1] This is the ZYZ Phie for phi=0,theta=pi

dphidq1 = diff(phi,q1);
dphidq2 = diff(phi,q2);
dphidd3 = diff(phi,d3);
dphidq4 = diff(phi,q4);
dthetadq1 = diff(theta,q1);
dthetadq2 = diff(theta,q2);
dthetadd3 = diff(theta,d3);
dthetadq4 = diff(theta,q4);
dpsidq1= diff(psi,q1);
dpsidq2 = diff(psi,q2);
dpsidd3 = diff(psi,d3);
dpsidq4 = diff(psi,q4);
JPhi = [dphidq1 dphidq2 dphidd3 dphidq4;...
      dthetadq1 dthetadq2 dthetadd3 dthetadq4;...
      dpsidq1 dpsidq2 dpsidd3 dpsidq4]
  
JA = simplify([Jp;JPhi])

%The T matrix relating omega and the derivative of the ZYZ Euler angles is:
%[ 0, -sin(phi), cos(phi)*sin(theta)]
%[ 0,  cos(phi), sin(phi)*sin(theta)]
%[ 1,         0,          cos(theta)]
%when phi=0 and theta=pi becomes:
%[0 0  0]
%[0 1  0]
%[1 0 -1]
%This matrix relates J(3:6,:) and JPHi

T = [0 -sin(phi) cos(phi)*sin(theta); 0 cos(phi) sin(phi)*sin(theta); 1 0 cos(theta)]
Jang_1 = J(4:6,:)
Jang_2 = double(simplify(T*JPhi))


