tdehaeze/stewart-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add link to tangled files

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-02-13 16:46:47 +01:00
parent d12891df43
commit b5b3a756a4
17 changed files with 1423 additions and 281 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
matlab/active_damping_dvf.m
View File

@@ -4,22 +4,27 @@ clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
simulinkproject('../');
open('simulink/stewart_active_damping.slx')
open('stewart_platform_model.slx')
% Identification of the Dynamics with perfect Joints
% We first initialize the Stewart platform without joint stiffness.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
@@ -30,12 +35,12 @@ options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Dm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [N]
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
%% Run the linearization
G = linearize(mdl, io, options);
@@ -46,7 +51,7 @@ G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% The transfer function from actuator forces to relative motion sensors is shown in Figure [[fig:dvf_plant_coupling]].
freqs = logspace(1, 3, 1000);
freqs = logspace(1, 4, 1000);
figure;
@@ -79,19 +84,28 @@ legend([p1, p2], {'$D_{m,i}/F_i$', '$D_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
% Effect of the Flexible Joint stiffness on the Dynamics
% Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
% We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
stewart = initializeJointDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% We now use the amplified actuators and re-identify the dynamics
stewart = initializeAmplifiedStrutDynamics(stewart);
Ga = linearize(mdl, io, options);
Ga.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Ga.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% The new dynamics from force actuator to relative motion sensor is shown in Figure [[fig:dvf_plant_flexible_joint_decentralized]].
freqs = logspace(1, 3, 1000);
freqs = logspace(1, 4, 1000);
figure;
@@ -99,6 +113,7 @@ ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
@@ -107,6 +122,7 @@ ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Amplified Actuators');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
@@ -126,7 +142,7 @@ linkaxes([ax1,ax2],'x');
% & & s
% \end{bmatrix} \]
% The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_dvf_rot_stiffness]].
% The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]].
gains = logspace(0, 5, 1000);
@@ -134,45 +150,27 @@ figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
plot(real(pole(Ga)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
plot(real(tzero(Ga)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
p1 = plot(real(cl_poles), imag(cl_poles), '.');
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
p2 = plot(real(cl_poles), imag(cl_poles), '.');
set(gca,'ColorOrderIndex',3);
cl_poles = pole(feedback(Ga, (gains(i)*s)*eye(6)));
p3 = plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,inf]);
xlim([-3000,0]);
ylim([0, 1.1*max(imag(pole(G)))]);
xlim([-1.1*max(imag(pole(G))),0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
% #+name: fig:root_locus_dvf_rot_stiffness
% #+caption: Root Locus plot with Direct Velocity Feedback when considering the Stiffness of flexible joints ([[./figs/root_locus_dvf_rot_stiffness.png][png]], [[./figs/root_locus_dvf_rot_stiffness.pdf][pdf]])
% [[file:figs/root_locus_dvf_rot_stiffness.png]]
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
legend([p1, p2, p3], {'Perfect Joints', 'Flexible Joints', 'Amplified Actuator'}, 'location', 'northwest');

65
matlab/active_damping_iff.m
View File

@@ -4,22 +4,27 @@ clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
simulinkproject('../');
open('simulink/stewart_active_damping.slx')
open('stewart_platform_model.slx')
% Identification of the Dynamics with perfect Joints
% We first initialize the Stewart platform without joint stiffness.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
@@ -30,12 +35,12 @@ options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor Outputs [N]
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'Taum'); io_i = io_i + 1; % Force Sensor Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
@@ -46,7 +51,7 @@ G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
% The transfer function from actuator forces to force sensors is shown in Figure [[fig:iff_plant_coupling]].
freqs = logspace(1, 3, 1000);
freqs = logspace(1, 4, 1000);
figure;
@@ -79,19 +84,28 @@ legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
% Effect of the Flexible Joint stiffness on the Dynamics
% Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
% We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
stewart = initializeJointDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
% We now use the amplified actuators and re-identify the dynamics
stewart = initializeAmplifiedStrutDynamics(stewart);
Ga = linearize(mdl, io, options);
Ga.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Ga.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
% The new dynamics from force actuator to force sensor is shown in Figure [[fig:iff_plant_flexible_joint_decentralized]].
freqs = logspace(1, 3, 1000);
freqs = logspace(1, 4, 1000);
figure;
@@ -99,6 +113,7 @@ ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
@@ -107,6 +122,7 @@ ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Amplified Actuators');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
@@ -134,22 +150,30 @@ figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
plot(real(pole(Ga)), imag(pole(Ga)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
plot(real(tzero(Ga)), imag(tzero(Ga)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
p1 = plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
p2 = plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Ga, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',3);
p3 = plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,inf]);
xlim([-3000,0]);
ylim([0, 1.1*max(imag(pole(G)))]);
xlim([-1.1*max(imag(pole(G))),0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
legend([p1, p2, p3], {'Perfect Joints', 'Flexible Joints', 'Amplified Actuator'}, 'location', 'northwest');
@@ -166,13 +190,20 @@ for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
p1 = plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
p2 = plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',3);
cl_poles = pole(feedback(Ga, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
p3 = plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
legend([p1, p2, p3], {'Perfect Joints', 'Flexible Joints', 'Amplified Actuator'}, 'location', 'northwest');

88
matlab/active_damping_inertial.m
View File

@@ -4,33 +4,38 @@ clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
simulinkproject('../');
open('simulink/stewart_active_damping.slx')
open('stewart_platform_model.slx')
% Identification of the Dynamics
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Absolute velocity of each leg [m/s]
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'Vm'); io_i = io_i + 1; % Absolute velocity of each leg [m/s]
%% Run the linearization
G = linearize(mdl, io, options);
@@ -41,7 +46,7 @@ G.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
% The transfer function from actuator forces to force sensors is shown in Figure [[fig:inertial_plant_coupling]].
freqs = logspace(1, 3, 1000);
freqs = logspace(1, 4, 1000);
figure;
@@ -74,19 +79,28 @@ legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
% Effect of the Flexible Joint stiffness on the Dynamics
% Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
% We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
stewart = initializeJointDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
% We now use the amplified actuators and re-identify the dynamics
stewart = initializeAmplifiedStrutDynamics(stewart);
Ga = linearize(mdl, io, options);
Ga.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Ga.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
% The new dynamics from force actuator to force sensor is shown in Figure [[fig:inertial_plant_flexible_joint_decentralized]].
freqs = logspace(1, 3, 1000);
freqs = logspace(1, 4, 1000);
figure;
@@ -94,6 +108,7 @@ ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
@@ -102,6 +117,7 @@ ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Amplified Actuator');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
@@ -121,53 +137,35 @@ linkaxes([ax1,ax2],'x');
% 0 & & 1
% \end{bmatrix} \]
% The root locus is shown in figure [[fig:root_locus_inertial_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_inertial_rot_stiffness]].
% The root locus is shown in figure [[fig:root_locus_inertial_rot_stiffness]].
gains = logspace(0, 5, 1000);
gains = logspace(2, 5, 100);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
plot(real(pole(Ga)), imag(pole(Ga)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
plot(real(tzero(Ga)), imag(tzero(Ga)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, gains(i)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
cl_poles = pole(feedback(G, gains(i)*eye(6)));
p1 = plot(real(cl_poles), imag(cl_poles), '.');
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
p2 = plot(real(cl_poles), imag(cl_poles), '.');
set(gca,'ColorOrderIndex',3);
cl_poles = pole(feedback(Ga, gains(i)*eye(6)));
p3 = plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,2000]);
xlim([-2000,0]);
ylim([0, 3*max(imag(pole(G)))]);
xlim([-3*max(imag(pole(G))),0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
% #+name: fig:root_locus_inertial_rot_stiffness
% #+caption: Root Locus plot with Decentralized Inertial Control when considering the stiffness of flexible joints ([[./figs/root_locus_inertial_rot_stiffness.png][png]], [[./figs/root_locus_inertial_rot_stiffness.pdf][pdf]])
% [[file:figs/root_locus_inertial_rot_stiffness.png]]
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, gains(i)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
legend([p1, p2, p3], {'Perfect Joints', 'Flexible Joints', 'Amplified Actuator'}, 'location', 'northwest');

93
matlab/cubic_conf_above_platforml.m Normal file
View File

@@ -0,0 +1,93 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('../');
% Having Cube's center above the top platform
% Let's say we want to have a diagonal stiffness matrix when $\{A\}$ and $\{B\}$ are located above the top platform.
% Thus, we want the cube's center to be located above the top center.
% Let's fix the Height of the Stewart platform and the position of frames $\{A\}$ and $\{B\}$:
H = 100e-3; % height of the Stewart platform [m]
MO_B = 20e-3; % Position {B} with respect to {M} [m]
% We find the several Cubic configuration for the Stewart platform where the center of the cube is located at frame $\{A\}$.
% The differences between the configuration are the cube's size:
% - Small Cube Size in Figure [[fig:stewart_cubic_conf_type_1]]
% - Medium Cube Size in Figure [[fig:stewart_cubic_conf_type_2]]
% - Large Cube Size in Figure [[fig:stewart_cubic_conf_type_3]]
% For each of the configuration, the Stiffness matrix is diagonal with $k_x = k_y = k_y = 2k$ with $k$ is the stiffness of each strut.
% However, the rotational stiffnesses are increasing with the cube's size but the required size of the platform is also increasing, so there is a trade-off here.
Hc = 0.4*H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');
% #+name: tab:stewart_cubic_conf_type_1
% #+caption: Stiffness Matrix
% #+RESULTS:
% | 2 | 0 | -2.8e-16 | 0 | 2.4e-17 | 0 |
% | 0 | 2 | 0 | -2.3e-17 | 0 | 0 |
% | -2.8e-16 | 0 | 2 | -2.1e-19 | 0 | 0 |
% | 0 | -2.3e-17 | -2.1e-19 | 0.0024 | -5.4e-20 | 6.5e-19 |
% | 2.4e-17 | 0 | 4.9e-19 | -2.3e-20 | 0.0024 | 0 |
% | -1.2e-18 | 1.1e-18 | 0 | 6.2e-19 | 0 | 0.0096 |
Hc = 1.5*H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');
% #+name: tab:stewart_cubic_conf_type_2
% #+caption: Stiffness Matrix
% #+RESULTS:
% | 2 | 0 | -1.9e-16 | 0 | 5.6e-17 | 0 |
% | 0 | 2 | 0 | -7.6e-17 | 0 | 0 |
% | -1.9e-16 | 0 | 2 | 2.5e-18 | 2.8e-17 | 0 |
% | 0 | -7.6e-17 | 2.5e-18 | 0.034 | 8.7e-19 | 8.7e-18 |
% | 5.7e-17 | 0 | 3.2e-17 | 2.9e-19 | 0.034 | 0 |
% | -1e-18 | -1.3e-17 | 5.6e-17 | 8.4e-18 | 0 | 0.14 |
Hc = 2.5*H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');

336
matlab/cubic_conf_coupling_cartesianl.m Normal file
View File

@@ -0,0 +1,336 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('../');
% Cube's center at the Center of Mass of the mobile platform
% Let's create a Cubic Stewart Platform where the *Center of Mass of the mobile platform is located at the center of the cube*.
% We define the size of the Stewart platform and the position of frames $\{A\}$ and $\{B\}$.
H = 200e-3; % height of the Stewart platform [m]
MO_B = -10e-3; % Position {B} with respect to {M} [m]
% Now, we set the cube's parameters such that the center of the cube is coincident with $\{A\}$ and $\{B\}$.
Hc = 2.5*H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1));
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
% Now we set the geometry and mass of the mobile platform such that its center of mass is coincident with $\{A\}$ and $\{B\}$.
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ...
'Mpm', 10, ...
'Mph', 20e-3, ...
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
% And we set small mass for the struts.
stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3);
stewart = initializeInertialSensor(stewart);
% No flexibility below the Stewart platform and no payload.
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
% The obtain geometry is shown in figure [[fig:stewart_cubic_conf_decouple_dynamics]].
displayArchitecture(stewart, 'labels', false, 'view', 'all');
% #+name: fig:stewart_cubic_conf_decouple_dynamics
% #+caption: Geometry used for the simulations - The cube's center, the frames $\{A\}$ and $\{B\}$ and the Center of mass of the mobile platform are coincident ([[./figs/stewart_cubic_conf_decouple_dynamics.png][png]], [[./figs/stewart_cubic_conf_decouple_dynamics.pdf][pdf]])
% [[file:figs/stewart_cubic_conf_decouple_dynamics.png]]
% We now identify the dynamics from forces applied in each strut $\bm{\tau}$ to the displacement of each strut $d \bm{\mathcal{L}}$.
open('stewart_platform_model.slx')
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% Now, thanks to the Jacobian (Figure [[fig:local_to_cartesian_coordinates]]), we compute the transfer function from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$.
Gc = inv(stewart.kinematics.J)*G*inv(stewart.kinematics.J');
Gc.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gc.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
% The obtain dynamics $\bm{G}_{c}(s) = \bm{J}^{-T} \bm{G}(s) \bm{J}^{-1}$ is shown in Figure [[fig:stewart_cubic_decoupled_dynamics_cartesian]].
freqs = logspace(1, 3, 500);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gc(1, 1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(2, 2), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(3, 3), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:6
for j = i+1:6
p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(1, 1), freqs, 'Hz'))));
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(2, 2), freqs, 'Hz'))));
p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(3, 3), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2, p3, p4], {'$D_x/F_x$','$D_y/F_y$', '$D_z/F_z$', '$D_i/F_j$'})
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gc(4, 4), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(5, 5), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(6, 6), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:6
for j = i+1:6
p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(4, 4), freqs, 'Hz'))));
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(5, 5), freqs, 'Hz'))));
p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(6, 6), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2, p3, p4], {'$R_x/M_x$','$R_y/M_y$', '$R_z/M_z$', '$R_i/M_j$'})
linkaxes([ax1,ax2,ax3,ax4],'x');
% Cube's center not coincident with the Mass of the Mobile platform
% Let's create a Stewart platform with a cubic architecture where the cube's center is at the center of the Stewart platform.
H = 200e-3; % height of the Stewart platform [m]
MO_B = -100e-3; % Position {B} with respect to {M} [m]
% Now, we set the cube's parameters such that the center of the cube is coincident with $\{A\}$ and $\{B\}$.
Hc = 2.5*H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1));
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
% However, the Center of Mass of the mobile platform is *not* located at the cube's center.
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ...
'Mpm', 10, ...
'Mph', 20e-3, ...
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
% And we set small mass for the struts.
stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3);
stewart = initializeInertialSensor(stewart);
% No flexibility below the Stewart platform and no payload.
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
% The obtain geometry is shown in figure [[fig:stewart_cubic_conf_mass_above]].
displayArchitecture(stewart, 'labels', false, 'view', 'all');
% #+name: fig:stewart_cubic_conf_mass_above
% #+caption: Geometry used for the simulations - The cube's center is coincident with the frames $\{A\}$ and $\{B\}$ but not with the Center of mass of the mobile platform ([[./figs/stewart_cubic_conf_mass_above.png][png]], [[./figs/stewart_cubic_conf_mass_above.pdf][pdf]])
% [[file:figs/stewart_cubic_conf_mass_above.png]]
% We now identify the dynamics from forces applied in each strut $\bm{\tau}$ to the displacement of each strut $d \bm{\mathcal{L}}$.
open('stewart_platform_model.slx')
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% And we use the Jacobian to compute the transfer function from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$.
Gc = inv(stewart.kinematics.J)*G*inv(stewart.kinematics.J');
Gc.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gc.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
% The obtain dynamics $\bm{G}_{c}(s) = \bm{J}^{-T} \bm{G}(s) \bm{J}^{-1}$ is shown in Figure [[fig:stewart_conf_coupling_mass_matrix]].
freqs = logspace(1, 3, 500);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gc(1, 1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(2, 2), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(3, 3), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:6
for j = i+1:6
p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(1, 1), freqs, 'Hz'))));
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(2, 2), freqs, 'Hz'))));
p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(3, 3), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2, p3, p4], {'$D_x/F_x$','$D_y/F_y$', '$D_z/F_z$', '$D_i/F_j$'})
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gc(4, 4), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(5, 5), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc(6, 6), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:6
for j = i+1:6
p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(4, 4), freqs, 'Hz'))));
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(5, 5), freqs, 'Hz'))));
p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(6, 6), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2, p3, p4], {'$R_x/M_x$','$R_y/M_y$', '$R_z/M_z$', '$R_i/M_j$'})
linkaxes([ax1,ax2,ax3,ax4],'x');

301
matlab/cubic_conf_coupling_strutsl.m Normal file
View File

@@ -0,0 +1,301 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('../');
% Coupling between the actuators and sensors - Cubic Architecture
% Let's generate a Cubic architecture where the cube's center and the frames $\{A\}$ and $\{B\}$ are coincident.
H = 200e-3; % height of the Stewart platform [m]
MO_B = -10e-3; % Position {B} with respect to {M} [m]
Hc = 2.5*H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1));
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ...
'Mpm', 10, ...
'Mph', 20e-3, ...
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3);
stewart = initializeInertialSensor(stewart);
% No flexibility below the Stewart platform and no payload.
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
displayArchitecture(stewart, 'labels', false, 'view', 'all');
% #+name: fig:stewart_architecture_coupling_struts_cubic
% #+caption: Geometry of the generated Stewart platform ([[./figs/stewart_architecture_coupling_struts_cubic.png][png]], [[./figs/stewart_architecture_coupling_struts_cubic.pdf][pdf]])
% [[file:figs/stewart_architecture_coupling_struts_cubic.png]]
% And we identify the dynamics from the actuator forces $\tau_{i}$ to the relative motion sensors $\delta \mathcal{L}_{i}$ (Figure [[fig:coupling_struts_relative_sensor_cubic]]) and to the force sensors $\tau_{m,i}$ (Figure [[fig:coupling_struts_force_sensor_cubic]]).
open('stewart_platform_model.slx')
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax3 = subplot(2, 1, 2);
hold on;
for i = 1:6
for j = i+1:6
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$L_i/\tau_i$', '$L_i/\tau_j$'})
linkaxes([ax1,ax2],'x');
% #+name: fig:coupling_struts_relative_sensor_cubic
% #+caption: Dynamics from the force actuators to the relative motion sensors ([[./figs/coupling_struts_relative_sensor_cubic.png][png]], [[./figs/coupling_struts_relative_sensor_cubic.pdf][pdf]])
% [[file:figs/coupling_struts_relative_sensor_cubic.png]]
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'Taum'); io_i = io_i + 1; % Force Sensor Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
freqs = logspace(1, 3, 500);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax3 = subplot(2, 1, 2);
hold on;
for i = 1:6
for j = i+1:6
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$F_{m,i}/\tau_i$', '$F_{m,i}/\tau_j$'})
linkaxes([ax1,ax2],'x');
% Coupling between the actuators and sensors - Non-Cubic Architecture
% Now we generate a Stewart platform which is not cubic but with approximately the same size as the previous cubic architecture.
H = 200e-3; % height of the Stewart platform [m]
MO_B = -10e-3; % Position {B} with respect to {M} [m]
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateGeneralConfiguration(stewart, 'FR', 250e-3, 'MR', 150e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1));
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ...
'Mpm', 10, ...
'Mph', 20e-3, ...
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)));
stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3);
stewart = initializeInertialSensor(stewart);
% No flexibility below the Stewart platform and no payload.
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
displayArchitecture(stewart, 'labels', false, 'view', 'all');
% #+name: fig:stewart_architecture_coupling_struts_non_cubic
% #+caption: Geometry of the generated Stewart platform ([[./figs/stewart_architecture_coupling_struts_non_cubic.png][png]], [[./figs/stewart_architecture_coupling_struts_non_cubic.pdf][pdf]])
% [[file:figs/stewart_architecture_coupling_struts_non_cubic.png]]
% And we identify the dynamics from the actuator forces $\tau_{i}$ to the relative motion sensors $\delta \mathcal{L}_{i}$ (Figure [[fig:coupling_struts_relative_sensor_non_cubic]]) and to the force sensors $\tau_{m,i}$ (Figure [[fig:coupling_struts_force_sensor_non_cubic]]).
open('stewart_platform_model.slx')
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax3 = subplot(2, 1, 2);
hold on;
for i = 1:6
for j = i+1:6
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$L_i/\tau_i$', '$L_i/\tau_j$'})
linkaxes([ax1,ax2],'x');
% #+name: fig:coupling_struts_relative_sensor_non_cubic
% #+caption: Dynamics from the force actuators to the relative motion sensors ([[./figs/coupling_struts_relative_sensor_non_cubic.png][png]], [[./figs/coupling_struts_relative_sensor_non_cubic.pdf][pdf]])
% [[file:figs/coupling_struts_relative_sensor_non_cubic.png]]
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'Taum'); io_i = io_i + 1; % Force Sensor Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
freqs = logspace(1, 3, 500);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax3 = subplot(2, 1, 2);
hold on;
for i = 1:6
for j = i+1:6
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-');
end
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$F_{m,i}/\tau_i$', '$F_{m,i}/\tau_j$'})
linkaxes([ax1,ax2],'x');

51
matlab/cubic_conf_size_analysisl.m Normal file
View File

@@ -0,0 +1,51 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('../');
% Analysis
% We initialize the wanted cube's size.
Hcs = 1e-3*[250:20:350]; % Heights for the Cube [m]
Ks = zeros(6, 6, length(Hcs));
% The height of the Stewart platform is fixed:
H = 100e-3; % height of the Stewart platform [m]
% The frames $\{A\}$ and $\{B\}$ are positioned at the Stewart platform center as well as the cube's center:
MO_B = -50e-3; % Position {B} with respect to {M} [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
for i = 1:length(Hcs)
Hc = Hcs(i);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
Ks(:,:,i) = stewart.kinematics.K;
end
% We find that for all the cube's size, $k_x = k_y = k_z = k$ where $k$ is the strut stiffness.
% We also find that $k_{\theta_x} = k_{\theta_y}$ and $k_{\theta_z}$ are varying with the cube's size (figure [[fig:stiffness_cube_size]]).
figure;
hold on;
plot(Hcs, squeeze(Ks(4, 4, :)), 'DisplayName', '$k_{\theta_x} = k_{\theta_y}$');
plot(Hcs, squeeze(Ks(6, 6, :)), 'DisplayName', '$k_{\theta_z}$');
hold off;
legend('location', 'northwest');
xlabel('Cube Size [m]'); ylabel('Rotational stiffnes [normalized]');

96
matlab/cubic_conf_stiffnessl.m Normal file
View File

@@ -0,0 +1,96 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('../');
% Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center
% We create a cubic Stewart platform (figure [[fig:cubic_conf_centered_J_center]]) in such a way that the center of the cube (black star) is located at the center of the Stewart platform (blue dot).
% The Jacobian matrix is estimated at the location of the center of the cube.
H = 100e-3; % height of the Stewart platform [m]
MO_B = -H/2; % Position {B} with respect to {M} [m]
Hc = H; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');
% Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center
% We create a cubic Stewart platform with center of the cube located at the center of the Stewart platform (figure [[fig:cubic_conf_centered_J_not_center]]).
% The Jacobian matrix is not estimated at the location of the center of the cube.
H = 100e-3; % height of the Stewart platform [m]
MO_B = 20e-3; % Position {B} with respect to {M} [m]
Hc = H; % Size of the useful part of the cube [m]
FOc = H/2; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');
% Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center
% Here, the "center" of the Stewart platform is not at the cube center (figure [[fig:cubic_conf_not_centered_J_center]]).
% The Jacobian is estimated at the cube center.
H = 80e-3; % height of the Stewart platform [m]
MO_B = -30e-3; % Position {B} with respect to {M} [m]
Hc = 100e-3; % Size of the useful part of the cube [m]
FOc = H + MO_B; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');
% Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center
% Here, the "center" of the Stewart platform is not at the cube center.
% The Jacobian is estimated at the center of the Stewart platform.
% The center of the cube is at $z = 110$.
% The Stewart platform is from $z = H_0 = 75$ to $z = H_0 + H_{tot} = 175$.
% The center height of the Stewart platform is then at $z = \frac{175-75}{2} = 50$.
% The center of the cube from the top platform is at $z = 110 - 175 = -65$.
H = 100e-3; % height of the Stewart platform [m]
MO_B = -H/2; % Position {B} with respect to {M} [m]
Hc = 1.5*H; % Size of the useful part of the cube [m]
FOc = H/2 + 10e-3; % Center of the cube with respect to {F}
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 0, 'MHb', 0);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 215e-3, 'Mpr', 195e-3);
displayArchitecture(stewart, 'labels', false);
scatter3(0, 0, FOc, 200, 'kh');

11
matlab/approximate_inverse_kinematics_validity.m → matlab/kinematic_study_approximation_validity.m
View File

@@ -1,11 +1,16 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
simulinkproject('../');
% Stewart architecture definition
% We first define some general Stewart architecture.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStewartPose(stewart);
@@ -29,7 +34,7 @@ Ls_exact = zeros(6, length(Xrs));
for i = 1:length(Xrs)
Xr = Xrs(i);
L_approx(:, i) = stewart.J*[Xr; 0; 0; 0; 0; 0;];
L_approx(:, i) = stewart.kinematics.J*[Xr; 0; 0; 0; 0; 0;];
[~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xr; 0; 0]);
end

13
matlab/mobility_from_actuator_stroke.m → matlab/kinematic_study_mobility.m
View File

@@ -1,17 +1,22 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
simulinkproject('../');
% Stewart architecture definition
% Let's first define the Stewart platform architecture that we want to study.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart);
stewart = computeJacobian(stewart);
@@ -44,7 +49,7 @@ for i = 1:length(thetas)
Ty = sin(thetas(i))*sin(phis(j));
Tz = cos(thetas(i));
dL = stewart.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
dL = stewart.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
rs(i, j) = max([dL(dL<0)*L_min; dL(dL>0)*L_max]);
end

23
matlab/required_stroke_from_mobility.m → matlab/kinematic_study_required_actuator_stroke.m
View File

@@ -1,17 +1,22 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
simulinkproject('../');
% Stewart architecture definition
% Let's first define the Stewart platform architecture that we want to study.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart);
stewart = computeJacobian(stewart);
@@ -30,12 +35,12 @@ Rz_max = 0; % Rotation [rad]
% We do that using either the Inverse Kinematic solution or the Jacobian matrix as an approximation.
LTx = stewart.J*[Tx_max 0 0 0 0 0]';
LTy = stewart.J*[0 Ty_max 0 0 0 0]';
LTz = stewart.J*[0 0 Tz_max 0 0 0]';
LRx = stewart.J*[0 0 0 Rx_max 0 0]';
LRy = stewart.J*[0 0 0 0 Ry_max 0]';
LRz = stewart.J*[0 0 0 0 0 Rz_max]';
LTx = stewart.kinematics.J*[Tx_max 0 0 0 0 0]';
LTy = stewart.kinematics.J*[0 Ty_max 0 0 0 0]';
LTz = stewart.kinematics.J*[0 0 Tz_max 0 0 0]';
LRx = stewart.kinematics.J*[0 0 0 Rx_max 0 0]';
LRy = stewart.kinematics.J*[0 0 0 0 Ry_max 0]';
LRz = stewart.kinematics.J*[0 0 0 0 0 Rz_max]';