You are viewing an old version of this page. View the current version.
Compare with Current
View Page History
« Previous
Version 4
Next »
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 models are 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. The drawback is that we have a parasitic motion on Y. This is why we need the inverse kinematics in place to keep all the degree 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'];