%%%%% Galen Mk.2 Kinematic Parameters %%%%%

% Define geometric parameters of the robot delta mechanism 

rb = 217.86092405/1000;
rd = 50/1000;
r_screw = 257.86092405/1000;            % Radius of location of ball screws

p_l = 440.35040068/1000;                % Length of the legs
p_w = 107.6254/1000;                    % Length from ball A to ball B   
P_base = [0, 0, 0];

offsetCarriage = [r_screw-rb, p_w/2, 0];   % Offset from carriage center to carriage ball center

theta_b1 = (2*1 - 1) * pi/3;          % Angle between ball screw 1 and +<x>
theta_b2 = (2*2 - 1) * pi/3;          % Angle between ball screw 2 and +<x>
theta_b3 = (2*3 - 1) * pi/3;          % Angle between ball screw 3 and +<x>

% Upstream kinematic parameters - please see forward kinematics figure with DH parameters (also provided in this folder)

a1 = 42.235 % (delta end to roll)
a2 = 58.180  % Changed from 61.18 to 58.18 to match the parameters from Mk2.0 (61.18 is from Mk2.1)
a3 = 589.600 % (roll to tilt - along roll axis - parallel to x-axis of inertial frame)
a4 = 13.049 % (roll to tilt - parallel to z-axis of inertial frame)
a5 = 25.000 
a6 = 35.090

%%%%% Simulation Kinematic Parameters - Translation (mm) - XYZ Euler Angles/Roll-Pitch-Yaw (Radians) %%%%%

% Delta kinematics

chassis (0, 0, 0, 0, 0, 0)

joint: chassis -> carriage0 (128.93046203, 223.31411087, 268.9, 0, 0, pi/3) 
joint: chassis -> carriage1 (-257.86092405, 0, 268.9, 0, 0, pi) 
joint: chassis -> carriage2 (128.93046203, -223.31411087, 268.9, 0, 0, 5*pi/3)
 
carriage0, carriage1, carriage2 (0, 0, 0, 0, 0, pi)

joint: carriage0 -> delta leg00 (40, 53.8127, 0, 0, 0, 0)
joint: carriage0 -> delta leg01 (40, -53.8127, 0, 0, 0, 0)
joint: carriage1 -> delta leg10 (40, 53.8127, 0, 0, 0, 0)
joint: carriage1 -> delta leg11 (40, -53.8127, 0, 0, 0, 0)
joint: carriage2 -> delta leg20 (40, 53.8127, 0, 0, 0, 0)
joint: carriage2 -> delta leg21 (40, -53.8127, 0, 0, 0, 0)

delta leg00, delta leg01, delta leg10, delta leg11, delta leg20, delta leg21 (0, 0, 0, 0.39025562074, 0, pi/2)

% Upstream kinematic chain (delta platform to end effector)

body: chassis -> delta platform (0, 0, 676.141, 0, 0, 0)

joint: delta platform -> delta platform-arm (42.235, 0, 58.180, -pi/2, 0, -pi/2)

body: delta platform-arm -> arm (0, 0, 59.85, 0, -pi/2, pi/2)

joint: delta platform-arm -> arm-tilt (0, 13.049, 589.6, pi/2, 0, -pi/2)

body: arm-tilt -> tilt (0, 0, 0, pi/2, 0, pi/2)

joint: tilt -> tilt-ee link (35.09, 0, 25, 0, 0, 0)

% Optical tracker

body: world frame -> optical tracker (1400, 700, 1000, 0.162365, -2.0616, 1.35676)






