Simulation runner joint is a simplified version of the Simulation runner where the motion is specified in the joint space. No inverse kinematic functions are required, this script commands directly the actuators with any arbitrary motion
Location
The model is located in project_root/Simulation scripts
- Sim_runner_joint_full.m: Runs the 5dof 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
% Trajectory parameters
A1_start = -2.5;
A1_end = 2.5;
A2_start = 0;
A2_end = 0;
A3_start = 0;
A3_end = 0;
A4_start = 0;
A4_end = 0;
A5_start = 0;
A5_end = 0;
% Generate trajectory
A1 = A1_start + time / time(end) * (A1_end - A1_start);
A2 = A2_start + time / time(end) * (A2_end - A2_start);
A3 = A3_start + time / time(end) * (A3_end - A3_start);
A4 = A4_start + time / time(end) * (A4_end - A4_start);
A5 = A5_start + time / time(end) * (A5_end - A5_start);
% Create position arrays for each actuator
A1_position = [time' A1'];
A2_position = [time' A2'];
A3_position = [time' A3'];
A4_position = [time' A4'];
A5_position = [time' A5'];
figure();
tiledlayout(3, 2);
nexttile;
plot(time, A1_position(:,2));
title('A1');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
nexttile;
plot(time, A2_position(:,2));
title('A2');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
nexttile;
plot(time, A3_position(:,2));
title('A3');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
nexttile;
plot(time, A4_position(:,2));
title('A4');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
nexttile;
plot(time, A5_position(:,2));
title('A5');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;
% 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);
%% Simulation
tic;
% comment the next line if you want to see the output on the Mechanics
% Explorer
load_system(fullPath);
set_param('SCU_FULL', 'SimMechanicsOpenEditorOnUpdate', 'on');
simOut = sim(fullPath, 'StopTime', string(simulation_time));
toc;
%% 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;
The code is very similar to Simulation runner where the only difference is the absence of the inverse kinematics calculations. The A1_position..A5_position are computed directly and then fed to the simulation
Motion Examples
Only A1
By only moving A1 we are mostly controlling the pitch of the strongback. However, this is not enough to obtain a pure pitch motion because we also have a parasitic motion on Y. This is why we need the inverse kinematics in place to keep all the degrees of freedom in the cartesian space as close as possible to the target configuration
Circle
Combined motions can be executed. In this case, I'm simulating a rotary motion in the joint space.
% generate a circular trajectory
R = 2;
time_norm = time / time(end);
theta = time_norm * 4*pi;
x = R*cos(theta);
y = R*sin(theta);
% Generate trajectory
A1 = A1_start + time / time(end) * (A1_end - A1_start);
A2 = A2_start + time / time(end) * (A2_end - A2_start);
A3 = A3_start + time / time(end) * (A3_end - A3_start);
A4 = A4_start + time / time(end) * (A4_end - A4_start);
A5 = A5_start + time / time(end) * (A5_end - A5_start);
% Create position arrays for each actuator
A1_position = [time' A1'];
A2_position = [time' y'];
A3_position = [time' y'];
A4_position = [time' A4'];
A5_position = [time' x'];
By completely ignoring the inverse kinematic we on both quad_x and quad_y the maximum target position (+-2 mm) is never achieved.
It is also clear that the quad_yaw is in phase with the quad_x and the quad_pitch is in phase with quad_y. Both show a maximum error of more than 1 mrad.
A4 and A5 opposite motion
Let's try to have an opposite linear motion on A4 and A5 (horizontal actuators) to generate a yaw motion. Since the rotation axis is not coincident with the quadrupole center we expect an error on quad_x as well as on quad_y due to the swing effect.
% Trajectory parameters
A1_start = 0;
A1_end = 0;
A2_start = 0;
A2_end = 0;
A3_start = 0;
A3_end = 0;
A4_start = -2.5;
A4_end = 2.5;
A5_start = 2.5;
A5_end = -2.5;
% generate a circular trajectory
R = 2;
time_norm = time / time(end);
theta = time_norm * 4*pi;
x = R*cos(theta);
y = R*sin(theta);
% Generate trajectory
A1 = A1_start + time / time(end) * (A1_end - A1_start);
A2 = A2_start + time / time(end) * (A2_end - A2_start);
A3 = A3_start + time / time(end) * (A3_end - A3_start);
A4 = A4_start + time / time(end) * (A4_end - A4_start);
A5 = A5_start + time / time(end) * (A5_end - A5_start);
Our predictions were right! The strongback mainly moved on yaw with some relevant parasitic motion on quad_x and quad_y.
From the plot is evident that the whole strongback is moving along z. This effect is mainly because the center of mass is moving during the imposed motion and, since the simulation is firstly a dynamical simulation, this change of position results in a displacement in the only non-constrined axis. In this model, the z-restrain mechanism is not implemented.
