%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ///--- Parameters of the Reverse Kinematics ---\\\                    %%    
%%                                                                       %%
%% // Designed by: Marco Eckert 21/07/2011                               %% 
%%                                                                       %%
%% // Version 1.16                                                       %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% /// variables/vectors:
% 
% s_i             -->   stroke at the linear drive of the i. actuator
% r_i1
% bc_i            -->   distance from the rail to the rotating axis of the 
%                       U joint on the i.actuator
% l(i,1)          -->   length of the leg between the two U joints on the 
%                       i. actuator
% p_i             -->   norm of the leg vector leg_i
% ed_i            -->   length of the follower joint from the TCP to the 
%                       roatating axis of the U joint on the i.actuator
% TCP (1;:)       -->   Given Tool centre points (8)
% TCP (2;:)       -->   Tool centre points entered by the User (mm)
% TCP_speed       -->   Speed of the TCP in x,y and z Coordinates (mm/s)
% TCP_acc         -->   Acceleration of the TCP in x,y and z Coordinates (mm/sē)
% railline_i      -->   relative axis parallel to the rail from the i. actuator
% ballcentre_i    -->   centre of the ball for the leg direction calculation from 
%                       the i. actuator
% max/min_si      -->   Maximum/ Minimum stroke of the prismatic from the 
%                       i. actuator
% stroke_si       -->   Difference between the max/min stroke of the prismatic from 
%                       the i. actuator
% u_i             -->   unit vector from the rail of the Jacobian Matrix from
%                       the i. actuator
% delta_ij        -->   imprecisions in the rail adjustment on the i. actuator
%                       with the j. coordinate


% // Boundary conditions:
%
% installation space:                                   800 x 1300 x 1155 mm
% working area theoretical:                             100 x 750 x 200 mm
% working area:                                         150 x 850 x 3000 mm
% safety distance between installation & working area:  200 mm

% Given 8 TCP points:

p = 8; 
TCP(1) = {[75; 1450; 427.5]};
TCP(2) = {[-75; 1450; 427.5]};
TCP(3) = {[75; 1450; 727.5]};
TCP(4) = {[-75; 1450; 727.5]};
TCP(5) = {[75; 2300; 427.5]};
TCP(6) = {[-75; 2300; 427.5]};
TCP(7) = {[75; 2300; 727.5]};
TCP(8) = {[-75; 2300; 727.5]};

% Leg Length:

l_1 = 926; 
l_2 = 926; 
l_3 = 926;
l = [l_1; l_2; l_3];


max_s1 = 500;
max_s2 = 500;
max_s3 = 500;
min_s1 = 500;
min_s2 = 500;
min_s3 = 500;

% // Transformation-Matrix TCP plate \\

lambda_11 = 1; lambda_12 = 0; lambda_13 = 0;
lambda_21 = 0; lambda_22 = 1; lambda_23 = 0;
lambda_31 = 0; lambda_32 = 0; lambda_33 = 1;

T_RB_Rp = [ lambda_11, lambda_12, lambda_13;...
            lambda_21, lambda_22, lambda_23;...
            lambda_31, lambda_32, lambda_33];

% // Leg 1 (upper leg) \\

delta1_11 = 1; delta1_12 = 0; delta1_13 = 0;
delta1_21 = 0; delta1_22 = 1; delta1_23 = 0;
delta1_31 = 0; delta1_32 = 0; delta1_33 = 1;

T_RB_R11 = [ delta1_11, delta1_12, delta1_13;...
             delta1_21, delta1_22, delta1_23;...
             delta1_31, delta1_32, delta1_33];

bc_1 = [0; 75.582; -118.773];

ed_1 = [0; -253.286; -120.328];

sigma1_1 = 0; sigma1_2 = 0; sigma1_3 = 0;

u_1 = [sigma1_1; 1 + sigma1_2; sigma1_3];

railline_1 = [0; 0; 975.227];


% // Leg 2 (lower right leg) \\

delta2_11 = 1; delta2_12 = 0; delta2_13 = 0;
delta2_21 = 0; delta2_22 = 1; delta2_23 = 0;
delta2_31 = 0; delta2_32 = 0; delta2_33 = 1;

T_RB_R21 = [ delta2_11, delta2_12, delta2_13;...
             delta2_21, delta2_22, delta2_23;...
             delta2_31, delta2_32, delta2_33];

bc_2 = [72.136; 95.041; 138.629];       

ed_2 = [109.737; -256.917; -306.316];

sigma2_1 = 0; sigma2_2 = 0; sigma2_3 = 0;

u_2 = [sigma2_1; 1 + sigma2_2; sigma2_3];

railline_2 = [299.933; 0; 139.349];


% // Leg 3 (lower left leg) \\

delta3_11 = 1; delta3_12 = 0; delta3_13 = 0;
delta3_21 = 0; delta3_22 = 1; delta3_23 = 0;
delta3_31 = 0; delta3_32 = 0; delta3_33 = 1;

T_RB_R31 = [ delta3_11, delta3_12, delta3_13;...
             delta3_21, delta3_22, delta3_23;...
             delta3_31, delta3_32, delta3_33];

bc_3 = [-72.136; 95.04; 138.629];

ed_3 = [-114.954; -262.446; -295.573];

sigma3_1 = 0; sigma3_2 = 0; sigma3_3 = 0;

u_3 = [sigma3_1; 1 + sigma3_2; sigma3_3];

railline_3 = [-299.933; 0; 139.349];