%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% DEFINITION OF SYMBOLIC VARIABLES
clear
%
%joint variables
syms qi
syms q1
syms q2
assume(qi,'real')
assume(q1,'real')
assume(q2,'real')
%link lengths
syms ai
syms sa1
syms sa2
assume(ai,'real')
assume(sa1,'real')
assume(sa2,'real')
%link offsets
syms di
assume(di,'real')
%link twists
syms alphai
assume(alphai,'real')

%COM    
syms l1
syms l2
assume(l1,'real')
assume(l2,'real')
%masses
syms m1
syms m2
assume(m1,'real')
assume(m2,'real')
%Inertia
syms Ixx1
syms Iyy1
syms Izz1
syms Ixy1
syms Ixz1
syms Iyz1
assume(Ixx1,'real')
assume(Iyy1,'real')
assume(Izz1,'real')
assume(Ixy1,'real')
assume(Ixz1,'real')
assume(Iyz1,'real')
syms Ixx2
syms Iyy2
syms Izz2
syms Ixy2
syms Ixz2
syms Iyz2
Il11 = [Ixx1 -Ixy1 -Ixz1; -Ixy1 Iyy1 -Iyz1; -Ixz1 -Iyz1 Izz1]
Il22 = [Ixx2 -Ixy2 -Ixz2; -Ixy2 Iyy2 -Iyz2; -Ixz2 -Iyz2 Izz2]
assume(Ixx2,'real')
assume(Iyy2,'real')
assume(Izz2,'real')
assume(Ixy2,'real')
assume(Ixz2,'real')
assume(Iyz2,'real')
%inertia matrix
syms b11
syms b12
syms b21
syms b22
assume(b11,'real')
assume(b12,'real')
assume(b21,'real')
%velocities, accelerations, gravity and torques
syms qdot1
syms qdot2
syms qdotdot1
syms qdotdot2
assume(qdot1,'real')
assume(qdot2,'real')
assume(qdotdot1,'real')
assume(qdotdot2,'real')
syms g
assume(g, 'real')
syms tau1
syms tau2
assume(tau1,'real')
assume(tau2,'real')
%friction
syms Bm1 
syms Bm2
assume(Bm1, 'real')
assume(Bm2, 'real')
%gear ratios
syms Gear1
syms Gear2
assume(Gear1, 'real')
assume(Gear2, 'real')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 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]
    
A01 = subs(A01i,{ai,di,alphai,qi},{ sa1, 0, 0, q1})
A12 = subs(A01i,{ai,di,alphai,qi},{ sa2, 0, 0, q2})

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% DIRECT KINEMATICS
A02  = A01*A12


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% JACOBIAN
%compute the origins and z vectors of the frames
origin = [0 0 0 1]'
p0 = origin
p1 = A01*origin
p2 = A01*A12*origin
z0 = [0 0 1]';
z1 = A01(1:3,1:3)*z0

J = [ cross(z0,p2(1:3)-p0(1:3)), cross(z1,p2(1:3)-p1(1:3));...
            z0   z1   ]
        
J = simplify(J)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% THE SERIAL_LINK
%load the predefines two-link manipulator in the xz plane
%mdl_twolink

%define the link lengths
vsa1 = 1
vsa2 = 1
%define the masses 
vm1 = 1
vm2 = 1
%define the inertia
vIzz1 = (1/12)*vm1*vsa1^2
vIzz2 = (1/12)*vm2*vsa2^2
%define the friction coefficient
vBm1 = 0.001
vBm2 = 0.001
%define the gear ratio 
vGear1 = 100
vGear2 = 100

%Define the links
L(1) = Link([0 0 vsa1 0]);
L(2) = Link([0 0 vsa2 0]);
%Create the serial link
twolink = SerialLink(L, 'name', 'two link')

%set link masses
twolink.links(1).m = vm1;
twolink.links(2).m = vm2;
%set CoM
twolink.links(1).r = [-vsa1/2, 0, 0];
twolink.links(2).r = [-vsa2/2, 0, 0];
%set link inertias 
twolink.links(1).I = zeros(3)
twolink.links(2).I = zeros(3)
twolink.links(1).I(3,3) = vIzz1
twolink.links(2).I(3,3) = vIzz2
%set the motor inertias
twolink.links(1).Jm = 0
twolink.links(2).Jm = 0
%set the gear ratios
twolink.links(1).G = vGear1
twolink.links(2).G = vGear2
%set the friction
twolink.links(1).B = vBm1 %Motor viscous friction is scaled up by G^2
twolink.links(2).B = vBm2
%set the gravity
twolink.gravity = [0 9.81 0]