% TOGAC42_AMBF_FullSymbolicFKderiv.m

% This file was used to calculate the forward kinematics and jacobian
% symbolically for the galen robot in ambf simulation. These results
% closely follow the deriviation of the delta platform forward kinematics
% for from TOGAC40_D_DeltaKin.PDF

% There necessary offsets, leg lengths, and other details are listed here
% as needed to make the solution for the delta platform kinematics possible
% while maintaining every other symbolic variable to get from the AMBF
% world origin to the tip of the Galen end effector tool point.


% Created by Brevin Banks
% Modified 4/25/2023

clear all

% The measured base location under the mobile platform relative to the AMBF
% world

%% The pseudoBase is the point in the center of the galen robot platform
syms pb11 pb12 pb13	pb14 pb21 pb22 pb23 pb24 pb31 pb32 pb33	pb34 real

pseudoBase = [pb11	pb12	pb13	pb14;
              pb21	pb22	pb23	pb24;
              pb31	pb32	pb33	pb34;
              0	0	0	1];


syms Offset l1 l2 l3 B1 B2 B3 UB1 UB2 UB3 SB12 SB13 SB23 WB12 WB13 WB23 P1 P2 P3 wP uP SP real


Offset = 0.290099055335036; % 0 z position offset for all 3 carriage joints
l1 = 0.466305825292883; % Length of carriage1 arm to mobile platform center
l2 = 0.458665154015044; % Length of carriage2 arm to mobile platform center
l3 = 0.458655703593590; % Length of carriage3 arm to mobile platform center
B1 = [-0.225938839596818;4.69421295953835e-07;0]; % length from pseudo base center to carriage 1 stand
B2 = [0.0965600507381253;-0.186160042922294;0]; % length from pseudo base center to carriage 2 stand
B3 = [0.0965611179715781;0.186158682400045;0]; % length from pseudo base center to carriage 3 stand
UB1 = norm(B1); % distance from pseudo base center to carriage 1 stand
UB2 = norm(B2); % distance from pseudo base center to carriage 2 stand
UB3 = norm(B3); % distance from pseudo base center to carriage 3 stand
SB12 = norm(B1-B2); % PseudoBase triangle edge from carraige 1 stand to stand 2
SB13 = norm(B1-B3); % PseudoBase triangle edge from carraige 1 stand to stand 3
SB23 = norm(B2-B3); % PseudoBase triangle edge from carraige 3 stand to stand 2
WB12 = UB2*sin(acos(((UB1^2)-(SB12^2)-(UB2^2))/(2*SB12*UB2))); % Length normal to pseudobase triangle edge from pseudobase origin for edge 1-2
WB13 = UB3*sin(acos(((UB1^2)-(SB13^2)-(UB3^2))/(2*SB13*UB3))); % Length normal to pseudobase triangle edge from pseudobase origin for edge 1-3
WB23 = UB3*sin(acos(((UB2^2)-(SB23^2)-(UB3^2))/(2*SB23*UB3))); % Length normal to pseudobase triangle edge from pseudobase origin for edge 2-3
P1 = [0;0;0]; % Mobile Platform triangle dimensions relative to self. Assumning single point, no triangle
P2 = [0;0;0]; % Mobile Platform triangle dimensions relative to self. Assumning single point, no triangle
P3 = [0;0;0]; % Mobile Platform triangle dimensions relative to self. Assumning single point, no triangle
wP = 0; % Mobile platform triangle traits set to zero for single point
uP = 0; % Mobile platform triangle traits set to zero for single point
SP = 0; % Mobile platform triangle traits set to zero for single point

% These equations follow the work in the TOGAC40_D_DeltaKin.pdf for the delta
% platform
syms x y z L1 L2 L3 real 
assume(L1,'positive')
assume(L2,'positive')
assume(L3,'positive')

B_B_1 = B1;
B_B_2 = B2;
B_B_3 = B3;
B_L_1 = [0;0;L1];
B_L_2 = [0;0;L2];
B_L_3 = [0;0;L3];
B_P_P = [x;y;z];
P_P_1 = [0;0;0];
P_P_2 = [0;0;0];
P_P_3 = [0;0;0];


l_1 = B_P_P + P_P_1 - B_B_1 - B_L_1;
l_2 = B_P_P + P_P_2 - B_B_2 - B_L_2;
l_3 = B_P_P + P_P_3 - B_B_3 - B_L_3;


eq1 = l_1(1)^2 + l_1(2)^2 + l_1(3)^2 - l1^2  == 0;
eq2 = l_2(1)^2 + l_2(2)^2 + l_2(3)^2 - l2^2  == 0;
eq3 = l_3(1)^2 + l_3(2)^2 + l_3(3)^2 - l3^2  == 0;


X = eval(simplify(solve(eq2-eq3,x)));
fprintf('X Simplified \n')
Y = eval(simplify(solve(subs(eq2-eq1,x,X),y)));
fprintf('Y Simplified \n')
Z = eval(simplify(vpa(solve(subs(subs(eq1,x,X),y,Y),z))));
fprintf('Z Simplified \n')

Z1 = Z(1);
Z2 = eval(simplify(Z(2)));
fprintf('Z2 Simplified \n')
Y1 = subs(Y,z,Z1);
Y2 = eval(simplify(subs(Y,z,Z2)));
fprintf('Y2 Simplified \n')
X1 = subs(subs(X,y,Y1),z,Z1);
X2 = eval(simplify(subs(subs(X,y,Y2),z,Z2)));
fprintf('X2 Simplified \n')



syms theta1 theta2 theta3 theta4 theta5 real
In1 = theta1;
In2 = theta2;
In3 = theta3;



Z1 = subs(Z1,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
Z2 = eval(simplify(subs(Z2,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset})));
fprintf('Z2 ReSimplified \n')
X1 = subs(X1,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
X2 = eval(simplify(subs(X2,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset})));
fprintf('X2 ReSimplified \n')
Y1 = subs(Y1,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
Y2 = eval(simplify(subs(Y2,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset})));
fprintf('Y2 ReSimplified \n')

% We solved for the forward kinematics of the delta platform for the X Y
% and Z coordinates. We assume the platform stays parallel to the base so
% no rotation is present. Note that we used Z2 in our final result because
% Z2 give us the robot conformation we most desire in our configuration

% From the pseudobase to the Platform of the robot
pseudoBase_F_P = [1, 0, 0, X2;
             0, 1, 0, Y2;
            0, 0, 1, Z2;
             0, 0, 0, 1];

% From the AMBF word frame to the platform of the robot
W_F_P = eval(simplify(pseudoBase*Frotz(-pi/2)*vpa(pseudoBase_F_P)*Frotz(pi/2)));
fprintf('W_F_P Simplified \n')

syms MpRABx MpRABy MpRABz real
MobplattoRABxyz=[MpRABx;
                MpRABy;
                MpRABz];

% From the World to the Roll joint of the robot
W_F_R = eval(simplify(W_F_P*Ftranz(MobplattoRABxyz(3))*Frotx(pi/2)*Ftranz(MobplattoRABxyz(1))*Frotz(theta4)));
fprintf('W_F_R Simplified \n')
syms RABTDLFx RABTDLFy RABTDLFz real
RABtoTDLFSxyz = [RABTDLFx;
                RABTDLFy;
                RABTDLFz];

% From the World to the tilt joint of the robot
W_F_T = eval(simplify(W_F_R*Ftranz(RABtoTDLFSxyz(1))*Frotz(-pi/2)*Ftranx(-RABtoTDLFSxyz(3))*Frotx(-pi/2)*Frotz(0.069414985474804+(pi/2))*Frotz(theta5)));
fprintf('W_F_T Simplified \n')
syms TDLFE35x TDLFE35y TDLFE35z real
TDLFStoEndo35xyz = [TDLFE35x;
                    TDLFE35y;
                    TDLFE35z];

% From the World to the endeffector of the robot
syms E35zoff E35xoff real
W_F_E35 = eval(simplify(W_F_T*Frotz(E35zoff)*Ftranx(-TDLFStoEndo35xyz(1))*Frotz(pi/2)*Ftranx(TDLFStoEndo35xyz(3))*Froty(-pi/2)*Frotx(E35xoff)));
fprintf('W_F_E35 Simplified \n')

syms THpx THpy THpz E35Hpx E35Hpy E35Hpz real
TipHomeposition = [THpx;
                    THpy;
                    THpz];
E35Homepos = [E35Hpx;
    E35Hpy;
    E35Hpz];

Endo35toEndoTipxyz = TipHomeposition-E35Homepos;

% From the World to the end of the tool of the robot
syms Exoff real

W_F_E = eval(simplify(W_F_E35*Frotx(-E35xoff)*Ftranz(-Endo35toEndoTipxyz(3))*Ftrany(Endo35toEndoTipxyz(1))*Frotx(Exoff)));
fprintf('W_F_E Simplified \n')

%% Find the jacobian
W_F_E_inv = ((FINV(W_F_E)));
fprintf('W_F_E_inv Simplified \n')
q = [theta1,theta2,theta3,theta4,theta5];
Jacobian = sym(zeros(6,5));
for k = 1:length(q)
Jac = diff(W_F_E,q(k))*W_F_E_inv;

Jacobian(:,k) = unskew_jac(Jac);
end
fprintf('Completed \n')
%% output the respective FK transformation matricies

W_F_P
W_F_R
W_F_T
W_F_E35
Galen_fk = W_F_E

%% Output the simplified jacobian
Galen_Jacobian = Jacobian

J = Galen_Jacobian;

J11 = vpa(eval(simplify(J(1,1))))
J12 = vpa(eval(J(1,2)))
J13 = vpa(eval(J(1,3)))
J14 = vpa(eval(J(1,4)))
J15 = vpa(eval(J(1,5)))
J21 = vpa(eval(J(2,1)))
J22 = vpa(eval(J(2,2)))
J23 = vpa(eval(J(2,3)))
J24 = vpa(eval(J(2,4)))
J25 = vpa(eval(J(2,5)))
J31 = vpa(eval(J(3,1)))
J32 = vpa(eval(J(3,2)))
J33 = vpa(eval(J(3,3)))
J34 = vpa(eval(J(3,4)))
J35 = vpa(eval(J(3,5)))

J = [J11 J12 J13 J14 J15;
    J21   J22 J23 J24 J25;
    J31   J32 J33 J34 J35;
    0 0 0 J(4,4) J(4,5);
    0 0 0 J(5,4) J(5,5);
    0 0 0 J(6,4) J(6,5);]


function [v_column] =unskew_jac(WV)

W = WV(1:3,1:3);
w = [W(3,2);W(1,3);W(2,1)];
v = WV(1:3,4);
v_column = [v;w];
end

function Finv = FINV(F)
Rinv = F(1:3,1:3)';
Pinv = -Rinv*F(1:3,4);
if isa(F,'sym')
    Finv = sym(eye(4));
else
    Finv = eye(4);
end
Finv(1:3,1:3) = Rinv;
Finv(1:3,4) = Pinv;
end

function F = Frotz(angle)
F = [cos(angle) -sin(angle) 0 0;
    sin(angle) cos(angle) 0 0;
    0 0 1 0;
    0 0 0 1];
end

function F = Frotx(angle)
F = [1 0 0 0;
    0 cos(angle) -sin(angle)  0;
    0 sin(angle) cos(angle) 0;
    0 0 0 1];
end

function F = Froty(angle)
F = [cos(angle) 0 sin(angle) 0;
    0 1 0 0;
    -sin(angle) 0 cos(angle) 0;
    0 0 0 1];
end

function F = Ftranx(dist)
if isa(dist,'sym')
    F = sym(eye(4));
else
F = eye(4);
end

F(1,4) = dist;
end

function F = Ftranz(dist)
if isa(dist,'sym')
    F = sym(eye(4));
else
F = eye(4);
end

F(3,4) = dist;
end

function F = Ftrany(dist)
if isa(dist,'sym')
    F = sym(eye(4));
else
F = eye(4);
end

F(2,4) = dist;
end

