Compare commits
No commits in common. "1c9d20fbba250125fd340d8172eafd0fc39e0b41" and "e69dbf4ebd3fef9426025517f1295060ad7e5efe" have entirely different histories.
1c9d20fbba
...
e69dbf4ebd
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 48 KiB |
File diff suppressed because it is too large
Load Diff
|
Before Width: | Height: | Size: 78 KiB |
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 44 KiB |
File diff suppressed because it is too large
Load Diff
|
Before Width: | Height: | Size: 164 KiB |
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 42 KiB |
File diff suppressed because it is too large
Load Diff
|
Before Width: | Height: | Size: 164 KiB |
Binary file not shown.
@ -1,37 +0,0 @@
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - 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:
|
||||
% - geometry.J [6x6] - The Jacobian Matrix
|
||||
% - geometry.K [6x6] - The Stiffness Matrix
|
||||
% - geometry.C [6x6] - The Compliance Matrix
|
||||
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
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.geometry.J = J;
|
||||
stewart.geometry.K = K;
|
||||
stewart.geometry.C = C;
|
||||
|
||||
end
|
||||
@ -1,80 +0,0 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - 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
|
||||
% - 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}
|
||||
|
||||
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;
|
||||
|
||||
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, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
||||
FO_M = stewart.geometry.FO_M;
|
||||
|
||||
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
|
||||
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));
|
||||
|
||||
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;
|
||||
|
||||
end
|
||||
@ -1,77 +0,0 @@
|
||||
function [] = describeStewartPlatform(stewart)
|
||||
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
|
||||
%
|
||||
% Syntax: [] = describeStewartPlatform(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart
|
||||
%
|
||||
% Outputs:
|
||||
|
||||
arguments
|
||||
stewart
|
||||
end
|
||||
|
||||
fprintf('GEOMETRY:\n')
|
||||
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
|
||||
|
||||
if stewart.platform_M.MO_B(3) > 0
|
||||
fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3))
|
||||
else
|
||||
fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
|
||||
end
|
||||
|
||||
fprintf('- The initial length of the struts are:\n')
|
||||
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('ACTUATORS:\n')
|
||||
if stewart.actuators.type == 1
|
||||
fprintf('- The actuators are modelled as 1DoF.\n')
|
||||
fprintf('- The Stiffness and Damping of each actuators is:\n')
|
||||
fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
|
||||
elseif stewart.actuators.type == 2
|
||||
fprintf('- The actuators are modelled as 2DoF (APA).\n')
|
||||
fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
|
||||
fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
|
||||
fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
|
||||
fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
|
||||
elseif stewart.actuators.type == 3
|
||||
fprintf('- The actuators are modelled with a flexible element (FEM).\n')
|
||||
end
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('JOINTS:\n')
|
||||
|
||||
switch stewart.joints_F.type
|
||||
case 1
|
||||
fprintf('- The joints on the fixed based are universal joints (2DoF)\n')
|
||||
case 2
|
||||
fprintf('- The joints on the fixed based are spherical joints (3DoF)\n')
|
||||
end
|
||||
|
||||
switch stewart.joints_M.type
|
||||
case 1
|
||||
fprintf('- The joints on the mobile based are universal joints (2DoF)\n')
|
||||
case 2
|
||||
fprintf('- The joints on the mobile based are spherical joints (3DoF)\n')
|
||||
end
|
||||
|
||||
fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
|
||||
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
|
||||
|
||||
fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
|
||||
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('KINEMATICS:\n')
|
||||
|
||||
if isfield(stewart.kinematics, 'K')
|
||||
fprintf('- The Stiffness matrix K is (in [N/m]):\n')
|
||||
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
|
||||
end
|
||||
|
||||
if isfield(stewart.kinematics, 'C')
|
||||
fprintf('- The Damping matrix C is (in [m/N]):\n')
|
||||
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
|
||||
end
|
||||
@ -1,240 +0,0 @@
|
||||
function [] = displayArchitecture(stewart, args)
|
||||
% displayArchitecture - 3D plot of the Stewart platform architecture
|
||||
%
|
||||
% Syntax: [] = displayArchitecture(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart
|
||||
% - args - Structure with 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}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
% - F_color [color] - Color used for the Fixed elements
|
||||
% - M_color [color] - Color used for the Mobile elements
|
||||
% - L_color [color] - Color used for the Legs elements
|
||||
% - frames [true/false] - Display the Frames
|
||||
% - legs [true/false] - Display the Legs
|
||||
% - joints [true/false] - Display the Joints
|
||||
% - labels [true/false] - Display the Labels
|
||||
% - platforms [true/false] - Display the Platforms
|
||||
% - views ['all', 'xy', 'yz', 'xz', 'default'] -
|
||||
%
|
||||
% Outputs:
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
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.platforms logical {mustBeNumericOrLogical} = true
|
||||
args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
|
||||
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;
|
||||
|
||||
if ~strcmp(args.views, 'all')
|
||||
figure;
|
||||
else
|
||||
f = figure('visible', 'off');
|
||||
end
|
||||
|
||||
hold on;
|
||||
|
||||
FTa = [eye(3), FO_A; ...
|
||||
zeros(1,3), 1];
|
||||
ATb = [args.ARB, args.AP; ...
|
||||
zeros(1,3), 1];
|
||||
BTm = [eye(3), -MO_B; ...
|
||||
zeros(1,3), 1];
|
||||
|
||||
FTm = FTa*ATb*BTm;
|
||||
|
||||
d_unit_vector = H/4;
|
||||
|
||||
d_label = H/20;
|
||||
|
||||
Ff = [0, 0, 0];
|
||||
if args.frames
|
||||
quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(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(Ff(1) + d_label, ...
|
||||
Ff(2) + d_label, ...
|
||||
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
|
||||
end
|
||||
end
|
||||
|
||||
if args.frames
|
||||
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(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 && 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.platform_F.R; % Radius of the circle [m]
|
||||
|
||||
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||||
|
||||
plot3(points(1,:), ...
|
||||
points(2,:), ...
|
||||
points(3,:), '-', 'Color', args.F_color);
|
||||
end
|
||||
|
||||
if args.joints
|
||||
scatter3(Fa(1,:), ...
|
||||
Fa(2,:), ...
|
||||
Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
||||
if args.labels
|
||||
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
|
||||
|
||||
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
|
||||
|
||||
if args.frames
|
||||
FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||||
quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
|
||||
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
|
||||
|
||||
if args.labels
|
||||
text(Fm(1) + d_label, ...
|
||||
Fm(2) + d_label, ...
|
||||
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
|
||||
end
|
||||
end
|
||||
|
||||
FB = FO_A + args.AP;
|
||||
|
||||
if args.frames
|
||||
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||||
quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
|
||||
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
|
||||
|
||||
if args.labels
|
||||
text(FB(1) - d_label, ...
|
||||
FB(2) + d_label, ...
|
||||
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
|
||||
end
|
||||
end
|
||||
|
||||
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.platform_M.R; % Radius of the circle [m]
|
||||
|
||||
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||||
|
||||
plot3(points(1,:), ...
|
||||
points(2,:), ...
|
||||
points(3,:), '-', 'Color', args.M_color);
|
||||
end
|
||||
|
||||
if args.joints
|
||||
Fb = FTm*[Mb;ones(1,6)];
|
||||
|
||||
scatter3(Fb(1,:), ...
|
||||
Fb(2,:), ...
|
||||
Fb(3,:), 'MarkerEdgeColor', args.M_color);
|
||||
|
||||
if args.labels
|
||||
for i = 1:size(Fb,2)
|
||||
text(Fb(1,i) + d_label, ...
|
||||
Fb(2,i), ...
|
||||
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
if args.legs
|
||||
for i = 1:6
|
||||
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((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
|
||||
|
||||
switch args.views
|
||||
case 'default'
|
||||
view([1 -0.6 0.4]);
|
||||
case 'xy'
|
||||
view([0 0 1]);
|
||||
case 'xz'
|
||||
view([0 -1 0]);
|
||||
case 'yz'
|
||||
view([1 0 0]);
|
||||
end
|
||||
axis equal;
|
||||
axis off;
|
||||
|
||||
if strcmp(args.views, 'all')
|
||||
hAx = findobj('type', 'axes');
|
||||
|
||||
figure;
|
||||
s1 = subplot(2,2,1);
|
||||
copyobj(get(hAx(1), 'Children'), s1);
|
||||
view([0 0 1]);
|
||||
axis equal;
|
||||
axis off;
|
||||
title('Top')
|
||||
|
||||
s2 = subplot(2,2,2);
|
||||
copyobj(get(hAx(1), 'Children'), s2);
|
||||
view([1 -0.6 0.4]);
|
||||
axis equal;
|
||||
axis off;
|
||||
|
||||
s3 = subplot(2,2,3);
|
||||
copyobj(get(hAx(1), 'Children'), s3);
|
||||
view([1 0 0]);
|
||||
axis equal;
|
||||
axis off;
|
||||
title('Front')
|
||||
|
||||
s4 = subplot(2,2,4);
|
||||
copyobj(get(hAx(1), 'Children'), s4);
|
||||
view([0 -1 0]);
|
||||
axis equal;
|
||||
axis off;
|
||||
title('Side')
|
||||
|
||||
close(f);
|
||||
end
|
||||
@ -1,41 +0,0 @@
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - 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
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
Fa = zeros(3,6);
|
||||
Mb = zeros(3,6);
|
||||
|
||||
for i = 1:6
|
||||
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;
|
||||
|
||||
end
|
||||
@ -1,45 +0,0 @@
|
||||
function [] = initializeController(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
|
||||
end
|
||||
|
||||
controller = struct();
|
||||
|
||||
switch args.type
|
||||
case 'open-loop'
|
||||
controller.type = 1;
|
||||
controller.name = 'Open-Loop';
|
||||
case 'dvf'
|
||||
controller.type = 2;
|
||||
controller.name = 'Decentralized Direct Velocity Feedback';
|
||||
case 'iff'
|
||||
controller.type = 3;
|
||||
controller.name = 'Decentralized Integral Force Feedback';
|
||||
case 'hac-dvf'
|
||||
controller.type = 4;
|
||||
controller.name = 'HAC-DVF';
|
||||
case 'ref-track-L'
|
||||
controller.type = 5;
|
||||
controller.name = 'Reference Tracking in the frame of the legs';
|
||||
case 'ref-track-iff-L'
|
||||
controller.type = 6;
|
||||
controller.name = 'Reference Tracking in the frame of the legs + IFF';
|
||||
case 'cascade-hac-lac'
|
||||
controller.type = 7;
|
||||
controller.name = 'Cascade Control + HAC-LAC';
|
||||
case 'hac-iff'
|
||||
controller.type = 8;
|
||||
controller.name = 'HAC-IFF';
|
||||
case 'stabilizing'
|
||||
controller.type = 9;
|
||||
controller.name = 'Stabilizing Controller';
|
||||
end
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
save('mat/nano_hexapod_model_controller.mat', 'controller');
|
||||
elseif exist('./matlab', 'dir')
|
||||
save('matlab/mat/nano_hexapod_model_controller.mat', 'controller');
|
||||
end
|
||||
|
||||
end
|
||||
@ -1,61 +0,0 @@
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - 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
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
end
|
||||
|
||||
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]);
|
||||
|
||||
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]);
|
||||
|
||||
stewart.platform_F.type = 1;
|
||||
|
||||
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;
|
||||
|
||||
end
|
||||
@ -1,50 +0,0 @@
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - 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
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
end
|
||||
|
||||
stewart.struts_M.type = 1;
|
||||
|
||||
stewart.struts_M.M = args.Msm;
|
||||
stewart.struts_M.R = args.Msr;
|
||||
stewart.struts_M.H = args.Msh;
|
||||
|
||||
stewart.struts_F.type = 1;
|
||||
|
||||
stewart.struts_F.M = args.Fsm;
|
||||
stewart.struts_F.R = args.Fsr;
|
||||
stewart.struts_F.H = args.Fsh;
|
||||
|
||||
end
|
||||
@ -1,37 +0,0 @@
|
||||
function [stewart] = initializeFramesPositions(stewart, args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - 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
|
||||
|
||||
H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
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.geometry.H = H;
|
||||
stewart.geometry.FO_M = FO_M;
|
||||
stewart.platform_M.MO_B = MO_B;
|
||||
stewart.platform_F.FO_A = FO_A;
|
||||
|
||||
end
|
||||
@ -1,129 +0,0 @@
|
||||
function [stewart] = initializeJointDynamics(stewart, args)
|
||||
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
|
||||
%
|
||||
% Syntax: [stewart] = initializeJointDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
||||
% - type_M - 'universal', 'spherical', 'universal_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:
|
||||
% - 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.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
||||
args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
||||
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.K_M double {mustBeNumeric} = zeros(6,6)
|
||||
args.M_M double {mustBeNumeric} = zeros(6,6)
|
||||
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
||||
args.xi_M double {mustBeNumeric} = 0.1
|
||||
args.step_file_M char {} = ''
|
||||
args.K_F double {mustBeNumeric} = zeros(6,6)
|
||||
args.M_F double {mustBeNumeric} = zeros(6,6)
|
||||
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
||||
args.xi_F double {mustBeNumeric} = 0.1
|
||||
args.step_file_F char {} = ''
|
||||
end
|
||||
|
||||
switch args.type_F
|
||||
case '2dof'
|
||||
stewart.joints_F.type = 1;
|
||||
case '3dof'
|
||||
stewart.joints_F.type = 2;
|
||||
case '4dof'
|
||||
stewart.joints_F.type = 3;
|
||||
case '6dof'
|
||||
stewart.joints_F.type = 4;
|
||||
case 'flexible'
|
||||
stewart.joints_F.type = 5;
|
||||
otherwise
|
||||
error("joints_F are not correctly defined")
|
||||
end
|
||||
|
||||
switch args.type_M
|
||||
case '2dof'
|
||||
stewart.joints_M.type = 1;
|
||||
case '3dof'
|
||||
stewart.joints_M.type = 2;
|
||||
case '4dof'
|
||||
stewart.joints_M.type = 3;
|
||||
case '6dof'
|
||||
stewart.joints_M.type = 4;
|
||||
case 'flexible'
|
||||
stewart.joints_M.type = 5;
|
||||
otherwise
|
||||
error("joints_M are not correctly defined")
|
||||
end
|
||||
|
||||
stewart.joints_M.Ka = args.Ka_M;
|
||||
stewart.joints_M.Kr = args.Kr_M;
|
||||
|
||||
stewart.joints_F.Ka = args.Ka_F;
|
||||
stewart.joints_F.Kr = args.Kr_F;
|
||||
|
||||
stewart.joints_M.Ca = args.Ca_M;
|
||||
stewart.joints_M.Cr = args.Cr_M;
|
||||
|
||||
stewart.joints_F.Ca = args.Ca_F;
|
||||
stewart.joints_F.Cr = args.Cr_F;
|
||||
|
||||
stewart.joints_M.Kf = args.Kf_M;
|
||||
stewart.joints_M.Kt = args.Kt_M;
|
||||
|
||||
stewart.joints_F.Kf = args.Kf_F;
|
||||
stewart.joints_F.Kt = args.Kt_F;
|
||||
|
||||
stewart.joints_M.Cf = args.Cf_M;
|
||||
stewart.joints_M.Ct = args.Ct_M;
|
||||
|
||||
stewart.joints_F.Cf = args.Cf_F;
|
||||
stewart.joints_F.Ct = args.Ct_F;
|
||||
|
||||
stewart.joints_F.M = args.M_F;
|
||||
stewart.joints_F.K = args.K_F;
|
||||
stewart.joints_F.n_xyz = args.n_xyz_F;
|
||||
stewart.joints_F.xi = args.xi_F;
|
||||
stewart.joints_F.xi = args.xi_F;
|
||||
stewart.joints_F.step_file = args.step_file_F;
|
||||
|
||||
stewart.joints_M.M = args.M_M;
|
||||
stewart.joints_M.K = args.K_M;
|
||||
stewart.joints_M.n_xyz = args.n_xyz_M;
|
||||
stewart.joints_M.xi = args.xi_M;
|
||||
stewart.joints_M.step_file = args.step_file_M;
|
||||
|
||||
end
|
||||
@ -1,33 +0,0 @@
|
||||
function [] = initializeLoggingConfiguration(args)
|
||||
|
||||
arguments
|
||||
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
|
||||
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||||
end
|
||||
|
||||
conf_log = struct();
|
||||
|
||||
switch args.log
|
||||
case 'none'
|
||||
conf_log.type = 0;
|
||||
case 'all'
|
||||
conf_log.type = 1;
|
||||
case 'forces'
|
||||
conf_log.type = 2;
|
||||
end
|
||||
|
||||
conf_log.Ts = args.Ts;
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nano_hexapod_model_conf_log.mat', 'file')
|
||||
save('mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append');
|
||||
else
|
||||
save('mat/nano_hexapod_model_conf_log.mat', 'conf_log');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nano_hexapod_model_conf_log.mat', 'file')
|
||||
save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append');
|
||||
else
|
||||
save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log');
|
||||
end
|
||||
end
|
||||
@ -1,38 +0,0 @@
|
||||
function [sample] = initializeSample(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none'
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 200e-3 % Height [m]
|
||||
args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m]
|
||||
args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg]
|
||||
end
|
||||
|
||||
sample = struct();
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
sample.type = 0;
|
||||
sample.m = 0;
|
||||
case 'cylindrical'
|
||||
sample.type = 1;
|
||||
|
||||
sample.H = args.H;
|
||||
sample.R = args.R;
|
||||
sample.m = args.m;
|
||||
end
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nano_hexapod.mat', 'file')
|
||||
save('mat/nano_hexapod.mat', 'sample', '-append');
|
||||
else
|
||||
save('mat/nano_hexapod.mat', 'sample');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nano_hexapod.mat', 'file')
|
||||
save('matlab/mat/nano_hexapod.mat', 'sample', '-append');
|
||||
else
|
||||
save('matlab/mat/nano_hexapod.mat', 'sample');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@ -1,141 +0,0 @@
|
||||
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)
|
||||
|
||||
arguments
|
||||
%% initializeFramesPositions
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
||||
%% generateGeneralConfiguration
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of fixed joints [m]
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
|
||||
args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of mobile joints [m]
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
|
||||
args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
|
||||
%% Actuators
|
||||
args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
||||
args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 380000
|
||||
args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605
|
||||
args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302
|
||||
args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 5
|
||||
args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100
|
||||
args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50
|
||||
%% initializeCylindricalPlatforms
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
|
||||
%% initializeCylindricalStruts
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
||||
%% Bottom and Top Flexible Joints
|
||||
args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
||||
args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
||||
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
%% inverseKinematics
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
stewart = initializeStewartPlatform();
|
||||
|
||||
stewart = initializeFramesPositions(stewart, ...
|
||||
'H', args.H, ...
|
||||
'MO_B', args.MO_B);
|
||||
|
||||
stewart = generateGeneralConfiguration(stewart, ...
|
||||
'FH', args.FH, ...
|
||||
'FR', args.FR, ...
|
||||
'FTh', args.FTh, ...
|
||||
'MH', args.MH, ...
|
||||
'MR', args.MR, ...
|
||||
'MTh', args.MTh);
|
||||
|
||||
stewart = computeJointsPose(stewart);
|
||||
|
||||
stewart = initializeStrutDynamics(stewart, ...
|
||||
'type', args.actuator_type, ...
|
||||
'k', args.actuator_k, ...
|
||||
'ke', args.actuator_ke, ...
|
||||
'ka', args.actuator_ka, ...
|
||||
'c', args.actuator_c, ...
|
||||
'ce', args.actuator_ce, ...
|
||||
'ca', args.actuator_ca);
|
||||
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', args.flex_type_F, ...
|
||||
'type_M', args.flex_type_M, ...
|
||||
'Kf_M', args.Kf_M, ...
|
||||
'Cf_M', args.Cf_M, ...
|
||||
'Kt_M', args.Kt_M, ...
|
||||
'Ct_M', args.Ct_M, ...
|
||||
'Kf_F', args.Kf_F, ...
|
||||
'Cf_F', args.Cf_F, ...
|
||||
'Kt_F', args.Kt_F, ...
|
||||
'Ct_F', args.Ct_F, ...
|
||||
'Ka_F', args.Ka_F, ...
|
||||
'Ca_F', args.Ca_F, ...
|
||||
'Kr_F', args.Kr_F, ...
|
||||
'Cr_F', args.Cr_F, ...
|
||||
'Ka_M', args.Ka_M, ...
|
||||
'Ca_M', args.Ca_M, ...
|
||||
'Kr_M', args.Kr_M, ...
|
||||
'Cr_M', args.Cr_M);
|
||||
|
||||
stewart = initializeCylindricalPlatforms(stewart, ...
|
||||
'Fpm', args.Fpm, ...
|
||||
'Fph', args.Fph, ...
|
||||
'Fpr', args.Fpr, ...
|
||||
'Mpm', args.Mpm, ...
|
||||
'Mph', args.Mph, ...
|
||||
'Mpr', args.Mpr);
|
||||
|
||||
stewart = initializeCylindricalStruts(stewart, ...
|
||||
'Fsm', args.Fsm, ...
|
||||
'Fsh', args.Fsh, ...
|
||||
'Fsr', args.Fsr, ...
|
||||
'Msm', args.Msm, ...
|
||||
'Msh', args.Msh, ...
|
||||
'Msr', args.Msr);
|
||||
|
||||
stewart = computeJacobian(stewart);
|
||||
|
||||
stewart = initializeStewartPose(stewart, ...
|
||||
'AP', args.AP, ...
|
||||
'ARB', args.ARB);
|
||||
|
||||
nano_hexapod = stewart;
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nano_hexapod.mat', 'file')
|
||||
save('mat/nano_hexapod.mat', 'nano_hexapod', '-append');
|
||||
else
|
||||
save('mat/nano_hexapod.mat', 'nano_hexapod');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nano_hexapod.mat', 'file')
|
||||
save('matlab/mat/nano_hexapod.mat', 'nano_hexapod', '-append');
|
||||
else
|
||||
save('matlab/mat/nano_hexapod.mat', 'nano_hexapod');
|
||||
end
|
||||
end
|
||||
end
|
||||
@ -1,33 +0,0 @@
|
||||
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();
|
||||
|
||||
end
|
||||
@ -1,29 +0,0 @@
|
||||
function [stewart] = initializeStewartPose(stewart, args)
|
||||
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
|
||||
% It uses the inverse kinematic
|
||||
%
|
||||
% Syntax: [stewart] = initializeStewartPose(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}
|
||||
% - 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}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - 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
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
||||
|
||||
stewart.actuators.Leq = dLi;
|
||||
|
||||
end
|
||||
@ -1,54 +0,0 @@
|
||||
function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - 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:
|
||||
% - 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.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
||||
args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
|
||||
args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
|
||||
args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
|
||||
args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
|
||||
args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6
|
||||
args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
|
||||
|
||||
args.F_gain (1,1) double {mustBeNumeric} = 1
|
||||
args.me (1,1) double {mustBeNumeric} = 0.01
|
||||
args.ma (1,1) double {mustBeNumeric} = 0.01
|
||||
end
|
||||
|
||||
if strcmp(args.type, '1dof')
|
||||
stewart.actuators.type = 1;
|
||||
elseif strcmp(args.type, '2dof')
|
||||
stewart.actuators.type = 2;
|
||||
elseif strcmp(args.type, 'flexible')
|
||||
stewart.actuators.type = 3;
|
||||
end
|
||||
|
||||
stewart.actuators.k = args.k;
|
||||
stewart.actuators.c = args.c;
|
||||
|
||||
stewart.actuators.ka = args.ka;
|
||||
stewart.actuators.ca = args.ca;
|
||||
|
||||
stewart.actuators.ke = args.ke;
|
||||
stewart.actuators.ce = args.ce;
|
||||
|
||||
stewart.actuators.F_gain = args.F_gain;
|
||||
|
||||
stewart.actuators.ma = args.ma;
|
||||
stewart.actuators.me = args.me;
|
||||
|
||||
end
|
||||
@ -1,38 +0,0 @@
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - 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}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - 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}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
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;
|
||||
|
||||
end
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user