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

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.

Rotary motion code 
% 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'];


  • No labels