Update Stewart Platforms initialization
This commit is contained in:
parent
7e615e74ae
commit
19c6456cd5
BIN
mat/conf_log.mat
BIN
mat/conf_log.mat
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
mat/stages.mat
BIN
mat/stages.mat
Binary file not shown.
BIN
matlab/Fixed_Based.slx
Normal file
BIN
matlab/Fixed_Based.slx
Normal file
Binary file not shown.
BIN
matlab/Mobile_Platform.slx
Normal file
BIN
matlab/Mobile_Platform.slx
Normal file
Binary file not shown.
BIN
matlab/micro_hexapod_strut.slx
Normal file
BIN
matlab/micro_hexapod_strut.slx
Normal file
Binary file not shown.
Binary file not shown.
BIN
matlab/nano_hexapod_strut.slx
Normal file
BIN
matlab/nano_hexapod_strut.slx
Normal file
Binary file not shown.
Binary file not shown.
BIN
matlab/stewart_strut.slx.r2019b
Normal file
BIN
matlab/stewart_strut.slx.r2019b
Normal file
Binary file not shown.
BIN
matlab/z_axis_accelerometer.slx
Normal file
BIN
matlab/z_axis_accelerometer.slx
Normal file
Binary file not shown.
BIN
matlab/z_axis_geophone.slx
Normal file
BIN
matlab/z_axis_geophone.slx
Normal file
Binary file not shown.
@ -861,25 +861,68 @@ The =rz= structure is saved.
|
|||||||
:UNNUMBERED: t
|
:UNNUMBERED: t
|
||||||
:END:
|
:END:
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
stewart = initializeStewartPlatform();
|
||||||
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);
|
stewart = initializeFramesPositions(stewart, ...
|
||||||
micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
|
'H', args.H, ...
|
||||||
micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
'MO_B', args.MO_B);
|
||||||
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);
|
stewart = generateGeneralConfiguration(stewart, ...
|
||||||
[Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
'FH', args.FH, ...
|
||||||
micro_hexapod.Li = Li;
|
'FR', args.FR, ...
|
||||||
micro_hexapod.dLi = dLi;
|
'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
|
#+end_src
|
||||||
|
|
||||||
Equilibrium position of the each joint.
|
Equilibrium position of the each joint.
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||||
load('mat/Foffset.mat', 'Fhm');
|
load('mat/Foffset.mat', 'Fhm');
|
||||||
micro_hexapod.dLeq = -Fhm'./args.Ki;
|
stewart.actuators.dLeq = -Fhm'./args.Ki;
|
||||||
else
|
else
|
||||||
micro_hexapod.dLeq = zeros(6,1);
|
stewart.actuators.dLeq = zeros(6,1);
|
||||||
end
|
end
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
@ -890,17 +933,17 @@ Equilibrium position of the each joint.
|
|||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
switch args.type
|
switch args.type
|
||||||
case 'none'
|
case 'none'
|
||||||
micro_hexapod.type = 0;
|
stewart.type = 0;
|
||||||
case 'rigid'
|
case 'rigid'
|
||||||
micro_hexapod.type = 1;
|
stewart.type = 1;
|
||||||
case 'flexible'
|
case 'flexible'
|
||||||
micro_hexapod.type = 2;
|
stewart.type = 2;
|
||||||
case 'modal-analysis'
|
case 'modal-analysis'
|
||||||
micro_hexapod.type = 3;
|
stewart.type = 3;
|
||||||
case 'init'
|
case 'init'
|
||||||
micro_hexapod.type = 4;
|
stewart.type = 4;
|
||||||
case 'compliance'
|
case 'compliance'
|
||||||
micro_hexapod.type = 5;
|
stewart.type = 5;
|
||||||
end
|
end
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
@ -910,6 +953,7 @@ Equilibrium position of the each joint.
|
|||||||
:END:
|
:END:
|
||||||
The =micro_hexapod= structure is saved.
|
The =micro_hexapod= structure is saved.
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
|
micro_hexapod = stewart;
|
||||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
@ -1238,6 +1282,17 @@ The =mirror= structure is saved.
|
|||||||
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||||
args.k (1,1) double {mustBeNumeric} = -1
|
args.k (1,1) double {mustBeNumeric} = -1
|
||||||
args.c (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
|
% initializeCylindricalPlatforms
|
||||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||||
@ -1267,32 +1322,55 @@ The =mirror= structure is saved.
|
|||||||
:UNNUMBERED: t
|
:UNNUMBERED: t
|
||||||
:END:
|
:END:
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
stewart = initializeStewartPlatform();
|
||||||
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 = 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
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
if args.k > 0 && args.c > 0
|
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
|
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')
|
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')
|
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
|
else
|
||||||
error('args.actuator should be piezo or lorentz');
|
error('args.actuator should be piezo or lorentz');
|
||||||
end
|
end
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab
|
#+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);
|
stewart = initializeJointDynamics(stewart, ...
|
||||||
nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
'type_F', args.type_F, ...
|
||||||
nano_hexapod = computeJacobian(nano_hexapod);
|
'type_M', args.type_M, ...
|
||||||
[Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
'Kf_M' , args.Kf_M, ...
|
||||||
nano_hexapod.Li = Li;
|
'Cf_M' , args.Cf_M, ...
|
||||||
nano_hexapod.dLi = dLi;
|
'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
|
#+end_src
|
||||||
|
|
||||||
Equilibrium position of the each joint.
|
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')
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||||
load('mat/Foffset.mat', 'Fnm');
|
load('mat/Foffset.mat', 'Fnm');
|
||||||
nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
|
stewart.actuators.dLeq = -Fnm'./stewart.Ki;
|
||||||
else
|
else
|
||||||
nano_hexapod.dLeq = args.dLeq;
|
stewart.actuators.dLeq = args.dLeq;
|
||||||
end
|
end
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
@ -1313,13 +1391,13 @@ Equilibrium position of the each joint.
|
|||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
switch args.type
|
switch args.type
|
||||||
case 'none'
|
case 'none'
|
||||||
nano_hexapod.type = 0;
|
stewart.type = 0;
|
||||||
case 'rigid'
|
case 'rigid'
|
||||||
nano_hexapod.type = 1;
|
stewart.type = 1;
|
||||||
case 'flexible'
|
case 'flexible'
|
||||||
nano_hexapod.type = 2;
|
stewart.type = 2;
|
||||||
case 'init'
|
case 'init'
|
||||||
nano_hexapod.type = 4;
|
stewart.type = 4;
|
||||||
end
|
end
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
@ -1328,6 +1406,7 @@ Equilibrium position of the each joint.
|
|||||||
:UNNUMBERED: t
|
:UNNUMBERED: t
|
||||||
:END:
|
:END:
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
|
nano_hexapod = stewart;
|
||||||
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -5,17 +5,31 @@ function [stewart] = computeJacobian(stewart)
|
|||||||
%
|
%
|
||||||
% Inputs:
|
% Inputs:
|
||||||
% - stewart - With at least the following fields:
|
% - stewart - With at least the following fields:
|
||||||
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
% - geometry.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.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||||
|
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||||
%
|
%
|
||||||
% Outputs:
|
% Outputs:
|
||||||
% - stewart - With the 3 added field:
|
% - stewart - With the 3 added field:
|
||||||
% - J [6x6] - The Jacobian Matrix
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||||
% - K [6x6] - The Stiffness Matrix
|
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||||
% - C [6x6] - The Compliance 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;
|
||||||
|
|||||||
@ -5,43 +5,74 @@ function [stewart] = computeJointsPose(stewart)
|
|||||||
%
|
%
|
||||||
% Inputs:
|
% Inputs:
|
||||||
% - stewart - A structure with the following fields
|
% - stewart - A structure with the following fields
|
||||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
% - platform_F.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_M.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}
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
||||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
||||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
||||||
%
|
%
|
||||||
% Outputs:
|
% Outputs:
|
||||||
% - stewart - A structure with the following added fields
|
% - stewart - A structure with the following added fields
|
||||||
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
% - geometry.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}
|
% - geometry.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}
|
% - geometry.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}
|
% - geometry.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
|
% - geometry.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}
|
% - geometry.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}
|
% - geometry.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}
|
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
||||||
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
% - 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]);
|
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||||||
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
|
Fa = stewart.platform_F.Fa;
|
||||||
|
|
||||||
stewart.Ab = stewart.Bb - 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')
|
||||||
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
|
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);
|
Aa = Fa - repmat(FO_A, [1, 6]);
|
||||||
stewart.MRb = zeros(3,3,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
|
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)];
|
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
||||||
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,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)];
|
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
||||||
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
|
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
||||||
end
|
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;
|
||||||
|
|||||||
83
src/describeStewartPlatform.m
Normal file
83
src/describeStewartPlatform.m
Normal file
@ -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
|
||||||
240
src/displayArchitecture.m
Normal file
240
src/displayArchitecture.m
Normal file
@ -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:
|
% Inputs:
|
||||||
% - stewart - A structure with the following fields
|
% - stewart - A structure with the following fields
|
||||||
% - J [6x6] - The Jacobian Matrix
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||||
% - args - Can have the following fields:
|
% - args - Can have the following fields:
|
||||||
% - dL [6x1] - Displacement of each strut [m]
|
% - dL [6x1] - Displacement of each strut [m]
|
||||||
%
|
%
|
||||||
@ -19,7 +19,10 @@ arguments
|
|||||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||||
end
|
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);
|
P = X(1:3);
|
||||||
|
|
||||||
|
|||||||
@ -5,7 +5,7 @@ function [stewart] = generateCubicConfiguration(stewart, args)
|
|||||||
%
|
%
|
||||||
% Inputs:
|
% Inputs:
|
||||||
% - stewart - A structure with the following fields
|
% - 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:
|
% - args - Can have the following fields:
|
||||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
% - 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]
|
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||||||
@ -14,17 +14,20 @@ function [stewart] = generateCubicConfiguration(stewart, args)
|
|||||||
%
|
%
|
||||||
% Outputs:
|
% Outputs:
|
||||||
% - stewart - updated Stewart structure with the added fields:
|
% - 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}
|
% - platform_F.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_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||||
|
|
||||||
arguments
|
arguments
|
||||||
stewart
|
stewart
|
||||||
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||||||
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||||||
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||||||
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||||||
end
|
end
|
||||||
|
|
||||||
|
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
||||||
|
H = stewart.geometry.H;
|
||||||
|
|
||||||
sx = [ 2; -1; -1];
|
sx = [ 2; -1; -1];
|
||||||
sy = [ 0; 1; -1];
|
sy = [ 0; 1; -1];
|
||||||
sz = [ 1; 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);
|
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||||||
|
|
||||||
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
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;
|
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:
|
% Outputs:
|
||||||
% - stewart - updated Stewart structure with the added fields:
|
% - 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}
|
% - platform_F.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_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||||
|
|
||||||
arguments
|
arguments
|
||||||
stewart
|
stewart
|
||||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
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.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.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);
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||||
end
|
end
|
||||||
|
|
||||||
stewart.Fa = zeros(3,6);
|
Fa = zeros(3,6);
|
||||||
stewart.Mb = zeros(3,6);
|
Mb = zeros(3,6);
|
||||||
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
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];
|
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||||
end
|
end
|
||||||
|
|
||||||
|
stewart.platform_F.Fa = Fa;
|
||||||
|
stewart.platform_M.Mb = Mb;
|
||||||
|
|||||||
43
src/initializeAmplifiedStrutDynamics.m
Normal file
43
src/initializeAmplifiedStrutDynamics.m
Normal file
@ -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:
|
% Outputs:
|
||||||
% - stewart - updated Stewart structure with the added fields:
|
% - stewart - updated Stewart structure with the added fields:
|
||||||
% - platforms [struct] - structure with the following fields:
|
% - platform_F [struct] - structure with the following fields:
|
||||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
% - type = 1
|
||||||
% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
% - M [1x1] - Fixed Platform Mass [kg]
|
||||||
% - Fph [1x1] - Fixed Platform Height [m]
|
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
% - H [1x1] - Fixed Platform Height [m]
|
||||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
% - R [1x1] - Fixed Platform Radius [m]
|
||||||
% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
% - platform_M [struct] - structure with the following fields:
|
||||||
% - Mph [1x1] - Mobile Platform Height [m]
|
% - M [1x1] - Mobile Platform Mass [kg]
|
||||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||||
|
% - H [1x1] - Mobile Platform Height [m]
|
||||||
|
% - R [1x1] - Mobile Platform Radius [m]
|
||||||
|
|
||||||
arguments
|
arguments
|
||||||
stewart
|
stewart
|
||||||
@ -34,20 +36,24 @@ arguments
|
|||||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||||
end
|
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;
|
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||||
platforms.Fph = args.Fph;
|
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||||
platforms.Fpr = args.Fpr;
|
1/2 *args.Mpm * args.Mpr^2]);
|
||||||
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]);
|
|
||||||
|
|
||||||
platforms.Mpm = args.Mpm;
|
stewart.platform_F.type = 1;
|
||||||
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.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:
|
% Outputs:
|
||||||
% - stewart - updated Stewart structure with the added fields:
|
% - stewart - updated Stewart structure with the added fields:
|
||||||
% - struts [struct] - structure with the following fields:
|
% - struts_F [struct] - structure with the following fields:
|
||||||
% - Fsm [6x1] - Mass of the Fixed part of the struts [kg]
|
% - M [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]
|
% - I [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]
|
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||||
% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||||
% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
% - struts_M [struct] - structure with the following fields:
|
||||||
% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
||||||
% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||||
% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
% - 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
|
arguments
|
||||||
stewart
|
stewart
|
||||||
@ -34,25 +35,37 @@ arguments
|
|||||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||||
end
|
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;
|
Msm = ones(6,1).*args.Msm;
|
||||||
struts.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;
|
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
||||||
struts.Fsr = ones(6,1).*args.Fsr;
|
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
||||||
struts.Msh = ones(6,1).*args.Msh;
|
|
||||||
struts.Msr = ones(6,1).*args.Msr;
|
|
||||||
|
|
||||||
struts.Fsi = zeros(3, 3, 6);
|
|
||||||
struts.Msi = zeros(3, 3, 6);
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||||
1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||||
1/2 * struts.Fsm(i) * struts.Fsr(i)^2]);
|
1/2 * Fsm(i) * 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), ...
|
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||||
1/2 * struts.Msm(i) * struts.Msr(i)^2]);
|
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||||
|
1/2 * Msm(i) * Msr(i)^2]);
|
||||||
end
|
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}
|
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||||
%
|
%
|
||||||
% Syntax: [stewart] = initializeFramesPositions(args)
|
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
||||||
%
|
%
|
||||||
% Inputs:
|
% Inputs:
|
||||||
% - args - Can have the following fields:
|
% - args - Can have the following fields:
|
||||||
@ -10,22 +10,26 @@ function [stewart] = initializeFramesPositions(args)
|
|||||||
%
|
%
|
||||||
% Outputs:
|
% Outputs:
|
||||||
% - stewart - A structure with the following fields:
|
% - stewart - A structure with the following fields:
|
||||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
||||||
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||||
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||||
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||||
|
|
||||||
arguments
|
arguments
|
||||||
|
stewart
|
||||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||||
end
|
end
|
||||||
|
|
||||||
stewart = struct();
|
H = args.H; % Total Height of the Stewart Platform [m]
|
||||||
|
|
||||||
stewart.H = args.H; % Total Height of the Stewart Platform [m]
|
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
||||||
|
|
||||||
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
|
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||||
|
|
||||||
stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
||||||
|
|
||||||
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
|
stewart.geometry.H = H;
|
||||||
|
stewart.geometry.FO_M = FO_M;
|
||||||
|
stewart.platform_M.MO_B = MO_B;
|
||||||
|
stewart.platform_F.FO_A = FO_A;
|
||||||
|
|||||||
48
src/initializeInertialSensor.m
Normal file
48
src/initializeInertialSensor.m
Normal file
@ -0,0 +1,48 @@
|
|||||||
|
function [stewart] = initializeInertialSensor(stewart, args)
|
||||||
|
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
||||||
|
%
|
||||||
|
% Syntax: [stewart] = initializeInertialSensor(args)
|
||||||
|
%
|
||||||
|
% Inputs:
|
||||||
|
% - args - Structure with the following fields:
|
||||||
|
% - type - 'geophone', 'accelerometer', 'none'
|
||||||
|
% - mass [1x1] - Weight of the inertial mass [kg]
|
||||||
|
% - freq [1x1] - Cutoff frequency [Hz]
|
||||||
|
%
|
||||||
|
% Outputs:
|
||||||
|
% - stewart - updated Stewart structure with the added fields:
|
||||||
|
% - stewart.sensors.inertial
|
||||||
|
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
||||||
|
% - K [1x1] - Stiffness [N/m]
|
||||||
|
% - C [1x1] - Damping [N/(m/s)]
|
||||||
|
% - M [1x1] - Inertial Mass [kg]
|
||||||
|
% - G [1x1] - Gain
|
||||||
|
|
||||||
|
arguments
|
||||||
|
stewart
|
||||||
|
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
||||||
|
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
||||||
|
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
||||||
|
end
|
||||||
|
|
||||||
|
sensor = struct();
|
||||||
|
|
||||||
|
switch args.type
|
||||||
|
case 'geophone'
|
||||||
|
sensor.type = 1;
|
||||||
|
|
||||||
|
sensor.M = args.mass;
|
||||||
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||||||
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||||||
|
case 'accelerometer'
|
||||||
|
sensor.type = 2;
|
||||||
|
|
||||||
|
sensor.M = args.mass;
|
||||||
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||||||
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||||||
|
sensor.G = -sensor.K/sensor.M;
|
||||||
|
case 'none'
|
||||||
|
sensor.type = 3;
|
||||||
|
end
|
||||||
|
|
||||||
|
stewart.sensors.inertial = sensor;
|
||||||
92
src/initializeJointDynamics.m
Normal file
92
src/initializeJointDynamics.m
Normal file
@ -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
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||||||
end
|
end
|
||||||
|
|
||||||
micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
stewart = initializeStewartPlatform();
|
||||||
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);
|
stewart = initializeFramesPositions(stewart, ...
|
||||||
micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
|
'H', args.H, ...
|
||||||
micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
'MO_B', args.MO_B);
|
||||||
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);
|
stewart = generateGeneralConfiguration(stewart, ...
|
||||||
[Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
'FH', args.FH, ...
|
||||||
micro_hexapod.Li = Li;
|
'FR', args.FR, ...
|
||||||
micro_hexapod.dLi = dLi;
|
'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')
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||||
load('mat/Foffset.mat', 'Fhm');
|
load('mat/Foffset.mat', 'Fhm');
|
||||||
micro_hexapod.dLeq = -Fhm'./args.Ki;
|
stewart.actuators.dLeq = -Fhm'./args.Ki;
|
||||||
else
|
else
|
||||||
micro_hexapod.dLeq = zeros(6,1);
|
stewart.actuators.dLeq = zeros(6,1);
|
||||||
end
|
end
|
||||||
|
|
||||||
switch args.type
|
switch args.type
|
||||||
case 'none'
|
case 'none'
|
||||||
micro_hexapod.type = 0;
|
stewart.type = 0;
|
||||||
case 'rigid'
|
case 'rigid'
|
||||||
micro_hexapod.type = 1;
|
stewart.type = 1;
|
||||||
case 'flexible'
|
case 'flexible'
|
||||||
micro_hexapod.type = 2;
|
stewart.type = 2;
|
||||||
case 'modal-analysis'
|
case 'modal-analysis'
|
||||||
micro_hexapod.type = 3;
|
stewart.type = 3;
|
||||||
case 'init'
|
case 'init'
|
||||||
micro_hexapod.type = 4;
|
stewart.type = 4;
|
||||||
case 'compliance'
|
case 'compliance'
|
||||||
micro_hexapod.type = 5;
|
stewart.type = 5;
|
||||||
end
|
end
|
||||||
|
|
||||||
|
micro_hexapod = stewart;
|
||||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||||
|
|||||||
@ -16,6 +16,17 @@ arguments
|
|||||||
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||||
args.k (1,1) double {mustBeNumeric} = -1
|
args.k (1,1) double {mustBeNumeric} = -1
|
||||||
args.c (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
|
% initializeCylindricalPlatforms
|
||||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||||
@ -39,45 +50,65 @@ arguments
|
|||||||
args.Foffset logical {mustBeNumericOrLogical} = false
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||||||
end
|
end
|
||||||
|
|
||||||
nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
stewart = initializeStewartPlatform();
|
||||||
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 = 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
|
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
|
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')
|
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')
|
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
|
else
|
||||||
error('args.actuator should be piezo or lorentz');
|
error('args.actuator should be piezo or lorentz');
|
||||||
end
|
end
|
||||||
|
|
||||||
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
stewart = initializeJointDynamics(stewart, ...
|
||||||
nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
'type_F', args.type_F, ...
|
||||||
nano_hexapod = computeJacobian(nano_hexapod);
|
'type_M', args.type_M, ...
|
||||||
[Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
|
'Kf_M' , args.Kf_M, ...
|
||||||
nano_hexapod.Li = Li;
|
'Cf_M' , args.Cf_M, ...
|
||||||
nano_hexapod.dLi = dLi;
|
'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')
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||||||
load('mat/Foffset.mat', 'Fnm');
|
load('mat/Foffset.mat', 'Fnm');
|
||||||
nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
|
stewart.actuators.dLeq = -Fnm'./stewart.Ki;
|
||||||
else
|
else
|
||||||
nano_hexapod.dLeq = args.dLeq;
|
stewart.actuators.dLeq = args.dLeq;
|
||||||
end
|
end
|
||||||
|
|
||||||
switch args.type
|
switch args.type
|
||||||
case 'none'
|
case 'none'
|
||||||
nano_hexapod.type = 0;
|
stewart.type = 0;
|
||||||
case 'rigid'
|
case 'rigid'
|
||||||
nano_hexapod.type = 1;
|
stewart.type = 1;
|
||||||
case 'flexible'
|
case 'flexible'
|
||||||
nano_hexapod.type = 2;
|
stewart.type = 2;
|
||||||
case 'init'
|
case 'init'
|
||||||
nano_hexapod.type = 4;
|
stewart.type = 4;
|
||||||
end
|
end
|
||||||
|
|
||||||
|
nano_hexapod = stewart;
|
||||||
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||||||
|
|||||||
31
src/initializeStewartPlatform.m
Normal file
31
src/initializeStewartPlatform.m
Normal file
@ -0,0 +1,31 @@
|
|||||||
|
function [stewart] = initializeStewartPlatform()
|
||||||
|
% initializeStewartPlatform - Initialize the stewart structure
|
||||||
|
%
|
||||||
|
% Syntax: [stewart] = initializeStewartPlatform(args)
|
||||||
|
%
|
||||||
|
% Outputs:
|
||||||
|
% - stewart - A structure with the following sub-structures:
|
||||||
|
% - platform_F -
|
||||||
|
% - platform_M -
|
||||||
|
% - joints_F -
|
||||||
|
% - joints_M -
|
||||||
|
% - struts_F -
|
||||||
|
% - struts_M -
|
||||||
|
% - actuators -
|
||||||
|
% - geometry -
|
||||||
|
% - properties -
|
||||||
|
|
||||||
|
stewart = struct();
|
||||||
|
stewart.platform_F = struct();
|
||||||
|
stewart.platform_M = struct();
|
||||||
|
stewart.joints_F = struct();
|
||||||
|
stewart.joints_M = struct();
|
||||||
|
stewart.struts_F = struct();
|
||||||
|
stewart.struts_M = struct();
|
||||||
|
stewart.actuators = struct();
|
||||||
|
stewart.sensors = struct();
|
||||||
|
stewart.sensors.inertial = struct();
|
||||||
|
stewart.sensors.force = struct();
|
||||||
|
stewart.sensors.relative = struct();
|
||||||
|
stewart.geometry = struct();
|
||||||
|
stewart.kinematics = struct();
|
||||||
27
src/initializeStewartPose.m
Normal file
27
src/initializeStewartPose.m
Normal file
@ -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:
|
% Inputs:
|
||||||
% - args - Structure with the following fields:
|
% - args - Structure with the following fields:
|
||||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
% - K [6x1] - Stiffness of each strut [N/m]
|
||||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
% - C [6x1] - Damping of each strut [N/(m/s)]
|
||||||
%
|
%
|
||||||
% Outputs:
|
% Outputs:
|
||||||
% - stewart - updated Stewart structure with the added fields:
|
% - stewart - updated Stewart structure with the added fields:
|
||||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
% - actuators.type = 1
|
||||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
||||||
|
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
||||||
|
|
||||||
arguments
|
arguments
|
||||||
stewart
|
stewart
|
||||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
||||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
||||||
end
|
end
|
||||||
|
|
||||||
stewart.Ki = args.Ki;
|
stewart.actuators.type = 1;
|
||||||
stewart.Ci = args.Ci;
|
|
||||||
|
stewart.actuators.K = args.K;
|
||||||
|
stewart.actuators.C = args.C;
|
||||||
|
|||||||
@ -5,8 +5,9 @@ function [Li, dLi] = inverseKinematics(stewart, args)
|
|||||||
%
|
%
|
||||||
% Inputs:
|
% Inputs:
|
||||||
% - stewart - A structure with the following fields
|
% - stewart - A structure with the following fields
|
||||||
% - Aa [3x6] - The positions ai expressed in {A}
|
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||||
% - Bb [3x6] - The positions bi expressed in {B}
|
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||||
|
% - geometry.l [6x1] - Length of each strut
|
||||||
% - args - Can have the following fields:
|
% - args - Can have the following fields:
|
||||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
% - 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}
|
||||||
@ -21,6 +22,15 @@ arguments
|
|||||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||||
end
|
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;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user