...
The user can change simulation_time and n_pts. The rest of the scripts initialize the arrays required by the model to execute the simulations. All the values are initialized to 0
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
%% 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); |
This is the block where the quadrupole trajectory is defined. The start and end positions are defined under Trajectory parameters comment. A pure vertical displacement (Y) of 3 mm can be defined as:
start_x = 0; end_x = 0;
start_y = -1.5; end_y = 1.5;
start_pitch = 0; end_pitch = 0;
start_roll = 0; end_roll = 0;
start_yaw = 0; end_yaw = 0;
This is the quad requested trajectory
Then the script call the inverse kinematics function that generates, starting from the end effector positions, the motion profiles for the actuators. The inverse kinematic function is a mathematical representation of the kinematic behaviour that characterize our system. The better and more complex teh inverse kinematics function is, the smaller will be the error between the desired position/orientation of the strongback compared with the actual one. A very simple model that doesn't keep in consideration the cross talks between the vertical and the horizontal axes can be written as:
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
function [joint_displacement] = inverse_kin_5(x_quadrupole, y_quadrupole,pitch, roll, yaw, d_aq, d_ab, d_bc, d_4q, d_5q)
%INVERSE_KIN_3 Summary of this function goes here
% Detailed explanation goes here
y_a = y_quadrupole + d_aq*pitch;
y_b = y_quadrupole + pitch*(d_aq-d_ab) + (roll*d_bc)/2;
y_c = y_quadrupole + pitch*(d_aq-d_ab) - (roll*d_bc)/2;
y_4 = -x_quadrupole + abs(d_4q)*yaw;
y_5 = -x_quadrupole - abs(d_5q)*yaw; %right hand rule sign assigment
joint_displacement = [y_a y_b y_c, y_4, y_5];
end
|
This is located at project_root/Functions/inverse_kin_5.m
