...
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.
