Update Stewart Platforms initialization
This commit is contained in:
@@ -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
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user