You are viewing an old version of this page. View the current version.
Compare with Current
View Page History
« Previous
Version 2
Next »
The main purpose of this page is to describe the working principle behind the simulation runner. This entry point enables the execution of arbitrary motion profiles on all actuators and measures the behavior of the quadrupole. This functionality is crucial for understanding the couplings that affect the entire system. For instance, a vertical displacement of all three actuators results in a parasitic lateral shift that needs further adjustment. Once the kinematic behavior is well-understood, this script can be used to evaluate inverse kinematics functions and measure the error between the actual and desired positions and orientations of the quadrupole.
Location
The models are located in project_root/Simulation scripts
- Sim_runner_full.m: Runs the 5dof model
- Sim_runner_3dof.m: Runs the 3dof model
Code
Parameters and array initialization
%% Get the full path of the current script
scriptFullPath = mfilename('fullpath'); % Get the full path of the current script
[scriptFolder, ~, ~] = fileparts(scriptFullPath); % Extract the folder containing the script
% Define the relative path to the SCU model file
relativePath = '../Models/SCU_FULL.slx'; % Relative path to the SCU model
fullPath = fullfile(scriptFolder, relativePath); % Construct the full path to the SCU model
%% Configure and generate signals
close all;
simulation_time = 10; % Simulation time in seconds
n_pts = 1000; % Number of points
time = linspace(0, simulation_time, n_pts); % Time vector
positions = zeros([n_pts, 6]); % Initialize positions matrix
% Define initial positions
initial_position = 0;
positions(:, 1:5) = initial_position;
% Create position arrays for each actuator
A1_position = [time' positions(:, 1)];
A2_position = [time' positions(:, 2)];
A3_position = [time' positions(:, 3)];
A4_position = [time' positions(:, 4)];
A5_position = [time' positions(:, 5)];
% Initial states of each actuator
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);
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
%% 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:
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
