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

Update Stewart Platforms initialization

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-05-04 12:03:08 +02:00
parent 7e615e74ae
commit 19c6456cd5
36 changed files with 2664 additions and 506 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
mat/conf_log.mat
View File

Binary file not shown.

BIN
mat/conf_simscape.mat
View File

Binary file not shown.

BIN
mat/controller.mat
View File

Binary file not shown.

BIN
mat/nass_disturbances.mat
View File

Binary file not shown.

BIN
mat/nass_references.mat
View File

Binary file not shown.

BIN
mat/stages.mat
View File

Binary file not shown.

BIN
matlab/Fixed_Based.slx Normal file
View File

Binary file not shown.

BIN
matlab/Mobile_Platform.slx Normal file
View File

Binary file not shown.

BIN
matlab/micro_hexapod_strut.slx Normal file
View File

Binary file not shown.

BIN
matlab/nano_hexapod_leg_rigid.slx
View File

Binary file not shown.

BIN
matlab/nano_hexapod_strut.slx Normal file
View File

Binary file not shown.

BIN
matlab/nass_model.slx
View File

Binary file not shown.

BIN
matlab/stewart_strut.slx.r2019b Normal file
View File

Binary file not shown.

BIN
matlab/z_axis_accelerometer.slx Normal file
View File

Binary file not shown.

BIN
matlab/z_axis_geophone.slx Normal file
View File

Binary file not shown.

153
org/simscape_subsystems.org
View File

@ -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

1971
org/stewart_platform.org
View File

File diff suppressed because it is too large Load Diff

30
src/computeJacobian.m
View File

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

85
src/computeJointsPose.m
View File

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

83
src/describeStewartPlatform.m Normal file
View 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
View 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

7
src/forwardKinematicsApprox.m
View File

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

20
src/generateCubicConfiguration.m
View File

@ -5,7 +5,7 @@ function [stewart] = generateCubicConfiguration(stewart, args)
%
% Inputs:
% - stewart - A structure with the following fields
% - H [1x1] - Total height of the platform [m]
% - geometry.H [1x1] - Total height of the platform [m]
% - args - Can have the following fields:
% - Hc [1x1] - Height of the "useful" part of the cube [m]
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
@ -14,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;

19
src/generateGeneralConfiguration.m
View File

@ -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;

43
src/initializeAmplifiedStrutDynamics.m Normal file
View 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;

52
src/initializeCylindricalPlatforms.m
View File

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

63
src/initializeCylindricalStruts.m
View File

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

26
src/initializeFramesPositions.m
View File

@ -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;

48
src/initializeInertialSensor.m Normal file
View File

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

92
src/initializeJointDynamics.m Normal file
View 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;

74
src/initializeMicroHexapod.m
View File

@ -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');

69
src/initializeNanoHexapod.m
View File

@ -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');

31
src/initializeStewartPlatform.m Normal file
View File

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

27
src/initializeStewartPose.m Normal file
View 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;

19
src/initializeStrutDynamics.m
View File

@ -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;

18
src/inverseKinematics.m
View File

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