%% Examples in the Robotics Toolbox
% MAE263B _ Dynamics of Robotics
% Discussion Section Date: 01.24.2020.
% by Seungmin Jung
%

clear all; clc; close all;
syms l1 l2 t1 t2 t3

%
%% Modified DH parameter

fprintf('* ========== Modified DH parameter ========== *')
syms l1 l2  % Create symbolic variables and functions

L1 = Link('revolute','d',0,'a',0,'alpha',0,'modified')
%: cannot specify 'theta' for revolute link
%: cannot specify 'd' for primatic link
L1_2 = Link([0 0 0 0],'modified')
%: Link([theta, d, a, alpha, offset],'modified/standard');
 
L2 = Link('revolute','d',0,'a',l1,'alpha',0,'modified')

L3 = Link('d',0,'a',l2,'alpha',0,'modified') %default: revolute
L3_2 = Link('revolute','d',0,'a',l2,'alpha',0,'modified')
L3_p = Link('prismatic','a',l2,'alpha',0,'theta', 0,'modified') 

Robot_Mod_DH = SerialLink([L1 L2 L3], 'name', '2D-RR Modified DH parameter')

%
%% Standard DH parameter

fprintf('* ========== Standard DH parameter ========== *')
L_S(1) = Link('revolute','d',0,'a',l1,'alpha',0,'standard')
L_S(2)= Link('revolute','d',0,'a',l2,'alpha',0,'standard')

Robot_Stand_DH = SerialLink(L_S, 'name', '2D-RR Standard DH parameter')
% Robot_Standard_DH = SerialLink([L1 L2], 'name', '2D-RR-Standard')
% L_S = [L1, L2] <==  L_S(1)=L1, L_S(2)=L2

%
%% Comparison - Modified vs Standard DH parameter
% Comparision of results

fprintf('* ========== Comparison of DH parameter ========== *')
Robot_Mod_DH
Robot_Stand_DH

%
%
%% Forward Kinematics
% Robotname.fkine()

fprintf('* ========== Foward Kinematics ========== *')
t3 = 0;
th_Mod = [t1 t2 t3] 
th_DH = [t1 t2]

fprintf('*** Foward Kinematics from Modified DH parameter')
Mod_DH_FK = Robot_Mod_DH.fkine(th_Mod)

fprintf('*** Foward Kinematics from Standard DH parameter')
Stand_DH_FK = Robot_Stand_DH.fkine(th_DH)

%
%% Homogeneous Transformation Matrix
% Robotname.A()

fprintf('* ========== Homogeneous Transformation Matrix ========== *')
t3 = 0;
th_Mod = [t1 t2 t3] %t3=0
th_Stand = [t1 t2]

fprintf('* T0_1_Mod')
T0_1_Mod = Robot_Mod_DH.A([1],th_Mod)
fprintf('* T0_1_Stand')
T0_1_Stand = Robot_Stand_DH.A([1],th_Stand)


fprintf('* T1_2_Mod')
T1_2_Mod = Robot_Mod_DH.A([2],th_Mod)
fprintf('* T1_2_Stand')
T1_2_Stand = Robot_Stand_DH.A([2],th_Stand)
fprintf('* T2_3_Mod')
T2_3_Mod = Robot_Mod_DH.A([3],th_Mod)

fprintf('* T0_2_Mod')
T0_2_Mod = simplify(Robot_Mod_DH.A([1 2],th_Mod))
fprintf('* T0_2_Stand')
T0_2_Stand = Robot_Stand_DH.A([1 2],th_Stand) ;
sim_T0_2_Stand = simplify(T0_2_Stand)
fprintf('* T0_3_Mod')
T0_3_Mod = simplify(Robot_Mod_DH.A([1 2 3],th_Mod))

%
%% Plot robots
% Robotname.plot()

fprintf('* ========== Plot robots ========== *')
l1 = 0.1 
l2 = 0.1  
l2_2 = 0.3

L(1) = Link('revolute','d',0,'a',0,'alpha',0,'modified') ;
L(2) = Link('revolute','d',0,'a',l1,'alpha',0,'modified');
L(3) = Link('revolute','d',0,'a',l2,'alpha',0,'modified');

Robot_1 = SerialLink(L, 'name', '2D-RR Robot - configuration 1');
Robot_2 = SerialLink(L, 'name', '2D-RR Robot - configuration 2');

L2(1) = L(1); L2(2) = L(2);
L2(3) = Link('revolute','d',0,'a',l2_2,'alpha',0,'modified');
Robot_3 = SerialLink(L2, 'name', '2D-RR Roboot - link value- change')

figure()
Robot_1.plot([0 0 0]) %[t1 t2 t3]
figure()
Robot_2.plot([pi/2 pi/2 0])
figure()
Robot_3.plot([0 0 0])

%
%% Simulation - Animation

l1 = 0.1; 
l2 = 0.1; %0.3 
L(1) = Link('revolute','d',0,'a',0,'alpha',0,'modified');
L(2) = Link('revolute','d',0,'a',l1,'alpha',0,'modified');
L(3) = Link('revolute','d',0,'a',l2,'alpha',0,'modified');

figure()
Robot = SerialLink(L, 'name', '2D-RR Robot Animation')

time = [0 5 10 15 20];  
t1 = [0 pi/10 pi/8 pi/4 pi/2];
t2 = [0 pi/10 pi/6 pi/4 pi/2];

figure()
subplot(2,1,1)
plot(time,t1); xlabel('time'); ylabel('theta1');
subplot(2,1,2)
plot(time,t2); xlabel('time'); ylabel('theta2');

figure()
for i = 1:5
    Robot.plot([t1(i) t2(i) 0])
end


%
%% Useful functions in Robotics Toolbox and MATLAB
% Useful functions in Robotics Toolbox: tr2, .t :Extract the rotatioinal matrix and the position matrix

%t2r : to extract the rotation matrix from the transformation matrix 
%.t : to extract the translation matrix from the transformation matrix 
fprintf('* ========== Examples ========== *')
syms l1 l2 t1 t2 c1 c2

L(1) = Link('revolute','d',0,'a',0,'alpha',0,'modified');
L(2) = Link('revolute','d',0,'a',l1,'alpha',0,'modified');
L(3) = Link('revolute','d',0,'a',l2,'alpha',0,'modified');
Robot = SerialLink(L, 'name', '2D-RR Robot1')

fprintf('* Tranformation matrix T')
T = Robot.fkine([t1 t2 0])
Rot_m = t2r(T)
tlans_m = T.t

% Useful functions in MATLAB: simplify , collect, equationToMatrix, Transpose, cross

fprintf('* T0_3_Mod')
T0_3_Mod = Robot.A([1 2 3],th_Mod)
fprintf('* T0_3_Mod')
sim_T0_3_Mod = simplify(Robot_Mod_DH.A([1 2 3],th_Mod))

equation = c1*t1 + c2*t2 + c2*t1 + c2*t2
collect = collect(equation,[t1 t2])
e2m = equationsToMatrix(equation,[t1 t2])

% Review with exam examples
%% Review with Puma 560 example
% DH parameter
% Foward Kinematics
% Simulation