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
For a 5-DOF (Degrees of Freedom) SCU with pitch, roll, yaw, x, and y, the descriptions of Cartesian space and joint space are as follows:
Cartesian Space
In this context, Cartesian space refers to the position and orientation of the quadrupole in a five-dimensional coordinate system:
- x: Linear displacement along the x-axis.
- y: Linear displacement along the y-axis.
- Pitch: Rotation about the x-axis.
- Roll: Rotation about the y-axis.
- Yaw: Rotation about the z-axis.
The Cartesian space describes the desired position (x, y) and orientation (pitch, roll, yaw) of the quadrupole within the working environment.
Joint Space
For a 5-DOF SCU, the joint space describes the states of its individual actuators. Each actuator's position or angle contributes to the overall configuration of the strongback. The joint space is represented by a set of five coordinates, one for each actuator, which could include:
- A1: Position or angle of the first actuator.
- A2: Position or angle of the second actuator.
- A3: Position or angle of the third actuator.
- A4: Position or angle of the fourth actuator.
- A5: Position or angle of the fifth actuator.
In summary:
- Cartesian Space: The quadrupole's position (x, y) and orientation (pitch, roll, yaw).
- Joint Space: The specific positions of the SCU's five linear actuators.
Definition of a simple Point To Point motion in the cartesian space
%% 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);
In this block 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 calls 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 behavior that characterizes our system. The better and more complex the inverse kinematics function is, the smaller 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
If we consider y the vector that represents the 5 actuator positions and x the vector that represents the quadrupole's 5 degree of freedom, we can write the relation between the two as:
Where K^-1 is the inverse kinematics function.
Now that the y vector is computed for all the time samples of the desired motion, all the required blocks to run our simulation are ready.
Running the simulation
% comment the next line if you want to see the output on the Mechanics
% Explorer
set_param('SCU_FULL', 'SimMechanicsOpenEditorOnUpdate', 'off');
simOut = sim(fullPath, 'StopTime', string(simulation_time));
Running the simulation is quite straightforward, the only command to execute is "sim()", and it will return a Simulink.SimulationOutput object. This is where all the simulation output signals are stored
Data analysis
Now it's time to analyze the results.
If we look at the positions/orientations measured during the simulation we can observe 2 characteristics:
- Y axis is properly following the commanded motion of 3 mm
- X position, instead of being 0 (as requested on our command profile) is almost 4 um at both the extremity of the motion with a clear quadratic law.
The interpolating equation shows a negligible error, confirming the theory about the quadraticity. This information can be added to our inverse kinematics function to further reduce this error
If our goal is to analyze how good an inverse kinematics algorithm is, we need to compare the desired quadrupole motion/orientation together with the real one defined by our inverse function and executed by the model. A good indicator is the error between the two arrays.
The error plots confirms our previous observation, a maximum error of 4 um is present at both extremities of the motion with a quadratic behavior. All the other degree of freedom shows a negligible error.
%% Analysis
% Analyze and plot data
quadrupole_position = simOut.yout.getElement('Quadrupole position').Values;
quadrupole_orientation = simOut.yout.getElement('Quadrupole orientation').Values;
% Plot quadrupole positions
figure();
tiledlayout(3, 1);
nexttile;
plot(quadrupole_position.Quad_Y.Time, quadrupole_position.Quad_Y.Data - quadrupole_offset(2));
title('Quadrupole x position');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
nexttile;
plot(quadrupole_position.Quad_Z.Time, quadrupole_position.Quad_Z.Data - quadrupole_offset(3));
title('Quadrupole y position');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
nexttile;
plot(quadrupole_position.Quad_X.Time, quadrupole_position.Quad_X.Data - quadrupole_offset(1));
title('Quadrupole z position');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
% Plot quadrupole orientations
figure();
tiledlayout(3, 1);
nexttile;
plot(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 1) - 90) * pi / 180);
title('Quadrupole yaw');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;
nexttile;
plot(quadrupole_orientation.Time, quadrupole_orientation.Data(:, 2) * pi / 180);
title('Quadrupole roll');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;
nexttile;
plot(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 3) - 90) * pi / 180);
title('Quadrupole pitch');
xlabel('Time [s]');
ylabel('Pitch [rad]');
grid on;
disp("Sim done");
% Error analysis
interpolated_x = interp1(quadrupole_position.Quad_X.Time - quadrupole_position.Quad_X.Time(1), quadrupole_position.Quad_Y.Data - quadrupole_offset(2), time);
interpolated_y = interp1(quadrupole_position.Quad_Z.Time - quadrupole_position.Quad_Z.Time(1), quadrupole_position.Quad_Z.Data - quadrupole_offset(3), time);
interpolated_z = interp1(quadrupole_position.Quad_Y.Time, quadrupole_position.Quad_Y.Data - quadrupole_offset(1), time);
interpolated_pitch = interp1(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 3) - 90) * pi / 180, time);
interpolated_roll = interp1(quadrupole_orientation.Time, quadrupole_orientation.Data(:, 2) * pi / 180, time);
interpolated_yaw = interp1(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 1) - 90) * pi / 180, time);
% Plot inverse kinematic errors
figure();
tiledlayout(3, 1);
nexttile;
plot(time, x - interpolated_x);
title('Inverse kinematic x error');
xlabel('Time [s]');
ylabel('Error [mm]');
grid on;
nexttile;
plot(time, y - interpolated_y);
title('Inverse kinematic y error');
xlabel('Time [s]');
ylabel('Error [mm]');
grid on;
nexttile;
plot(time, 0 - interpolated_z);
title('Inverse kinematic z error');
xlabel('Time [s]');
ylabel('Error [mm]');
grid on;
% Plot orientation errors
figure();
tiledlayout(3, 1);
nexttile;
plot(time, yaw - interpolated_yaw);
title('Inverse kinematic yaw error');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;
nexttile;
plot(time, roll - interpolated_roll);
title('Inverse kinematic roll error');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;
nexttile;
plot(time, yaw - interpolated_yaw);
title('Inverse kinematic yaw error');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;
%%
%XY
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_y, interpolated_x)
title('Cross correlation X-Y')
xlabel('Y position [mm]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_x, interpolated_y)
title('Cross correlation Y-X')
xlabel('X position [mm]')
ylabel('Y position [mm]')
grid on
%YAW
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_yaw, interpolated_x)
title('Cross correlation X-Yaw')
xlabel('Yaw [rad]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_yaw, interpolated_y)
title('Cross correlation Y-Yaw ')
xlabel('Yaw [rad]')
ylabel('Y position [mm]')
grid on
%PITCH
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_pitch, interpolated_x)
title('Cross correlation X-Pitch')
xlabel('Pitch [rad]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_pitch, interpolated_y)
title('Cross correlation Y-Pitch')
xlabel('Pitch [rad]')
ylabel('Y position [mm]')
grid on
%ROLL
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_roll, interpolated_x)
title('Cross correlation X-Roll')
xlabel('Roll [rad]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_roll, interpolated_y)
title('Cross correlation Y-Roll')
xlabel('Pitch [rad]')
ylabel('Y position [mm]')
grid on
Conclusions
In this page, we have learned how to define a simple PTP motion (infinite acceleration and jerk) in the cartesian space, solve the inverse kinematics, load the motion profiles for all the actuators on the model, run the simulation, and then analyze the data.
