Start to write Stewart section

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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