Add matlab functions and Simplify Simscape model
This commit is contained in:
parent
45bef195d6
commit
67071e033a
35
matlab/src/computeJacobian.m
Normal file
35
matlab/src/computeJacobian.m
Normal file
@ -0,0 +1,35 @@
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
|
||||
J = [As' , cross(Ab, As)'];
|
||||
|
||||
K = J'*diag(Ki)*J;
|
||||
|
||||
C = inv(K);
|
||||
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
78
matlab/src/computeJointsPose.m
Normal file
78
matlab/src/computeJointsPose.m
Normal file
@ -0,0 +1,78 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
||||
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
||||
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
||||
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
|
||||
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||||
Fa = stewart.platform_F.Fa;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||||
Mb = stewart.platform_M.Mb;
|
||||
|
||||
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||||
FO_A = stewart.platform_F.FO_A;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||||
MO_B = stewart.platform_M.MO_B;
|
||||
|
||||
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
||||
FO_M = stewart.geometry.FO_M;
|
||||
|
||||
Aa = Fa - repmat(FO_A, [1, 6]);
|
||||
Bb = Mb - repmat(MO_B, [1, 6]);
|
||||
|
||||
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
||||
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
||||
|
||||
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
||||
|
||||
l = vecnorm(Ab - Aa)';
|
||||
|
||||
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
||||
|
||||
FRa = zeros(3,3,6);
|
||||
MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
||||
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
||||
|
||||
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
||||
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
||||
end
|
||||
|
||||
stewart.geometry.Aa = Aa;
|
||||
stewart.geometry.Ab = Ab;
|
||||
stewart.geometry.Ba = Ba;
|
||||
stewart.geometry.Bb = Bb;
|
||||
stewart.geometry.As = As;
|
||||
stewart.geometry.Bs = Bs;
|
||||
stewart.geometry.l = l;
|
||||
|
||||
stewart.struts_F.l = l/2;
|
||||
stewart.struts_M.l = l/2;
|
||||
|
||||
stewart.platform_F.FRa = FRa;
|
||||
stewart.platform_M.MRb = MRb;
|
||||
39
matlab/src/generateGeneralConfiguration.m
Normal file
39
matlab/src/generateGeneralConfiguration.m
Normal file
@ -0,0 +1,39 @@
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
Fa = zeros(3,6);
|
||||
Mb = zeros(3,6);
|
||||
|
||||
for i = 1:6
|
||||
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
|
||||
stewart.platform_F.Fa = Fa;
|
||||
stewart.platform_M.Mb = Mb;
|
||||
59
matlab/src/initializeCylindricalPlatforms.m
Normal file
59
matlab/src/initializeCylindricalPlatforms.m
Normal file
@ -0,0 +1,59 @@
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F [struct] - structure with the following fields:
|
||||
% - type = 1
|
||||
% - M [1x1] - Fixed Platform Mass [kg]
|
||||
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Fixed Platform Height [m]
|
||||
% - R [1x1] - Fixed Platform Radius [m]
|
||||
% - platform_M [struct] - structure with the following fields:
|
||||
% - M [1x1] - Mobile Platform Mass [kg]
|
||||
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Mobile Platform Height [m]
|
||||
% - R [1x1] - Mobile Platform Radius [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
end
|
||||
|
||||
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/2 *args.Fpm * args.Fpr^2]);
|
||||
|
||||
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/2 *args.Mpm * args.Mpr^2]);
|
||||
|
||||
stewart.platform_F.type = 1;
|
||||
|
||||
stewart.platform_F.I = I_F;
|
||||
stewart.platform_F.M = args.Fpm;
|
||||
stewart.platform_F.R = args.Fpr;
|
||||
stewart.platform_F.H = args.Fph;
|
||||
|
||||
stewart.platform_M.type = 1;
|
||||
|
||||
stewart.platform_M.I = I_M;
|
||||
stewart.platform_M.M = args.Mpm;
|
||||
stewart.platform_M.R = args.Mpr;
|
||||
stewart.platform_M.H = args.Mph;
|
||||
71
matlab/src/initializeCylindricalStruts.m
Normal file
71
matlab/src/initializeCylindricalStruts.m
Normal file
@ -0,0 +1,71 @@
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts_F [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - struts_M [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
end
|
||||
|
||||
Fsm = ones(6,1).*args.Fsm;
|
||||
Fsh = ones(6,1).*args.Fsh;
|
||||
Fsr = ones(6,1).*args.Fsr;
|
||||
|
||||
Msm = ones(6,1).*args.Msm;
|
||||
Msh = ones(6,1).*args.Msh;
|
||||
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
|
||||
|
||||
for i = 1:6
|
||||
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_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;
|
||||
@ -23,7 +23,7 @@
|
||||
args.Frz_z logical {mustBeNumericOrLogical} = true
|
||||
end
|
||||
|
||||
load('./mat/dist_psd.mat', 'dist_f');
|
||||
load('dist_psd.mat', 'dist_f');
|
||||
|
||||
dist_f.f = dist_f.f(2:end);
|
||||
dist_f.psd_gm = dist_f.psd_gm(2:end);
|
||||
@ -132,7 +132,6 @@
|
||||
Fty_z = Fty_z - Fty_z(1);
|
||||
Frz_z = Frz_z - Frz_z(1);
|
||||
|
||||
micro_hexapod = stewart;
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_disturbances.mat', 'file')
|
||||
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
|
||||
|
||||
35
matlab/src/initializeFramesPositions.m
Normal file
35
matlab/src/initializeFramesPositions.m
Normal file
@ -0,0 +1,35 @@
|
||||
function [stewart] = initializeFramesPositions(stewart, args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||
end
|
||||
|
||||
H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
||||
|
||||
stewart.geometry.H = H;
|
||||
stewart.geometry.FO_M = FO_M;
|
||||
stewart.platform_M.MO_B = MO_B;
|
||||
stewart.platform_F.FO_A = FO_A;
|
||||
48
matlab/src/initializeInertialSensor.m
Normal file
48
matlab/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;
|
||||
131
matlab/src/initializeJointDynamics.m
Normal file
131
matlab/src/initializeJointDynamics.m
Normal file
@ -0,0 +1,131 @@
|
||||
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_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
|
||||
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
|
||||
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
||||
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
||||
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
||||
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
||||
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
||||
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
||||
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
||||
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
||||
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.K_M double {mustBeNumeric} = zeros(6,6)
|
||||
args.M_M double {mustBeNumeric} = zeros(6,6)
|
||||
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
||||
args.xi_M double {mustBeNumeric} = 0.1
|
||||
args.step_file_M char {} = ''
|
||||
args.K_F double {mustBeNumeric} = zeros(6,6)
|
||||
args.M_F double {mustBeNumeric} = zeros(6,6)
|
||||
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
||||
args.xi_F double {mustBeNumeric} = 0.1
|
||||
args.step_file_F char {} = ''
|
||||
end
|
||||
|
||||
switch args.type_F
|
||||
case '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;
|
||||
case 'flexible'
|
||||
stewart.joints_F.type = 5;
|
||||
case 'universal_3dof'
|
||||
stewart.joints_F.type = 6;
|
||||
case 'spherical_3dof'
|
||||
stewart.joints_F.type = 7;
|
||||
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;
|
||||
case 'flexible'
|
||||
stewart.joints_M.type = 5;
|
||||
case 'universal_3dof'
|
||||
stewart.joints_M.type = 6;
|
||||
case 'spherical_3dof'
|
||||
stewart.joints_M.type = 7;
|
||||
end
|
||||
|
||||
stewart.joints_M.Ka = args.Ka_M;
|
||||
stewart.joints_M.Kr = args.Kr_M;
|
||||
|
||||
stewart.joints_F.Ka = args.Ka_F;
|
||||
stewart.joints_F.Kr = args.Kr_F;
|
||||
|
||||
stewart.joints_M.Ca = args.Ca_M;
|
||||
stewart.joints_M.Cr = args.Cr_M;
|
||||
|
||||
stewart.joints_F.Ca = args.Ca_F;
|
||||
stewart.joints_F.Cr = args.Cr_F;
|
||||
|
||||
stewart.joints_M.Kf = args.Kf_M;
|
||||
stewart.joints_M.Kt = args.Kt_M;
|
||||
|
||||
stewart.joints_F.Kf = args.Kf_F;
|
||||
stewart.joints_F.Kt = args.Kt_F;
|
||||
|
||||
stewart.joints_M.Cf = args.Cf_M;
|
||||
stewart.joints_M.Ct = args.Ct_M;
|
||||
|
||||
stewart.joints_F.Cf = args.Cf_F;
|
||||
stewart.joints_F.Ct = args.Ct_F;
|
||||
|
||||
stewart.joints_F.M = args.M_F;
|
||||
stewart.joints_F.K = args.K_F;
|
||||
stewart.joints_F.n_xyz = args.n_xyz_F;
|
||||
stewart.joints_F.xi = args.xi_F;
|
||||
stewart.joints_F.xi = args.xi_F;
|
||||
stewart.joints_F.step_file = args.step_file_F;
|
||||
|
||||
stewart.joints_M.M = args.M_M;
|
||||
stewart.joints_M.K = args.K_M;
|
||||
stewart.joints_M.n_xyz = args.n_xyz_M;
|
||||
stewart.joints_M.xi = args.xi_M;
|
||||
stewart.joints_M.step_file = args.step_file_M;
|
||||
@ -184,58 +184,16 @@
|
||||
Dh = struct('time', t, 'signals', struct('values', Dh));
|
||||
Dhl = struct('time', t, 'signals', struct('values', Dhl));
|
||||
|
||||
%% Axis Compensation - Rm
|
||||
t = [0, Ts];
|
||||
|
||||
Rm = [args.Rm_pos, args.Rm_pos];
|
||||
Rm = struct('time', t, 'signals', struct('values', Rm));
|
||||
|
||||
%% Nano-Hexapod
|
||||
t = [0, Ts];
|
||||
Dn = zeros(length(t), 6);
|
||||
|
||||
switch args.Dn_type
|
||||
case 'constant'
|
||||
Dn = [args.Dn_pos, args.Dn_pos];
|
||||
|
||||
load('nass_stages.mat', 'nano_hexapod');
|
||||
|
||||
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
|
||||
|
||||
tx = args.Dn_pos(4);
|
||||
ty = args.Dn_pos(5);
|
||||
tz = args.Dn_pos(6);
|
||||
|
||||
ARB = [cos(tz) -sin(tz) 0;
|
||||
sin(tz) cos(tz) 0;
|
||||
0 0 1]*...
|
||||
[ cos(ty) 0 sin(ty);
|
||||
0 1 0;
|
||||
-sin(ty) 0 cos(ty)]*...
|
||||
[1 0 0;
|
||||
0 cos(tx) -sin(tx);
|
||||
0 sin(tx) cos(tx)];
|
||||
|
||||
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
|
||||
Dnl = [Dnl, Dnl];
|
||||
otherwise
|
||||
warning('Dn_type is not set correctly');
|
||||
end
|
||||
|
||||
Dn = struct('time', t, 'signals', struct('values', Dn));
|
||||
Dnl = struct('time', t, 'signals', struct('values', Dnl));
|
||||
|
||||
micro_hexapod = stewart;
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_references.mat', 'file')
|
||||
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
|
||||
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
||||
else
|
||||
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
|
||||
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_references.mat', 'file')
|
||||
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
|
||||
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
|
||||
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
||||
end
|
||||
end
|
||||
|
||||
31
matlab/src/initializeStewartPlatform.m
Normal file
31
matlab/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
matlab/src/initializeStewartPose.m
Normal file
27
matlab/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;
|
||||
49
matlab/src/initializeStrutDynamics.m
Normal file
49
matlab/src/initializeStrutDynamics.m
Normal file
@ -0,0 +1,49 @@
|
||||
function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - K [6x1] - Stiffness of each strut [N/m]
|
||||
% - C [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - actuators.type = 1
|
||||
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
||||
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
|
||||
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
||||
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
||||
args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
|
||||
args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
|
||||
args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
|
||||
args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
|
||||
args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
|
||||
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
||||
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
||||
end
|
||||
|
||||
if strcmp(args.type, 'classical')
|
||||
stewart.actuators.type = 1;
|
||||
elseif strcmp(args.type, 'amplified')
|
||||
stewart.actuators.type = 2;
|
||||
end
|
||||
|
||||
stewart.actuators.K = args.K;
|
||||
stewart.actuators.C = args.C;
|
||||
|
||||
stewart.actuators.k1 = args.k1;
|
||||
stewart.actuators.c1 = args.c1;
|
||||
|
||||
stewart.actuators.ka = args.ka;
|
||||
stewart.actuators.ke = args.ke;
|
||||
|
||||
stewart.actuators.F_gain = args.F_gain;
|
||||
|
||||
stewart.actuators.ma = args.ma;
|
||||
stewart.actuators.me = args.me;
|
||||
36
matlab/src/inverseKinematics.m
Normal file
36
matlab/src/inverseKinematics.m
Normal file
@ -0,0 +1,36 @@
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
|
||||
dLi = Li-l;
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user