diff --git a/matlab/nano_hexapod_model.slx b/matlab/nano_hexapod_model.slx new file mode 100644 index 0000000..4e8e0fd Binary files /dev/null and b/matlab/nano_hexapod_model.slx differ diff --git a/matlab/src/computeJacobian.m b/matlab/src/computeJacobian.m new file mode 100644 index 0000000..6adaa7f --- /dev/null +++ b/matlab/src/computeJacobian.m @@ -0,0 +1,37 @@ +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: +% - geometry.J [6x6] - The Jacobian Matrix +% - geometry.K [6x6] - The Stiffness Matrix +% - geometry.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.geometry.J = J; + stewart.geometry.K = K; + stewart.geometry.C = C; + +end diff --git a/matlab/src/computeJointsPose.m b/matlab/src/computeJointsPose.m new file mode 100644 index 0000000..904f37a --- /dev/null +++ b/matlab/src/computeJointsPose.m @@ -0,0 +1,80 @@ +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; + +end diff --git a/matlab/src/describeStewartPlatform.m b/matlab/src/describeStewartPlatform.m new file mode 100644 index 0000000..14dd29e --- /dev/null +++ b/matlab/src/describeStewartPlatform.m @@ -0,0 +1,77 @@ +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 modelled as 1DoF.\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 modelled as 2DoF (APA).\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)) +elseif stewart.actuators.type == 3 + fprintf('- The actuators are modelled with a flexible element (FEM).\n') +end +fprintf('\n') + +fprintf('JOINTS:\n') + +switch stewart.joints_F.type + case 1 + fprintf('- The joints on the fixed based are universal joints (2DoF)\n') + case 2 + fprintf('- The joints on the fixed based are spherical joints (3DoF)\n') +end + +switch stewart.joints_M.type + case 1 + fprintf('- The joints on the mobile based are universal joints (2DoF)\n') + case 2 + fprintf('- The joints on the mobile based are spherical joints (3DoF)\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 diff --git a/matlab/src/displayArchitecture.m b/matlab/src/displayArchitecture.m new file mode 100644 index 0000000..f8d9012 --- /dev/null +++ b/matlab/src/displayArchitecture.m @@ -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 diff --git a/matlab/src/generateGeneralConfiguration.m b/matlab/src/generateGeneralConfiguration.m new file mode 100644 index 0000000..69b9786 --- /dev/null +++ b/matlab/src/generateGeneralConfiguration.m @@ -0,0 +1,41 @@ +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; + +end diff --git a/matlab/src/initializeController.m b/matlab/src/initializeController.m new file mode 100644 index 0000000..1b55d91 --- /dev/null +++ b/matlab/src/initializeController.m @@ -0,0 +1,45 @@ +function [] = initializeController(args) + + arguments + args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop' + end + + controller = struct(); + + switch args.type + case 'open-loop' + controller.type = 1; + controller.name = 'Open-Loop'; + case 'dvf' + controller.type = 2; + controller.name = 'Decentralized Direct Velocity Feedback'; + case 'iff' + controller.type = 3; + controller.name = 'Decentralized Integral Force Feedback'; + case 'hac-dvf' + controller.type = 4; + controller.name = 'HAC-DVF'; + case 'ref-track-L' + controller.type = 5; + controller.name = 'Reference Tracking in the frame of the legs'; + case 'ref-track-iff-L' + controller.type = 6; + controller.name = 'Reference Tracking in the frame of the legs + IFF'; + case 'cascade-hac-lac' + controller.type = 7; + controller.name = 'Cascade Control + HAC-LAC'; + case 'hac-iff' + controller.type = 8; + controller.name = 'HAC-IFF'; + case 'stabilizing' + controller.type = 9; + controller.name = 'Stabilizing Controller'; + end + + if exist('./mat', 'dir') + save('mat/nano_hexapod_model_controller.mat', 'controller'); + elseif exist('./matlab', 'dir') + save('matlab/mat/nano_hexapod_model_controller.mat', 'controller'); + end + +end diff --git a/matlab/src/initializeCylindricalPlatforms.m b/matlab/src/initializeCylindricalPlatforms.m new file mode 100644 index 0000000..1c048f3 --- /dev/null +++ b/matlab/src/initializeCylindricalPlatforms.m @@ -0,0 +1,61 @@ +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; + +end diff --git a/matlab/src/initializeCylindricalStruts.m b/matlab/src/initializeCylindricalStruts.m new file mode 100644 index 0000000..16d4fd2 --- /dev/null +++ b/matlab/src/initializeCylindricalStruts.m @@ -0,0 +1,50 @@ +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 + + stewart.struts_M.type = 1; + + stewart.struts_M.M = args.Msm; + stewart.struts_M.R = args.Msr; + stewart.struts_M.H = args.Msh; + + stewart.struts_F.type = 1; + + stewart.struts_F.M = args.Fsm; + stewart.struts_F.R = args.Fsr; + stewart.struts_F.H = args.Fsh; + +end diff --git a/matlab/src/initializeFramesPositions.m b/matlab/src/initializeFramesPositions.m new file mode 100644 index 0000000..f1e065b --- /dev/null +++ b/matlab/src/initializeFramesPositions.m @@ -0,0 +1,37 @@ +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; + +end diff --git a/matlab/src/initializeJointDynamics.m b/matlab/src/initializeJointDynamics.m new file mode 100644 index 0000000..4c4d1b5 --- /dev/null +++ b/matlab/src/initializeJointDynamics.m @@ -0,0 +1,129 @@ +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,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' + args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' + args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + 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 '2dof' + stewart.joints_F.type = 1; + case '3dof' + stewart.joints_F.type = 2; + case '4dof' + stewart.joints_F.type = 3; + case '6dof' + stewart.joints_F.type = 4; + case 'flexible' + stewart.joints_F.type = 5; + otherwise + error("joints_F are not correctly defined") + end + + switch args.type_M + case '2dof' + stewart.joints_M.type = 1; + case '3dof' + stewart.joints_M.type = 2; + case '4dof' + stewart.joints_M.type = 3; + case '6dof' + stewart.joints_M.type = 4; + case 'flexible' + stewart.joints_M.type = 5; + otherwise + error("joints_M are not correctly defined") + 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; + +end diff --git a/matlab/src/initializeLoggingConfiguration.m b/matlab/src/initializeLoggingConfiguration.m new file mode 100644 index 0000000..21b3a34 --- /dev/null +++ b/matlab/src/initializeLoggingConfiguration.m @@ -0,0 +1,33 @@ + function [] = initializeLoggingConfiguration(args) + + arguments + args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none' + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 + end + + conf_log = struct(); + + switch args.log + case 'none' + conf_log.type = 0; + case 'all' + conf_log.type = 1; + case 'forces' + conf_log.type = 2; + end + + conf_log.Ts = args.Ts; + +if exist('./mat', 'dir') + if exist('./mat/nano_hexapod_model_conf_log.mat', 'file') + save('mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append'); + else + save('mat/nano_hexapod_model_conf_log.mat', 'conf_log'); + end +elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nano_hexapod_model_conf_log.mat', 'file') + save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append'); + else + save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log'); + end +end diff --git a/matlab/src/initializeSample.m b/matlab/src/initializeSample.m new file mode 100644 index 0000000..0ed1946 --- /dev/null +++ b/matlab/src/initializeSample.m @@ -0,0 +1,38 @@ +function [sample] = initializeSample(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none' + args.H (1,1) double {mustBeNumeric, mustBePositive} = 200e-3 % Height [m] + args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m] + args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg] + end + + sample = struct(); + + switch args.type + case 'none' + sample.type = 0; + sample.m = 0; + case 'cylindrical' + sample.type = 1; + + sample.H = args.H; + sample.R = args.R; + sample.m = args.m; + end + + if exist('./mat', 'dir') + if exist('./mat/nano_hexapod.mat', 'file') + save('mat/nano_hexapod.mat', 'sample', '-append'); + else + save('mat/nano_hexapod.mat', 'sample'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nano_hexapod.mat', 'file') + save('matlab/mat/nano_hexapod.mat', 'sample', '-append'); + else + save('matlab/mat/nano_hexapod.mat', 'sample'); + end + end + +end diff --git a/matlab/src/initializeSimplifiedNanoHexapod.m b/matlab/src/initializeSimplifiedNanoHexapod.m new file mode 100644 index 0000000..5b9ed84 --- /dev/null +++ b/matlab/src/initializeSimplifiedNanoHexapod.m @@ -0,0 +1,141 @@ +function [nano_hexapod] = initializeSimplifiedNanoHexapod(args) + + arguments + %% initializeFramesPositions + args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m] + args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m] + %% generateGeneralConfiguration + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of fixed joints [m] + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m] + args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad] + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of mobile joints [m] + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m] + args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad] + %% Actuators + args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof' + args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 380000 + args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605 + args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302 + args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 5 + args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100 + args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50 + %% initializeCylindricalPlatforms + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg] + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m] + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m] + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg] + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m] + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m] + %% initializeCylindricalStruts + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg] + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m] + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m] + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg] + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m] + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m] + %% Bottom and Top Flexible Joints + args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' + args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' + args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + %% inverseKinematics + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + end + + 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, ... + 'type', args.actuator_type, ... + 'k', args.actuator_k, ... + 'ke', args.actuator_ke, ... + 'ka', args.actuator_ka, ... + 'c', args.actuator_c, ... + 'ce', args.actuator_ce, ... + 'ca', args.actuator_ca); + + stewart = initializeJointDynamics(stewart, ... + 'type_F', args.flex_type_F, ... + 'type_M', args.flex_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, ... + 'Ka_F', args.Ka_F, ... + 'Ca_F', args.Ca_F, ... + 'Kr_F', args.Kr_F, ... + 'Cr_F', args.Cr_F, ... + 'Ka_M', args.Ka_M, ... + 'Ca_M', args.Ca_M, ... + 'Kr_M', args.Kr_M, ... + 'Cr_M', args.Cr_M); + + 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); + + nano_hexapod = stewart; + if exist('./mat', 'dir') + if exist('./mat/nano_hexapod.mat', 'file') + save('mat/nano_hexapod.mat', 'nano_hexapod', '-append'); + else + save('mat/nano_hexapod.mat', 'nano_hexapod'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nano_hexapod.mat', 'file') + save('matlab/mat/nano_hexapod.mat', 'nano_hexapod', '-append'); + else + save('matlab/mat/nano_hexapod.mat', 'nano_hexapod'); + end + end +end diff --git a/matlab/src/initializeStewartPlatform.m b/matlab/src/initializeStewartPlatform.m new file mode 100644 index 0000000..42b4188 --- /dev/null +++ b/matlab/src/initializeStewartPlatform.m @@ -0,0 +1,33 @@ +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(); + +end diff --git a/matlab/src/initializeStewartPose.m b/matlab/src/initializeStewartPose.m new file mode 100644 index 0000000..272262e --- /dev/null +++ b/matlab/src/initializeStewartPose.m @@ -0,0 +1,29 @@ +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; + +end diff --git a/matlab/src/initializeStrutDynamics.m b/matlab/src/initializeStrutDynamics.m new file mode 100644 index 0000000..9808789 --- /dev/null +++ b/matlab/src/initializeStrutDynamics.m @@ -0,0 +1,54 @@ +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,{'1dof', '2dof', 'flexible'})} = '1dof' + args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6 + args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6 + args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6 + args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1 + args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6 + args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10 + + args.F_gain (1,1) double {mustBeNumeric} = 1 + args.me (1,1) double {mustBeNumeric} = 0.01 + args.ma (1,1) double {mustBeNumeric} = 0.01 + end + + if strcmp(args.type, '1dof') + stewart.actuators.type = 1; + elseif strcmp(args.type, '2dof') + stewart.actuators.type = 2; + elseif strcmp(args.type, 'flexible') + stewart.actuators.type = 3; + end + + stewart.actuators.k = args.k; + stewart.actuators.c = args.c; + + stewart.actuators.ka = args.ka; + stewart.actuators.ca = args.ca; + + stewart.actuators.ke = args.ke; + stewart.actuators.ce = args.ce; + + stewart.actuators.F_gain = args.F_gain; + + stewart.actuators.ma = args.ma; + stewart.actuators.me = args.me; + +end diff --git a/matlab/src/inverseKinematics.m b/matlab/src/inverseKinematics.m new file mode 100644 index 0000000..3096ab1 --- /dev/null +++ b/matlab/src/inverseKinematics.m @@ -0,0 +1,38 @@ +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; + +end diff --git a/matlab/subsystems/Fixed_Based.slx b/matlab/subsystems/Fixed_Based.slx new file mode 100644 index 0000000..9b7075a Binary files /dev/null and b/matlab/subsystems/Fixed_Based.slx differ diff --git a/matlab/subsystems/Mobile_Platform.slx b/matlab/subsystems/Mobile_Platform.slx new file mode 100644 index 0000000..78ef458 Binary files /dev/null and b/matlab/subsystems/Mobile_Platform.slx differ diff --git a/matlab/subsystems/Stewart_Platform.slx b/matlab/subsystems/Stewart_Platform.slx new file mode 100644 index 0000000..b8faba3 Binary files /dev/null and b/matlab/subsystems/Stewart_Platform.slx differ diff --git a/matlab/subsystems/metrology_6dof.slx b/matlab/subsystems/metrology_6dof.slx new file mode 100644 index 0000000..6227ebc Binary files /dev/null and b/matlab/subsystems/metrology_6dof.slx differ diff --git a/matlab/subsystems/stewart_strut.slx b/matlab/subsystems/stewart_strut.slx new file mode 100644 index 0000000..063a625 Binary files /dev/null and b/matlab/subsystems/stewart_strut.slx differ diff --git a/simscape-nano-hexapod.org b/simscape-nano-hexapod.org index 88fd7af..f40fcde 100644 --- a/simscape-nano-hexapod.org +++ b/simscape-nano-hexapod.org @@ -851,20 +851,1443 @@ Compute: ** Initialize Path #+NAME: m-init-path #+BEGIN_SRC matlab -%% Path for functions, data and scripts -addpath('./matlab/mat/'); % Path for data addpath('./matlab/'); % Path for scripts + +%% Path for functions, data and scripts +addpath('./matlab/mat/'); % Path for Computed FRF +addpath('./matlab/src/'); % Path for functions +addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './matlab/mat/' #+END_SRC #+NAME: m-init-path-tangle #+BEGIN_SRC matlab %% Path for functions, data and scripts -addpath('./mat/'); % Path for data +addpath('./mat/'); % Path for Data +addpath('./src/'); % Path for functions +addpath('./subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './mat/'; #+END_SRC +** Initialize Simscape Model +#+NAME: m-init-simscape +#+begin_src matlab +% Simulink Model name +mdl = 'nano_hexapod_model'; +#+end_src + ** Initialize other elements #+NAME: m-init-other #+BEGIN_SRC matlab %% Colors for the figures colors = colororder; + +%% Frequency Vector +freqs = logspace(0, 3, 1000); #+END_SRC + +* Matlab Functions :noexport: +** =initializeLoggingConfiguration=: Logging Configuration +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +*** Function description +#+begin_src matlab + function [] = initializeLoggingConfiguration(args) +#+end_src + +*** Optional Parameters +#+begin_src matlab + arguments + args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none' + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 + end +#+end_src + +*** Structure initialization +#+begin_src matlab + conf_log = struct(); +#+end_src + +*** Add Type +#+begin_src matlab + switch args.log + case 'none' + conf_log.type = 0; + case 'all' + conf_log.type = 1; + case 'forces' + conf_log.type = 2; + end +#+end_src + +*** Sampling Time +#+begin_src matlab + conf_log.Ts = args.Ts; +#+end_src + +*** Save the Structure +#+begin_src matlab +if exist('./mat', 'dir') + if exist('./mat/nano_hexapod_model_conf_log.mat', 'file') + save('mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append'); + else + save('mat/nano_hexapod_model_conf_log.mat', 'conf_log'); + end +elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nano_hexapod_model_conf_log.mat', 'file') + save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append'); + else + save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log'); + end +end +#+end_src + +** =initializeController=: Initialize Controller +#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no +function [] = initializeController(args) + + arguments + args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop' + end + + controller = struct(); + + switch args.type + case 'open-loop' + controller.type = 1; + controller.name = 'Open-Loop'; + case 'dvf' + controller.type = 2; + controller.name = 'Decentralized Direct Velocity Feedback'; + case 'iff' + controller.type = 3; + controller.name = 'Decentralized Integral Force Feedback'; + case 'hac-dvf' + controller.type = 4; + controller.name = 'HAC-DVF'; + case 'ref-track-L' + controller.type = 5; + controller.name = 'Reference Tracking in the frame of the legs'; + case 'ref-track-iff-L' + controller.type = 6; + controller.name = 'Reference Tracking in the frame of the legs + IFF'; + case 'cascade-hac-lac' + controller.type = 7; + controller.name = 'Cascade Control + HAC-LAC'; + case 'hac-iff' + controller.type = 8; + controller.name = 'HAC-IFF'; + case 'stabilizing' + controller.type = 9; + controller.name = 'Stabilizing Controller'; + end + + if exist('./mat', 'dir') + save('mat/nano_hexapod_model_controller.mat', 'controller'); + elseif exist('./matlab', 'dir') + save('matlab/mat/nano_hexapod_model_controller.mat', 'controller'); + end + +end +#+end_src + +** =initializeSample=: Sample + +#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no +function [sample] = initializeSample(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none' + args.H (1,1) double {mustBeNumeric, mustBePositive} = 200e-3 % Height [m] + args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m] + args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg] + end + + sample = struct(); + + switch args.type + case 'none' + sample.type = 0; + sample.m = 0; + case 'cylindrical' + sample.type = 1; + + sample.H = args.H; + sample.R = args.R; + sample.m = args.m; + end + + if exist('./mat', 'dir') + if exist('./mat/nano_hexapod.mat', 'file') + save('mat/nano_hexapod.mat', 'sample', '-append'); + else + save('mat/nano_hexapod.mat', 'sample'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nano_hexapod.mat', 'file') + save('matlab/mat/nano_hexapod.mat', 'sample', '-append'); + else + save('matlab/mat/nano_hexapod.mat', 'sample'); + end + end + +end +#+end_src + +** Stewart platform +*** =initializeSimplifiedNanoHexapod=: Nano Hexapod + +#+begin_src matlab :tangle matlab/src/initializeSimplifiedNanoHexapod.m :comments none :mkdirp yes :eval no +function [nano_hexapod] = initializeSimplifiedNanoHexapod(args) + + arguments + %% initializeFramesPositions + args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m] + args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m] + %% generateGeneralConfiguration + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of fixed joints [m] + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m] + args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad] + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of mobile joints [m] + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m] + args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad] + %% Actuators + args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof' + args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 380000 + args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605 + args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302 + args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 5 + args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100 + args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50 + %% initializeCylindricalPlatforms + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg] + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m] + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m] + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg] + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m] + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m] + %% initializeCylindricalStruts + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg] + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m] + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m] + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg] + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m] + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m] + %% Bottom and Top Flexible Joints + args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' + args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' + args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + %% inverseKinematics + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + end + + 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, ... + 'type', args.actuator_type, ... + 'k', args.actuator_k, ... + 'ke', args.actuator_ke, ... + 'ka', args.actuator_ka, ... + 'c', args.actuator_c, ... + 'ce', args.actuator_ce, ... + 'ca', args.actuator_ca); + + stewart = initializeJointDynamics(stewart, ... + 'type_F', args.flex_type_F, ... + 'type_M', args.flex_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, ... + 'Ka_F', args.Ka_F, ... + 'Ca_F', args.Ca_F, ... + 'Kr_F', args.Kr_F, ... + 'Cr_F', args.Cr_F, ... + 'Ka_M', args.Ka_M, ... + 'Ca_M', args.Ca_M, ... + 'Kr_M', args.Kr_M, ... + 'Cr_M', args.Cr_M); + + 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); + + nano_hexapod = stewart; + if exist('./mat', 'dir') + if exist('./mat/nano_hexapod.mat', 'file') + save('mat/nano_hexapod.mat', 'nano_hexapod', '-append'); + else + save('mat/nano_hexapod.mat', 'nano_hexapod'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nano_hexapod.mat', 'file') + save('matlab/mat/nano_hexapod.mat', 'nano_hexapod', '-append'); + else + save('matlab/mat/nano_hexapod.mat', 'nano_hexapod'); + end + end +end +#+end_src + +*** =initializeStewartPlatform=: Initialize the Stewart Platform structure + +#+begin_src matlab :tangle matlab/src/initializeStewartPlatform.m :comments none :mkdirp yes :eval no +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(); + +end +#+end_src + +*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M} + +#+begin_src matlab :tangle matlab/src/initializeFramesPositions.m :comments none :mkdirp yes :eval no +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; + +end +#+end_src + +*** =generateGeneralConfiguration=: Generate a Very General Configuration + +#+begin_src matlab :tangle matlab/src/generateGeneralConfiguration.m :comments none :mkdirp yes :eval no +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; + +end +#+end_src + +*** =computeJointsPose=: Compute the Pose of the Joints + +#+begin_src matlab :tangle matlab/src/computeJointsPose.m :comments none :mkdirp yes :eval no +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; + +end +#+end_src + +*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms + +#+begin_src matlab :tangle matlab/src/initializeCylindricalPlatforms.m :comments none :mkdirp yes :eval no +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; + +end +#+end_src + +*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts + +#+begin_src matlab :tangle matlab/src/initializeCylindricalStruts.m :comments none :mkdirp yes :eval no +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 + + stewart.struts_M.type = 1; + + stewart.struts_M.M = args.Msm; + stewart.struts_M.R = args.Msr; + stewart.struts_M.H = args.Msh; + + stewart.struts_F.type = 1; + + stewart.struts_F.M = args.Fsm; + stewart.struts_F.R = args.Fsr; + stewart.struts_F.H = args.Fsh; + +end +#+end_src + +*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut + +#+begin_src matlab :tangle matlab/src/initializeStrutDynamics.m :comments none :mkdirp yes :eval no +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,{'1dof', '2dof', 'flexible'})} = '1dof' + args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6 + args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6 + args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6 + args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1 + args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6 + args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10 + + args.F_gain (1,1) double {mustBeNumeric} = 1 + args.me (1,1) double {mustBeNumeric} = 0.01 + args.ma (1,1) double {mustBeNumeric} = 0.01 + end + + if strcmp(args.type, '1dof') + stewart.actuators.type = 1; + elseif strcmp(args.type, '2dof') + stewart.actuators.type = 2; + elseif strcmp(args.type, 'flexible') + stewart.actuators.type = 3; + end + + stewart.actuators.k = args.k; + stewart.actuators.c = args.c; + + stewart.actuators.ka = args.ka; + stewart.actuators.ca = args.ca; + + stewart.actuators.ke = args.ke; + stewart.actuators.ce = args.ce; + + stewart.actuators.F_gain = args.F_gain; + + stewart.actuators.ma = args.ma; + stewart.actuators.me = args.me; + +end +#+end_src + +*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints + +#+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no +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,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' + args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' + args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + 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 '2dof' + stewart.joints_F.type = 1; + case '3dof' + stewart.joints_F.type = 2; + case '4dof' + stewart.joints_F.type = 3; + case '6dof' + stewart.joints_F.type = 4; + case 'flexible' + stewart.joints_F.type = 5; + otherwise + error("joints_F are not correctly defined") + end + + switch args.type_M + case '2dof' + stewart.joints_M.type = 1; + case '3dof' + stewart.joints_M.type = 2; + case '4dof' + stewart.joints_M.type = 3; + case '6dof' + stewart.joints_M.type = 4; + case 'flexible' + stewart.joints_M.type = 5; + otherwise + error("joints_M are not correctly defined") + 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; + +end +#+end_src + +*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose + +#+begin_src matlab :tangle matlab/src/initializeStewartPose.m :comments none :mkdirp yes :eval no +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; + +end +#+end_src + +*** =computeJacobian=: Compute the Jacobian Matrix + +#+begin_src matlab :tangle matlab/src/computeJacobian.m :comments none :mkdirp yes :eval no +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: +% - geometry.J [6x6] - The Jacobian Matrix +% - geometry.K [6x6] - The Stiffness Matrix +% - geometry.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.geometry.J = J; + stewart.geometry.K = K; + stewart.geometry.C = C; + +end +#+end_src + +*** =inverseKinematics=: Compute Inverse Kinematics + +#+begin_src matlab :tangle matlab/src/inverseKinematics.m :comments none :mkdirp yes :eval no +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; + +end +#+end_src + +*** =displayArchitecture=: 3D plot of the Stewart platform architecture +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/displayArchitecture.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/displayArchitecture.m][here]]. + +**** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +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: +#+end_src + +**** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +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 +#+end_src + +**** Check the =stewart= structure elements +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +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; +#+end_src + + +**** Figure Creation, Frames and Homogeneous transformations +:PROPERTIES: +:UNNUMBERED: t +:END: + +The reference frame of the 3d plot corresponds to the frame $\{F\}$. +#+begin_src matlab +if ~strcmp(args.views, 'all') + figure; +else + f = figure('visible', 'off'); +end + +hold on; +#+end_src + +We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$. +#+begin_src matlab +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; +#+end_src + +Let's define a parameter that define the length of the unit vectors used to display the frames. +#+begin_src matlab +d_unit_vector = H/4; +#+end_src + +Let's define a parameter used to position the labels with respect to the center of the element. +#+begin_src matlab +d_label = H/20; +#+end_src + +**** Fixed Base elements +:PROPERTIES: +:UNNUMBERED: t +:END: +Let's first plot the frame $\{F\}$. +#+begin_src matlab +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 +#+end_src + +Now plot the frame $\{A\}$ fixed to the Base. +#+begin_src matlab +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 +#+end_src + +Let's then plot the circle corresponding to the shape of the Fixed base. +#+begin_src matlab +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 +#+end_src + +Let's now plot the position and labels of the Fixed Joints +#+begin_src matlab +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 +#+end_src + +**** Mobile Platform elements +:PROPERTIES: +:UNNUMBERED: t +:END: + +Plot the frame $\{M\}$. +#+begin_src matlab +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 +#+end_src + +Plot the frame $\{B\}$. +#+begin_src matlab +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 +#+end_src + +Let's then plot the circle corresponding to the shape of the Mobile platform. +#+begin_src matlab +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 +#+end_src + +Plot the position and labels of the rotation joints fixed to the mobile platform. +#+begin_src matlab +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 +#+end_src + +**** Legs +:PROPERTIES: +:UNNUMBERED: t +:END: +Plot the legs connecting the joints of the fixed base to the joints of the mobile platform. +#+begin_src matlab +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 +#+end_src + +**** Figure parameters +#+begin_src matlab +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; +#+end_src + +**** Subplots +#+begin_src matlab +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 +#+end_src + + +*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/describeStewartPlatform.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/describeStewartPlatform.m][here]]. + +**** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +function [] = describeStewartPlatform(stewart) +% describeStewartPlatform - Display some text describing the current defined Stewart Platform +% +% Syntax: [] = describeStewartPlatform(args) +% +% Inputs: +% - stewart +% +% Outputs: +#+end_src + +**** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +arguments + stewart +end +#+end_src + +**** Geometry +#+begin_src matlab +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') +#+end_src + +**** Actuators +#+begin_src matlab +fprintf('ACTUATORS:\n') +if stewart.actuators.type == 1 + fprintf('- The actuators are modelled as 1DoF.\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 modelled as 2DoF (APA).\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)) +elseif stewart.actuators.type == 3 + fprintf('- The actuators are modelled with a flexible element (FEM).\n') +end +fprintf('\n') +#+end_src + +**** Joints +#+begin_src matlab +fprintf('JOINTS:\n') +#+end_src + +Type of the joints on the fixed base. +#+begin_src matlab +switch stewart.joints_F.type + case 1 + fprintf('- The joints on the fixed based are universal joints (2DoF)\n') + case 2 + fprintf('- The joints on the fixed based are spherical joints (3DoF)\n') +end +#+end_src + +Type of the joints on the mobile platform. +#+begin_src matlab +switch stewart.joints_M.type + case 1 + fprintf('- The joints on the mobile based are universal joints (2DoF)\n') + case 2 + fprintf('- The joints on the mobile based are spherical joints (3DoF)\n') +end +#+end_src + +Position of the fixed joints +#+begin_src matlab +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) +#+end_src + +Position of the mobile joints +#+begin_src matlab +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') +#+end_src + +**** Kinematics +#+begin_src matlab +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 +#+end_src + +* Footnotes + +[fn:1]Different architecture exists, typically referred as "6-SPS" (Spherical, Prismatic, Spherical) or "6-UPS" (Universal, Prismatic, Spherical)