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

[Done] Partition Stewart struct - Variant Simscape

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-02-11 15:16:07 +01:00
parent 2231aa53ce
commit e1cd9b0aa8
30 changed files with 2326 additions and 1211 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/computeJacobian.m
View File

@@ -5,17 +5,31 @@ function [stewart] = computeJacobian(stewart)
%
% Inputs:
% - stewart - With at least the following fields:
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
% - Ab [3x6] - The 6 position of the joints bi expressed in {A}
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
% - actuators.K [6x1] - Total stiffness of the actuators
%
% Outputs:
% - stewart - With the 3 added field:
% - J [6x6] - The Jacobian Matrix
% - K [6x6] - The Stiffness Matrix
% - C [6x6] - The Compliance Matrix
% - kinematics.J [6x6] - The Jacobian Matrix
% - kinematics.K [6x6] - The Stiffness Matrix
% - kinematics.C [6x6] - The Compliance Matrix
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
As = stewart.geometry.As;
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
Ab = stewart.geometry.Ab;
stewart.C = inv(stewart.K);
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
Ki = stewart.actuators.K;
J = [As' , cross(Ab, As)'];
K = J'*diag(Ki)*J;
C = inv(K);
stewart.kinematics.J = J;
stewart.kinematics.K = K;
stewart.kinematics.C = C;

85
src/computeJointsPose.m
View File

@@ -5,43 +5,74 @@ function [stewart] = computeJointsPose(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - FO_A [3x1] - Position of {A} with respect to {F}
% - MO_B [3x1] - Position of {B} with respect to {M}
% - FO_M [3x1] - Position of {M} with respect to {F}
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
%
% Outputs:
% - stewart - A structure with the following added fields
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
% - Ab [3x6] - The i'th column is the position of bi with respect to {A}
% - Ba [3x6] - The i'th column is the position of ai with respect to {B}
% - Bb [3x6] - The i'th column is the position of bi with respect to {B}
% - l [6x1] - The i'th element is the initial length of strut i
% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
% - geometry.l [6x1] - The i'th element is the initial length of strut i
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;
stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
Mb = stewart.platform_M.Mb;
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
FO_A = stewart.platform_F.FO_A;
stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
MO_B = stewart.platform_M.MO_B;
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
FO_M = stewart.geometry.FO_M;
stewart.FRa = zeros(3,3,6);
stewart.MRb = zeros(3,3,6);
Aa = Fa - repmat(FO_A, [1, 6]);
Bb = Mb - repmat(MO_B, [1, 6]);
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
l = vecnorm(Ab - Aa)';
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
FRa = zeros(3,3,6);
MRb = zeros(3,3,6);
for i = 1:6
stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
end
stewart.geometry.Aa = Aa;
stewart.geometry.Ab = Ab;
stewart.geometry.Ba = Ba;
stewart.geometry.Bb = Bb;
stewart.geometry.As = As;
stewart.geometry.Bs = Bs;
stewart.geometry.l = l;
stewart.struts_F.l = l/2;
stewart.struts_M.l = l/2;
stewart.platform_F.FRa = FRa;
stewart.platform_M.MRb = MRb;

79
src/displayArchitecture.m
View File

@@ -27,28 +27,43 @@ arguments
args.F_color = [0 0.4470 0.7410]
args.M_color = [0.8500 0.3250 0.0980]
args.L_color = [0 0 0]
args.frames logical {mustBeNumericOrLogical} = true
args.legs logical {mustBeNumericOrLogical} = true
args.joints logical {mustBeNumericOrLogical} = true
args.labels logical {mustBeNumericOrLogical} = true
args.frames logical {mustBeNumericOrLogical} = true
args.legs logical {mustBeNumericOrLogical} = true
args.joints logical {mustBeNumericOrLogical} = true
args.labels logical {mustBeNumericOrLogical} = true
args.platforms logical {mustBeNumericOrLogical} = true
end
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
FO_A = stewart.platform_F.FO_A;
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
MO_B = stewart.platform_M.MO_B;
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
H = stewart.geometry.H;
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
Mb = stewart.platform_M.Mb;
figure;
hold on;
FTa = [eye(3), stewart.FO_A; ...
FTa = [eye(3), FO_A; ...
zeros(1,3), 1];
ATb = [args.ARB, args.AP; ...
zeros(1,3), 1];
BTm = [eye(3), -stewart.MO_B; ...
BTm = [eye(3), -MO_B; ...
zeros(1,3), 1];
FTm = FTa*ATb*BTm;
d_unit_vector = stewart.H/4;
d_unit_vector = H/4;
d_label = stewart.H/20;
d_label = H/20;
Ff = [0, 0, 0];
if args.frames
@@ -62,24 +77,22 @@ if args.frames
end
end
Fa = stewart.FO_A;
if args.frames
quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ...
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(Fa(1) + d_label, ...
Fa(2) + d_label, ...
Fa(3) + d_label, '$\{A\}$', 'Color', args.F_color);
text(FO_A(1) + d_label, ...
FO_A(2) + d_label, ...
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
end
end
if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr')
if args.platforms && stewart.platform_F.type == 1
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
center = [0; 0; 0]; % Center of the circle
radius = stewart.platforms.Fpr; % Radius of the circle [m]
radius = stewart.platform_F.R; % Radius of the circle [m]
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
@@ -89,14 +102,14 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms,
end
if args.joints
scatter3(stewart.Fa(1,:), ...
stewart.Fa(2,:), ...
stewart.Fa(3,:), 'MarkerEdgeColor', args.F_color);
scatter3(Fa(1,:), ...
Fa(2,:), ...
Fa(3,:), 'MarkerEdgeColor', args.F_color);
if args.labels
for i = 1:size(stewart.Fa,2)
text(stewart.Fa(1,i) + d_label, ...
stewart.Fa(2,i), ...
stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
for i = 1:size(Fa,2)
text(Fa(1,i) + d_label, ...
Fa(2,i), ...
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
end
end
end
@@ -115,7 +128,7 @@ if args.frames
end
end
FB = stewart.FO_A + args.AP;
FB = FO_A + args.AP;
if args.frames
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
@@ -129,11 +142,11 @@ if args.frames
end
end
if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr')
if args.platforms && stewart.platform_M.type == 1
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
center = Fm(1:3); % Center of the circle
radius = stewart.platforms.Mpr; % Radius of the circle [m]
radius = stewart.platform_M.R; % Radius of the circle [m]
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
@@ -143,7 +156,7 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms,
end
if args.joints
Fb = FTm*[stewart.Mb;ones(1,6)];
Fb = FTm*[Mb;ones(1,6)];
scatter3(Fb(1,:), ...
Fb(2,:), ...
@@ -160,14 +173,14 @@ end
if args.legs
for i = 1:6
plot3([stewart.Fa(1,i), Fb(1,i)], ...
[stewart.Fa(2,i), Fb(2,i)], ...
[stewart.Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
plot3([Fa(1,i), Fb(1,i)], ...
[Fa(2,i), Fb(2,i)], ...
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
if args.labels
text((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ...
(stewart.Fa(2,i)+Fb(2,i))/2, ...
(stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
(Fa(2,i)+Fb(2,i))/2, ...
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
end
end
end

7
src/forwardKinematicsApprox.m
View File

@@ -6,7 +6,7 @@ function [P, R] = forwardKinematicsApprox(stewart, args)
%
% Inputs:
% - stewart - A structure with the following fields
% - J [6x6] - The Jacobian Matrix
% - kinematics.J [6x6] - The Jacobian Matrix
% - args - Can have the following fields:
% - dL [6x1] - Displacement of each strut [m]
%
@@ -19,7 +19,10 @@ arguments
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
end
X = stewart.J\args.dL;
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
J = stewart.kinematics.J;
X = J\args.dL;
P = X(1:3);

16
src/generateCubicConfiguration.m
View File

@@ -5,7 +5,7 @@ function [stewart] = generateCubicConfiguration(stewart, args)
%
% Inputs:
% - stewart - A structure with the following fields
% - H [1x1] - Total height of the platform [m]
% - geometry.H [1x1] - Total height of the platform [m]
% - args - Can have the following fields:
% - Hc [1x1] - Height of the "useful" part of the cube [m]
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
@@ -14,8 +14,8 @@ function [stewart] = generateCubicConfiguration(stewart, args)
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
arguments
stewart
@@ -25,6 +25,9 @@ arguments
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
end
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
H = stewart.geometry.H;
sx = [ 2; -1; -1];
sy = [ 0; 1; -1];
sz = [ 1; 1; 1];
@@ -40,5 +43,8 @@ CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresp
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;

15
src/generateGeneralConfiguration.m
View File

@@ -14,8 +14,8 @@ function [stewart] = generateGeneralConfiguration(stewart, args)
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
arguments
stewart
@@ -27,10 +27,13 @@ arguments
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
end
stewart.Fa = zeros(3,6);
stewart.Mb = zeros(3,6);
Fa = zeros(3,6);
Mb = zeros(3,6);
for i = 1:6
stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
end
stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;

46
src/initializeAmplifiedStrutDynamics.m
View File

@@ -5,35 +5,39 @@ function [stewart] = initializeAmplifiedStrutDynamics(stewart, args)
%
% Inputs:
% - args - Structure with the following fields:
% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
% - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
% - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
% - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m]
% - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - Ki [6x1] - Total Stiffness of each strut [N/m]
% - Ci [6x1] - Total Damping of each strut [N/(m/s)]
% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
% - actuators.type = 2
% - actuators.K [6x1] - Total Stiffness of each strut [N/m]
% - actuators.C [6x1] - Total Damping of each strut [N/(m/s)]
% - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
% - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
% - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
% - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
arguments
stewart
args.Kdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1)
args.Cdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.Ksi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1)
args.Csi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1)
args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1)
args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
end
stewart.Ksi = args.Ksi;
stewart.Csi = args.Csi;
K = args.Ka + args.Kr;
C = args.Ca + args.Cr;
stewart.Kdi = args.Kdi;
stewart.Cdi = args.Cdi;
stewart.actuators.type = 2;
stewart.Ki = args.Ksi + args.Kdi;
stewart.Ci = args.Csi + args.Cdi;
stewart.actuators.Ka = args.Ka;
stewart.actuators.Ca = args.Ca;
stewart.actuator_type = 2;
stewart.actuators.Kr = args.Kr;
stewart.actuators.Cr = args.Cr;
stewart.actuators.K = K;
stewart.actuators.C = K;

52
src/initializeCylindricalPlatforms.m
View File

@@ -14,15 +14,17 @@ function [stewart] = initializeCylindricalPlatforms(stewart, args)
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - platforms [struct] - structure with the following fields:
% - Fpm [1x1] - Fixed Platform Mass [kg]
% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
% - Fph [1x1] - Fixed Platform Height [m]
% - Fpr [1x1] - Fixed Platform Radius [m]
% - Mpm [1x1] - Mobile Platform Mass [kg]
% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
% - Mph [1x1] - Mobile Platform Height [m]
% - Mpr [1x1] - Mobile Platform Radius [m]
% - platform_F [struct] - structure with the following fields:
% - type = 1
% - M [1x1] - Fixed Platform Mass [kg]
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
% - H [1x1] - Fixed Platform Height [m]
% - R [1x1] - Fixed Platform Radius [m]
% - platform_M [struct] - structure with the following fields:
% - M [1x1] - Mobile Platform Mass [kg]
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
% - H [1x1] - Mobile Platform Height [m]
% - R [1x1] - Mobile Platform Radius [m]
arguments
stewart
@@ -34,20 +36,24 @@ arguments
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
end
platforms = struct();
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/2 *args.Fpm * args.Fpr^2]);
platforms.Fpm = args.Fpm;
platforms.Fph = args.Fph;
platforms.Fpr = args.Fpr;
platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
1/2 * platforms.Fpm * platforms.Fpr^2]);
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/2 *args.Mpm * args.Mpr^2]);
platforms.Mpm = args.Mpm;
platforms.Mph = args.Mph;
platforms.Mpr = args.Mpr;
platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
1/2 * platforms.Mpm * platforms.Mpr^2]);
stewart.platform_F.type = 1;
stewart.platforms = platforms;
stewart.platform_F.I = I_F;
stewart.platform_F.M = args.Fpm;
stewart.platform_F.R = args.Fpr;
stewart.platform_F.H = args.Fph;
stewart.platform_M.type = 1;
stewart.platform_M.I = I_M;
stewart.platform_M.M = args.Mpm;
stewart.platform_M.R = args.Mpr;
stewart.platform_M.H = args.Mph;

63
src/initializeCylindricalStruts.m
View File

@@ -14,15 +14,16 @@ function [stewart] = initializeCylindricalStruts(stewart, args)
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - struts [struct] - structure with the following fields:
% - Fsm [6x1] - Mass of the Fixed part of the struts [kg]
% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
% - Msm [6x1] - Mass of the Mobile part of the struts [kg]
% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m]
% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m]
% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m]
% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m]
% - struts_F [struct] - structure with the following fields:
% - M [6x1] - Mass of the Fixed part of the struts [kg]
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
% - struts_M [struct] - structure with the following fields:
% - M [6x1] - Mass of the Mobile part of the struts [kg]
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
arguments
stewart
@@ -34,25 +35,37 @@ arguments
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
end
struts = struct();
Fsm = ones(6,1).*args.Fsm;
Fsh = ones(6,1).*args.Fsh;
Fsr = ones(6,1).*args.Fsr;
struts.Fsm = ones(6,1).*args.Fsm;
struts.Msm = ones(6,1).*args.Msm;
Msm = ones(6,1).*args.Msm;
Msh = ones(6,1).*args.Msh;
Msr = ones(6,1).*args.Msr;
struts.Fsh = ones(6,1).*args.Fsh;
struts.Fsr = ones(6,1).*args.Fsr;
struts.Msh = ones(6,1).*args.Msh;
struts.Msr = ones(6,1).*args.Msr;
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
struts.Fsi = zeros(3, 3, 6);
struts.Msi = zeros(3, 3, 6);
for i = 1:6
struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
1/2 * struts.Fsm(i) * struts.Fsr(i)^2]);
struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
1/2 * struts.Msm(i) * struts.Msr(i)^2]);
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
1/2 * Fsm(i) * Fsr(i)^2]);
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
1/2 * Msm(i) * Msr(i)^2]);
end
stewart.struts = struts;
stewart.struts_M.type = 1;
stewart.struts_M.I = I_M;
stewart.struts_M.M = Msm;
stewart.struts_M.R = Msr;
stewart.struts_M.H = Msh;
stewart.struts_F.type = 1;
stewart.struts_F.I = I_F;
stewart.struts_F.M = Fsm;
stewart.struts_F.R = Fsr;
stewart.struts_F.H = Fsh;

24
src/initializeFramesPositions.m
View File

@@ -1,4 +1,4 @@
function [stewart] = initializeFramesPositions(args)
function [stewart] = initializeFramesPositions(stewart, args)
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
%
% Syntax: [stewart] = initializeFramesPositions(args)
@@ -10,22 +10,26 @@ function [stewart] = initializeFramesPositions(args)
%
% Outputs:
% - stewart - A structure with the following fields:
% - H [1x1] - Total Height of the Stewart Platform [m]
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
arguments
stewart
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
end
stewart = struct();
H = args.H; % Total Height of the Stewart Platform [m]
stewart.H = args.H; % Total Height of the Stewart Platform [m]
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
stewart.geometry.H = H;
stewart.geometry.FO_M = FO_M;
stewart.platform_M.MO_B = MO_B;
stewart.platform_F.FO_A = FO_A;

48
src/initializeInertialSensor.m Normal file
View File

@@ -0,0 +1,48 @@
function [stewart] = initializeInertialSensor(stewart, args)
% initializeInertialSensor - Initialize the inertial sensor in each strut
%
% Syntax: [stewart] = initializeInertialSensor(args)
%
% Inputs:
% - args - Structure with the following fields:
% - type - 'geophone', 'accelerometer', 'none'
% - mass [1x1] - Weight of the inertial mass [kg]
% - freq [1x1] - Cutoff frequency [Hz]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - stewart.sensors.inertial
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
% - K [1x1] - Stiffness [N/m]
% - C [1x1] - Damping [N/(m/s)]
% - M [1x1] - Inertial Mass [kg]
% - G [1x1] - Gain
arguments
stewart
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
end
sensor = struct();
switch args.type
case 'geophone'
sensor.type = 1;
sensor.M = args.mass;
sensor.K = sensor.M * (2*pi*args.freq)^2;
sensor.C = 2*sqrt(sensor.M * sensor.K);
case 'accelerometer'
sensor.type = 2;
sensor.M = args.mass;
sensor.K = sensor.M * (2*pi*args.freq)^2;
sensor.C = 2*sqrt(sensor.M * sensor.K);
sensor.G = -sensor.K/sensor.M;
case 'none'
sensor.type = 3;
end
stewart.sensors.inertial = sensor;

110
src/initializeJointDynamics.m
View File

@@ -5,46 +5,88 @@ function [stewart] = initializeJointDynamics(stewart, args)
%
% Inputs:
% - args - Structure with the following fields:
% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [(N.m)/rad]
% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [(N.m)/(rad/s)]
% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [(N.m)/rad]
% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [(N.m)/(rad/s)]
% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [(N.m)/rad]
% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [(N.m)/(rad/s)]
% - disable [boolean] - Sets all the stiffness/damping to zero
% - type_F - 'universal', 'spherical', 'univesal_p', 'spherical_p'
% - type_M - 'universal', 'spherical', 'univesal_p', 'spherical_p'
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [(N.m)/rad]
% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [(N.m)/(rad/s)]
% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [(N.m)/rad]
% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [(N.m)/(rad/s)]
% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [(N.m)/rad]
% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [(N.m)/(rad/s)]
% - stewart.joints_F and stewart.joints_M:
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
arguments
stewart
args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
args.Csbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
args.Cubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
args.disable logical {mustBeNumericOrLogical} = false
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'universal'
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'spherical'
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1)
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1)
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
end
if args.disable
stewart.Ksbi = zeros(6,1);
stewart.Csbi = zeros(6,1);
stewart.Ksti = zeros(6,1);
stewart.Csti = zeros(6,1);
stewart.Kubi = zeros(6,1);
stewart.Cubi = zeros(6,1);
else
stewart.Ksbi = args.Ksbi;
stewart.Csbi = args.Csbi;
stewart.Ksti = args.Ksti;
stewart.Csti = args.Csti;
stewart.Kubi = args.Kubi;
stewart.Cubi = args.Cubi;
switch args.type_F
case 'universal'
stewart.joints_F.type = 1;
case 'spherical'
stewart.joints_F.type = 2;
case 'universal_p'
stewart.joints_F.type = 3;
case 'spherical_p'
stewart.joints_F.type = 4;
end
switch args.type_M
case 'universal'
stewart.joints_M.type = 1;
case 'spherical'
stewart.joints_M.type = 2;
case 'universal_p'
stewart.joints_M.type = 3;
case 'spherical_p'
stewart.joints_M.type = 4;
end
stewart.joints_M.Kx = zeros(6,1);
stewart.joints_M.Ky = zeros(6,1);
stewart.joints_M.Kz = zeros(6,1);
stewart.joints_F.Kx = zeros(6,1);
stewart.joints_F.Ky = zeros(6,1);
stewart.joints_F.Kz = zeros(6,1);
stewart.joints_M.Cx = zeros(6,1);
stewart.joints_M.Cy = zeros(6,1);
stewart.joints_M.Cz = zeros(6,1);
stewart.joints_F.Cx = zeros(6,1);
stewart.joints_F.Cy = zeros(6,1);
stewart.joints_F.Cz = zeros(6,1);
stewart.joints_M.Kf = args.Kf_M;
stewart.joints_M.Kt = args.Kf_M;
stewart.joints_F.Kf = args.Kf_F;
stewart.joints_F.Kt = args.Kf_F;
stewart.joints_M.Cf = args.Cf_M;
stewart.joints_M.Ct = args.Cf_M;
stewart.joints_F.Cf = args.Cf_F;
stewart.joints_F.Ct = args.Cf_F;

31
src/initializeStewartPlatform.m Normal file
View File

@@ -0,0 +1,31 @@
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();

4
src/initializeStewartPose.m
View File

@@ -14,7 +14,7 @@ function [stewart] = initializeStewartPose(stewart, args)
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - dLi[6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
arguments
stewart
@@ -24,4 +24,4 @@ end
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
stewart.dLi = dLi;
stewart.actuators.Leq = dLi;

19
src/initializeStrutDynamics.m
View File

@@ -5,21 +5,22 @@ function [stewart] = initializeStrutDynamics(stewart, args)
%
% Inputs:
% - args - Structure with the following fields:
% - Ki [6x1] - Stiffness of each strut [N/m]
% - Ci [6x1] - Damping of each strut [N/(m/s)]
% - K [6x1] - Stiffness of each strut [N/m]
% - C [6x1] - Damping of each strut [N/(m/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - Ki [6x1] - Stiffness of each strut [N/m]
% - Ci [6x1] - Damping of each strut [N/(m/s)]
% - actuators.type = 1
% - actuators.K [6x1] - Stiffness of each strut [N/m]
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
arguments
stewart
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1)
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1)
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
end
stewart.Ki = args.Ki;
stewart.Ci = args.Ci;
stewart.actuators.type = 1;
stewart.actuator_type = 1;
stewart.actuators.K = args.K;
stewart.actuators.C = args.C;

18
src/inverseKinematics.m
View File

@@ -5,8 +5,9 @@ function [Li, dLi] = inverseKinematics(stewart, args)
%
% Inputs:
% - stewart - A structure with the following fields
% - Aa [3x6] - The positions ai expressed in {A}
% - Bb [3x6] - The positions bi expressed in {B}
% - geometry.Aa [3x6] - The positions ai expressed in {A}
% - geometry.Bb [3x6] - The positions bi expressed in {B}
% - geometry.l [6x1] - Length of each strut
% - args - Can have the following fields:
% - AP [3x1] - The wanted position of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
@@ -21,6 +22,15 @@ arguments
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
Aa = stewart.geometry.Aa;
dLi = Li-stewart.l;
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
Bb = stewart.geometry.Bb;
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
l = stewart.geometry.l;
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
dLi = Li-l;