Update Stewart Platforms initialization
This commit is contained in:
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+116
-37
@@ -861,25 +861,68 @@ The =rz= structure is saved.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
||||
micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
||||
micro_hexapod = computeJointsPose(micro_hexapod);
|
||||
micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
|
||||
micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
||||
micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
||||
micro_hexapod = computeJacobian(micro_hexapod);
|
||||
[Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
||||
micro_hexapod.Li = Li;
|
||||
micro_hexapod.dLi = dLi;
|
||||
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);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStrutDynamics(stewart, ...
|
||||
'K', args.Ki, ...
|
||||
'C', args.Ci);
|
||||
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', 'universal_p', ...
|
||||
'type_M', 'spherical_p');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
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);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
#+end_src
|
||||
|
||||
Equilibrium position of the each joint.
|
||||
#+begin_src matlab
|
||||
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||
load('mat/Foffset.mat', 'Fhm');
|
||||
micro_hexapod.dLeq = -Fhm'./args.Ki;
|
||||
stewart.actuators.dLeq = -Fhm'./args.Ki;
|
||||
else
|
||||
micro_hexapod.dLeq = zeros(6,1);
|
||||
stewart.actuators.dLeq = zeros(6,1);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
@@ -890,17 +933,17 @@ Equilibrium position of the each joint.
|
||||
#+begin_src matlab
|
||||
switch args.type
|
||||
case 'none'
|
||||
micro_hexapod.type = 0;
|
||||
stewart.type = 0;
|
||||
case 'rigid'
|
||||
micro_hexapod.type = 1;
|
||||
stewart.type = 1;
|
||||
case 'flexible'
|
||||
micro_hexapod.type = 2;
|
||||
stewart.type = 2;
|
||||
case 'modal-analysis'
|
||||
micro_hexapod.type = 3;
|
||||
stewart.type = 3;
|
||||
case 'init'
|
||||
micro_hexapod.type = 4;
|
||||
stewart.type = 4;
|
||||
case 'compliance'
|
||||
micro_hexapod.type = 5;
|
||||
stewart.type = 5;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
@@ -910,6 +953,7 @@ Equilibrium position of the each joint.
|
||||
:END:
|
||||
The =micro_hexapod= structure is saved.
|
||||
#+begin_src matlab
|
||||
micro_hexapod = stewart;
|
||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||
#+end_src
|
||||
|
||||
@@ -1238,6 +1282,17 @@ The =mirror= structure is saved.
|
||||
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||
args.k (1,1) double {mustBeNumeric} = -1
|
||||
args.c (1,1) double {mustBeNumeric} = -1
|
||||
% initializeJointDynamics
|
||||
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'universal'
|
||||
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_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)
|
||||
% initializeCylindricalPlatforms
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
@@ -1267,32 +1322,55 @@ The =mirror= structure is saved.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
||||
nano_hexapod = generateGeneralConfiguration(nano_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
||||
nano_hexapod = computeJointsPose(nano_hexapod);
|
||||
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);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
if args.k > 0 && args.c > 0
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', args.k*ones(6,1), 'Ci', args.c*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', args.k*ones(6,1), 'C', args.c*ones(6,1));
|
||||
elseif args.k > 0
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', args.k*ones(6,1), 'Ci', 1.5*sqrt(args.k)*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', args.k*ones(6,1), 'C', 1.5*sqrt(args.k)*ones(6,1));
|
||||
elseif strcmp(args.actuator, 'piezo')
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e7*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', 1e7*ones(6,1), 'C', 1e2*ones(6,1));
|
||||
elseif strcmp(args.actuator, 'lorentz')
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e4*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', 1e4*ones(6,1), 'C', 1e2*ones(6,1));
|
||||
else
|
||||
error('args.actuator should be piezo or lorentz');
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
||||
nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
||||
nano_hexapod = computeJacobian(nano_hexapod);
|
||||
[Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
||||
nano_hexapod.Li = Li;
|
||||
nano_hexapod.dLi = dLi;
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', args.type_F, ...
|
||||
'type_M', args.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);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
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);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer');
|
||||
#+end_src
|
||||
|
||||
Equilibrium position of the each joint.
|
||||
@@ -1300,9 +1378,9 @@ Equilibrium position of the each joint.
|
||||
|
||||
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||
load('mat/Foffset.mat', 'Fnm');
|
||||
nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
|
||||
stewart.actuators.dLeq = -Fnm'./stewart.Ki;
|
||||
else
|
||||
nano_hexapod.dLeq = args.dLeq;
|
||||
stewart.actuators.dLeq = args.dLeq;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
@@ -1313,13 +1391,13 @@ Equilibrium position of the each joint.
|
||||
#+begin_src matlab
|
||||
switch args.type
|
||||
case 'none'
|
||||
nano_hexapod.type = 0;
|
||||
stewart.type = 0;
|
||||
case 'rigid'
|
||||
nano_hexapod.type = 1;
|
||||
stewart.type = 1;
|
||||
case 'flexible'
|
||||
nano_hexapod.type = 2;
|
||||
stewart.type = 2;
|
||||
case 'init'
|
||||
nano_hexapod.type = 4;
|
||||
stewart.type = 4;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
@@ -1328,6 +1406,7 @@ Equilibrium position of the each joint.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
nano_hexapod = stewart;
|
||||
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||||
#+end_src
|
||||
|
||||
|
||||
+1662
-309
File diff suppressed because it is too large
Load Diff
+22
-8
@@ -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;
|
||||
|
||||
+58
-27
@@ -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;
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
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 classical.\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 mechanicaly amplified.\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))
|
||||
end
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('JOINTS:\n')
|
||||
|
||||
switch stewart.joints_F.type
|
||||
case 1
|
||||
fprintf('- The joints on the fixed based are universal joints\n')
|
||||
case 2
|
||||
fprintf('- The joints on the fixed based are spherical joints\n')
|
||||
case 3
|
||||
fprintf('- The joints on the fixed based are perfect universal joints\n')
|
||||
case 4
|
||||
fprintf('- The joints on the fixed based are perfect spherical joints\n')
|
||||
end
|
||||
|
||||
switch stewart.joints_M.type
|
||||
case 1
|
||||
fprintf('- The joints on the mobile based are universal joints\n')
|
||||
case 2
|
||||
fprintf('- The joints on the mobile based are spherical joints\n')
|
||||
case 3
|
||||
fprintf('- The joints on the mobile based are perfect universal joints\n')
|
||||
case 4
|
||||
fprintf('- The joints on the mobile based are perfect spherical joints\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
|
||||
@@ -0,0 +1,240 @@
|
||||
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
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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,17 +14,20 @@ 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
|
||||
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||||
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||||
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||||
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;
|
||||
|
||||
@@ -14,23 +14,26 @@ 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
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-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} = 70e-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
|
||||
|
||||
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;
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
function [stewart] = initializeAmplifiedStrutDynamics(stewart, args)
|
||||
% initializeAmplifiedStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeAmplifiedStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - 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:
|
||||
% - 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.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
|
||||
|
||||
K = args.Ka + args.Kr;
|
||||
C = args.Ca + args.Cr;
|
||||
|
||||
stewart.actuators.type = 2;
|
||||
|
||||
stewart.actuators.Ka = args.Ka;
|
||||
stewart.actuators.Ca = args.Ca;
|
||||
|
||||
stewart.actuators.Kr = args.Kr;
|
||||
stewart.actuators.Cr = args.Cr;
|
||||
|
||||
stewart.actuators.K = K;
|
||||
stewart.actuators.C = K;
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
function [stewart] = initializeFramesPositions(args)
|
||||
function [stewart] = initializeFramesPositions(stewart, args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(args)
|
||||
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
@@ -0,0 +1,92 @@
|
||||
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,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'universal'
|
||||
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_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
|
||||
|
||||
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;
|
||||
@@ -36,37 +36,75 @@ arguments
|
||||
args.Foffset logical {mustBeNumericOrLogical} = false
|
||||
end
|
||||
|
||||
micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
||||
micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
||||
micro_hexapod = computeJointsPose(micro_hexapod);
|
||||
micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
|
||||
micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
||||
micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
||||
micro_hexapod = computeJacobian(micro_hexapod);
|
||||
[Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
||||
micro_hexapod.Li = Li;
|
||||
micro_hexapod.dLi = dLi;
|
||||
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, ...
|
||||
'K', args.Ki, ...
|
||||
'C', args.Ci);
|
||||
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', 'universal_p', ...
|
||||
'type_M', 'spherical_p');
|
||||
|
||||
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);
|
||||
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
|
||||
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||
load('mat/Foffset.mat', 'Fhm');
|
||||
micro_hexapod.dLeq = -Fhm'./args.Ki;
|
||||
stewart.actuators.dLeq = -Fhm'./args.Ki;
|
||||
else
|
||||
micro_hexapod.dLeq = zeros(6,1);
|
||||
stewart.actuators.dLeq = zeros(6,1);
|
||||
end
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
micro_hexapod.type = 0;
|
||||
stewart.type = 0;
|
||||
case 'rigid'
|
||||
micro_hexapod.type = 1;
|
||||
stewart.type = 1;
|
||||
case 'flexible'
|
||||
micro_hexapod.type = 2;
|
||||
stewart.type = 2;
|
||||
case 'modal-analysis'
|
||||
micro_hexapod.type = 3;
|
||||
stewart.type = 3;
|
||||
case 'init'
|
||||
micro_hexapod.type = 4;
|
||||
stewart.type = 4;
|
||||
case 'compliance'
|
||||
micro_hexapod.type = 5;
|
||||
stewart.type = 5;
|
||||
end
|
||||
|
||||
micro_hexapod = stewart;
|
||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||
|
||||
+50
-19
@@ -16,6 +16,17 @@ arguments
|
||||
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||
args.k (1,1) double {mustBeNumeric} = -1
|
||||
args.c (1,1) double {mustBeNumeric} = -1
|
||||
% initializeJointDynamics
|
||||
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'universal'
|
||||
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_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)
|
||||
% initializeCylindricalPlatforms
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
@@ -39,45 +50,65 @@ arguments
|
||||
args.Foffset logical {mustBeNumericOrLogical} = false
|
||||
end
|
||||
|
||||
nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
||||
nano_hexapod = generateGeneralConfiguration(nano_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
||||
nano_hexapod = computeJointsPose(nano_hexapod);
|
||||
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);
|
||||
|
||||
if args.k > 0 && args.c > 0
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', args.k*ones(6,1), 'Ci', args.c*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', args.k*ones(6,1), 'C', args.c*ones(6,1));
|
||||
elseif args.k > 0
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', args.k*ones(6,1), 'Ci', 1.5*sqrt(args.k)*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', args.k*ones(6,1), 'C', 1.5*sqrt(args.k)*ones(6,1));
|
||||
elseif strcmp(args.actuator, 'piezo')
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e7*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', 1e7*ones(6,1), 'C', 1e2*ones(6,1));
|
||||
elseif strcmp(args.actuator, 'lorentz')
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e4*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'K', 1e4*ones(6,1), 'C', 1e2*ones(6,1));
|
||||
else
|
||||
error('args.actuator should be piezo or lorentz');
|
||||
end
|
||||
|
||||
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
||||
nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
||||
nano_hexapod = computeJacobian(nano_hexapod);
|
||||
[Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
||||
nano_hexapod.Li = Li;
|
||||
nano_hexapod.dLi = dLi;
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', args.type_F, ...
|
||||
'type_M', args.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);
|
||||
|
||||
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);
|
||||
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer');
|
||||
|
||||
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||
load('mat/Foffset.mat', 'Fnm');
|
||||
nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
|
||||
stewart.actuators.dLeq = -Fnm'./stewart.Ki;
|
||||
else
|
||||
nano_hexapod.dLeq = args.dLeq;
|
||||
stewart.actuators.dLeq = args.dLeq;
|
||||
end
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
nano_hexapod.type = 0;
|
||||
stewart.type = 0;
|
||||
case 'rigid'
|
||||
nano_hexapod.type = 1;
|
||||
stewart.type = 1;
|
||||
case 'flexible'
|
||||
nano_hexapod.type = 2;
|
||||
stewart.type = 2;
|
||||
case 'init'
|
||||
nano_hexapod.type = 4;
|
||||
stewart.type = 4;
|
||||
end
|
||||
|
||||
nano_hexapod = stewart;
|
||||
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||||
|
||||
@@ -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();
|
||||
@@ -0,0 +1,27 @@
|
||||
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;
|
||||
@@ -5,19 +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, mustBePositive} = 1e6*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
||||
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
||||
end
|
||||
|
||||
stewart.Ki = args.Ki;
|
||||
stewart.Ci = args.Ci;
|
||||
stewart.actuators.type = 1;
|
||||
|
||||
stewart.actuators.K = args.K;
|
||||
stewart.actuators.C = args.C;
|
||||
|
||||
+14
-4
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user