...
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
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
%% 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:
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
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
|
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:
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:
In summary:
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
%% 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:
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
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:
| Latex |
|---|
\( \vec{y} = K^{-1}(\vec{x})\) |
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.
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
% 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
Now it's time to analyze the results.
If we look at the positions/orientations measured during the simulation we can observe 2 characteristics:
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.
| Code Block | ||||||
|---|---|---|---|---|---|---|
| ||||||
%% 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 |
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.This is located at project_root/Functions/inverse_kin_5.m
