Page History

%% Geometric parameters
quadrupole_offset = [925.92, 0, -95.25]; % Quadrupole offset
left_mounting_line_offset = -695.4; % Offset for left mounting line
right_mounting_line_offset = 983.17; % Offset for right mounting line
d_4q = 633.175 + 925.92; % Distance for 4th quadrupole
d_5q = 1003.65 - 925.92; % Distance for 5th quadrupole

% Distance calculations
d_aq = quadrupole_offset(1) - left_mounting_line_offset;
d_ab = right_mounting_line_offset - left_mounting_line_offset;
d_bc = 400; % Distance between B and C

% Trajectory parameters
start_x = -1; end_x = 1;
start_y = 0; end_y = 0;
start_pitch = -0.00127; end_pitch = 0.00127;
start_roll = 0; end_roll = 0;
start_yaw = 0; end_yaw = 0;

% Generate trajectory
x = start_x + time / time(end) * (end_x - start_x);
y = start_y + time / time(end) * (end_y - start_y);
pitch = start_pitch + time / time(end) * (end_pitch - start_pitch);
roll = start_roll + time / time(end) * (end_roll - start_roll);
yaw = start_yaw + time / time(end) * (end_yaw - start_yaw);

% Plot command positions
figure();
plot(time, y);
hold on;
plot(time, x);
title('Command positions');
xlabel('Time [s]');
ylabel('Position [mm]');
legend('y', 'x');
grid on;

% Plot command orientations
figure();
plot(time, pitch);
hold on;
plot(time, roll);
hold on;
plot(time, yaw);
title('Command orientations');
xlabel('Time [s]');
ylabel('Orientation [rad]');
legend('pitch', 'roll', 'yaw');
grid on;

% Calculate joint positions using inverse kinematics
tic;
res = arrayfun(@(a, b, c, d, e) inverse_kin_5(a, b, c, d, e, d_aq, d_ab, d_bc, d_4q, d_5q)', x, y, pitch, roll, yaw, 'UniformOutput', false);
toc;
joint_pos = cell2mat(res);

% Plot joint positions
figure();
plot(time, joint_pos(1, :));
hold on;
plot(time, joint_pos(2, :));
hold on;
plot(time, joint_pos(3, :));
hold on;
plot(time, joint_pos(4, :));
hold on;
plot(time, joint_pos(5, :));

% Apply compensations
x_compensation = 0;
y_compensation = 0;
A1_position = [time' joint_pos(1, :)' - y_compensation'];
A2_position = [time' joint_pos(2, :)' - y_compensation'];
A3_position = [time' joint_pos(3, :)' - y_compensation'];
A4_position = [time' joint_pos(4, :)' + x_compensation'];
A5_position = [time' joint_pos(5, :)' + x_compensation'];

% Update initial states
A1_initial_state = A1_position(1, 2);
A2_initial_state = A2_position(1, 2);
A3_initial_state = A3_position(1, 2);
A4_initial_state = A4_position(1, 2);
A5_initial_state = A5_position(1, 2);