% 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