% TOGAC41_AMBF_Fkderiv.m

% This file was used to calculate the forward kinematics and jacobian
% 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 
% Note that the result is extremely computationally demanding and may not
% complete for the jacobian. The simplified form of the jacobian may give
% errors.


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

clear all

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

pseudoBase = [-3.67324084038501e-06	-0.999999999993254	-6.97425863880356e-13	-0.504161160403451;
0.999999999993236	-3.67324084038501e-06	1.89860988282034e-07	-6.16349500154316e-07;
-1.89860988283315e-07	-2.07277167824804e-17	0.999999999999982	-1.14230000000000;
0	0	0	1];

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 = solve(eq2-eq3,x);
Y = solve(subs(eq2-eq1,x,X),y);
Z = simplify(vpa(solve(subs(subs(eq1,x,X),y,Y),z)));

Z1 = Z(1);
Z2 = Z(2);
Y1 = subs(Y,z,Z1);
Y2 = subs(Y,z,Z2);
X1 = subs(subs(X,y,Y1),z,Z1);
X2 = subs(subs(X,y,Y2),z,Z2);



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 = subs(Z2,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
X1 = subs(X1,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
X2 = subs(X2,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
Y1 = subs(Y1,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});
Y2 = subs(Y2,{L1,L2,L3},{In1+Offset,In2+Offset,In3+Offset});

% 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 = pseudoBase*Frotz(-pi/2)*vpa(pseudoBase_F_P)*Frotz(pi/2);


MobplattoRABxyz=[0.0324986733401938;
                0;
                0.0580014126707007];
% From the World to the Roll joint of the robot
W_F_R = W_F_P*Ftranz(MobplattoRABxyz(3))*Frotx(pi/2)*Ftranz(MobplattoRABxyz(1))*Frotz(theta4);


RABtoTDLFSxyz = [0.588399570422111;
                0;
                -0.0129995573458910];
% From the World to the tilt joint of the robot
W_F_T = W_F_R*Ftranz(RABtoTDLFSxyz(1))*Frotz(-pi/2)*Ftranx(-RABtoTDLFSxyz(3))*Frotx(-pi/2)*Frotz(0.069414985474804+(pi/2))*Frotz(theta5);


TDLFStoEndo35xyz = [0.042926375096223;
                    0.000100604375186015;
                    0.024214916397148];
% From the World to the end effector of the robot
W_F_E35 = W_F_T*Frotz(-0.069414985474804)*Ftranx(-TDLFStoEndo35xyz(1))*Frotz(pi/2)*Ftranx(TDLFStoEndo35xyz(3))*Froty(-pi/2)*Frotx(2.251067817075432-(pi/2));


TipHomeposition = [0.07290743048672868;
                    0.0001016308354309764;
                    -0.5192307434950012];
E35Homepos = [0.15966197324;
    1.022064708172975e-04;
    -0.375122340856];

Endo35toEndoTipxyz = TipHomeposition-E35Homepos;
% From the World to the end of the tool of the robot
W_F_E = W_F_E35*Frotx(-2.251067817075432+(pi/2))*Ftranz(-Endo35toEndoTipxyz(3))*Ftrany(Endo35toEndoTipxyz(1))*Frotx(-0.890522834903571-(pi/2));


%% Find the jacobian
W_F_E_inv = FINV(W_F_E);

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
%% 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