Add index.org file with matlab functions included

.gitignore

@@ -0,0 +1,2 @@
+Figures/
+slprj/

index.org

@@ -0,0 +1,362 @@
+#+TITLE: Stewart Platform with Simscape
+
+* Functions
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :exports code
+  :HEADER-ARGS:matlab+: :comments no
+  :HEADER-ARGS:matlab+: :mkdir yes
+  :HEADER-ARGS:matlab+: :eval no
+  :END:
+** getMaxPositions
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :tangle src/getMaxPositions.m
+  :END:
+#+begin_src matlab
+  function [X, Y, Z] = getMaxPositions(stewart)
+      Leg = stewart.Leg;
+      J = stewart.J;
+      theta = linspace(0, 2*pi, 100);
+      phi = linspace(-pi/2 , pi/2, 100);
+      dmax = zeros(length(theta), length(phi));
+
+      for i = 1:length(theta)
+          for j = 1:length(phi)
+              L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
+              dmax(i, j) = Leg.stroke/max(abs(L));
+          end
+      end
+
+      X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
+      Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
+      Z = dmax.*sin(repmat(phi,length(theta),1));
+  end
+#+end_src
+
+** getMaxPureDisplacement
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :tangle src/getMaxPureDisplacement.m
+  :END:
+#+begin_src matlab
+  function [max_disp] = getMaxPureDisplacement(Leg, J)
+      max_disp = zeros(6, 1);
+      max_disp(1) = Leg.stroke/max(abs(J*[1 0 0 0 0 0]'));
+      max_disp(2) = Leg.stroke/max(abs(J*[0 1 0 0 0 0]'));
+      max_disp(3) = Leg.stroke/max(abs(J*[0 0 1 0 0 0]'));
+      max_disp(4) = Leg.stroke/max(abs(J*[0 0 0 1 0 0]'));
+      max_disp(5) = Leg.stroke/max(abs(J*[0 0 0 0 1 0]'));
+      max_disp(6) = Leg.stroke/max(abs(J*[0 0 0 0 0 1]'));
+  end
+#+end_src
+
+** getStiffnessMatrix
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :tangle src/getStiffnessMatrix.m
+  :END:
+#+begin_src matlab
+  function [K] = getStiffnessMatrix(k, J)
+  % k - leg stiffness
+  % J - Jacobian matrix
+      K = k*(J'*J);
+  end
+#+end_src
+
+** identifyPlant
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :tangle src/identifyPlant.m
+  :END:
+#+begin_src matlab
+  function [sys] = identifyPlant(opts_param)
+  %% Default values for opts
+      opts = struct();
+
+      %% Populate opts with input parameters
+      if exist('opts_param','var')
+          for opt = fieldnames(opts_param)'
+              opts.(opt{1}) = opts_param.(opt{1});
+          end
+      end
+
+      %% Options for Linearized
+      options = linearizeOptions;
+      options.SampleTime = 0;
+
+      %% Name of the Simulink File
+      mdl = 'stewart_identification';
+
+      %% Input/Output definition
+      io(1) = linio([mdl, '/F'],  1, 'input'); % Cartesian forces
+      io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces
+      io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces
+      io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion
+
+      io(5) = linio([mdl, '/Dm'],  1, 'output'); % Relative Motion
+      io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg
+      io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg
+      io(8) = linio([mdl, '/Xm'],  1, 'output'); % Absolute motion of platform
+
+      %% Run the linearization
+      G = linearize(mdl, io, 0);
+
+      %% Input/Output names
+      G.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ...
+                      'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
+                      'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ...
+                      'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'};
+      G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ...
+                      'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ...
+                      'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ...
+                      'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'};
+
+      %% Cut into sub transfer functions
+      sys.G_cart = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}));
+      sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
+      sys.G_legs = G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'});
+      sys.G_tran = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}));
+      sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
+      sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
+      sys.G_all  = minreal(G);
+  end
+#+end_src
+
+** initializeHexapod
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :tangle src/initializeHexapod.m
+  :END:
+#+begin_src matlab
+  function [stewart] = initializeHexapod(opts_param)
+  %% Default values for opts
+      opts = struct(...
+          'height',   90,       ... % Height of the platform [mm]
+          'jacobian', 150,      ... % Jacobian offset [mm]
+          'density',  8000,     ... % Density of hexapod [mm]
+          'name',     'stewart' ... % Name of the file
+          );
+
+      %% Populate opts with input parameters
+      if exist('opts_param','var')
+          for opt = fieldnames(opts_param)'
+              opts.(opt{1}) = opts_param.(opt{1});
+          end
+      end
+
+      %% Stewart Object
+      stewart = struct();
+      stewart.h        = opts.height;   % Total height of the platform [mm]
+      stewart.jacobian = opts.jacobian; % distance from the center of the top platform
+                                        % where the jacobian is computed [mm]
+
+      %% Bottom Plate
+      BP = struct();
+
+      BP.rad.int   = 0;    % Internal Radius [mm]
+      BP.rad.ext   = 150;  % External Radius [mm]
+      BP.thickness = 10;   % Thickness [mm]
+      BP.leg.rad   = 100;  % Radius where the legs articulations are positionned [mm]
+      BP.leg.ang   = 5;    % Angle Offset [deg]
+      BP.density   = opts.density; % Density of the material [kg/m3]
+      BP.color     = [0.7 0.7 0.7]; % Color [rgb]
+      BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
+
+      %% Top Plate
+      TP = struct();
+
+      TP.rad.int   = 0;    % Internal Radius [mm]
+      TP.rad.ext   = 100;  % Internal Radius [mm]
+      TP.thickness = 10;   % Thickness [mm]
+      TP.leg.rad   = 90;   % Radius where the legs articulations are positionned [mm]
+      TP.leg.ang   = 5;    % Angle Offset [deg]
+      TP.density   = opts.density; % Density of the material [kg/m3]
+      TP.color     = [0.7 0.7 0.7]; % Color [rgb]
+      TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
+
+      %% Leg
+      Leg = struct();
+
+      Leg.stroke     = 80e-6; % Maximum Stroke of each leg [m]
+      if strcmp(opts.actuator, 'piezo')
+          Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
+          Leg.c.ax = 500; % [N/(m/s)]
+      elseif strcmp(opts.actuator, 'lorentz')
+          Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
+          Leg.c.ax = 200; % [N/(m/s)]
+      elseif isnumeric(opts.actuator)
+          Leg.k.ax = opts.actuator; % Stiffness of each leg [N/m]
+          Leg.c.ax = 100;           % [N/(m/s)]
+      else
+          error('opts.actuator should be piezo or lorentz or numeric value');
+      end
+      Leg.rad.bottom = 12;   % Radius of the cylinder of the bottom part [mm]
+      Leg.rad.top    = 10;   % Radius of the cylinder of the top part [mm]
+      Leg.density    = opts.density; % Density of the material [kg/m3]
+      Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
+      Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]
+
+      Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
+      Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
+
+      %% Sphere
+      SP = struct();
+
+      SP.height.bottom  = 15; % [mm]
+      SP.height.top     = 15; % [mm]
+      SP.density.bottom = opts.density; % [kg/m^3]
+      SP.density.top    = opts.density; % [kg/m^3]
+      SP.color.bottom   = [0.7 0.7 0.7]; % [rgb]
+      SP.color.top      = [0.7 0.7 0.7]; % [rgb]
+      SP.k.ax           = 0; % [N*m/deg]
+      SP.c.ax           = 0; % [N*m/deg]
+
+      SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
+      SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
+      SP.rad.bottom       = Leg.sphere.bottom; % [mm]
+      SP.rad.top          = Leg.sphere.top; % [mm]
+
+
+      %%
+      Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
+      Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
+
+      %%
+      stewart.BP = BP;
+      stewart.TP = TP;
+      stewart.Leg = Leg;
+      stewart.SP = SP;
+
+      %%
+      stewart = initializeParameters(stewart);
+
+      %%
+      save('./mat/stewart.mat', 'stewart')
+
+      %% ==============================================================
+      %  Additional Functions
+      % ===============================================================
+
+      %% Initialize Parameters
+      function [stewart] = initializeParameters(stewart)
+      %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
+          stewart.pos_base = zeros(6, 3);
+          stewart.pos_top = zeros(6, 3);
+
+          alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
+          alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
+
+          % Height [m] TODO
+          height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001;
+
+          radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
+          radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
+
+          for i = 1:3
+              % base points
+              angle_m_b = (2*pi/3)* (i-1) - alpha_b;
+              angle_p_b = (2*pi/3)* (i-1) + alpha_b;
+              stewart.pos_base(2*i-1,:) =  [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
+              stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
+
+              % top points
+              % Top points are 60 degrees offset
+              angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
+              angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
+              stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
+              stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
+          end
+
+          % permute pos_top points so that legs are end points of base and top points
+          stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
+          stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
+
+          %% leg vectors
+          legs = stewart.pos_top - stewart.pos_base;
+          leg_length = zeros(6, 1);
+          leg_vectors = zeros(6, 3);
+          for i = 1:6
+              leg_length(i) = norm(legs(i,:));
+              leg_vectors(i,:)  = legs(i,:) / leg_length(i);
+          end
+
+          stewart.Leg.lenght = 1000*leg_length(1)/1.5;
+          stewart.Leg.shape.bot = [0 0; ...
+                              stewart.Leg.rad.bottom 0; ...
+                              stewart.Leg.rad.bottom stewart.Leg.lenght; ...
+                              stewart.Leg.rad.top stewart.Leg.lenght; ...
+                              stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
+                              0 0.2*stewart.Leg.lenght];
+
+          %% Calculate revolute and cylindrical axes
+          rev1 = zeros(6, 3);
+          rev2 = zeros(6, 3);
+          cyl1 = zeros(6, 3);
+          for i = 1:6
+              rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
+              rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
+
+              rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
+              rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
+
+              cyl1(i,:) = leg_vectors(i,:);
+          end
+
+
+          %% Coordinate systems
+          stewart.lower_leg = struct('rotation', eye(3));
+          stewart.upper_leg = struct('rotation', eye(3));
+
+          for i = 1:6
+              stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+              stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+          end
+
+          %% Position Matrix
+          % TODO
+          stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
+
+          %% Compute Jacobian Matrix
+          % TODO
+          %         aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
+          bb = stewart.pos_top_tranform - (stewart.TP.thickness + stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
+          bb = bb - stewart.jacobian*1e-3*[zeros(6, 2),ones(6, 1)];
+          stewart.J = getJacobianMatrix(leg_vectors', bb');
+
+          stewart.K = stewart.Leg.k.ax*stewart.J'*stewart.J;
+      end
+
+      %% Compute the Jacobian Matrix
+      function J  = getJacobianMatrix(RM, M_pos_base)
+      % RM         - [3x6] unit vector of each leg in the fixed frame
+      % M_pos_base - [3x6] vector of the leg connection at the top platform location in the fixed frame
+          J = zeros(6);
+
+          J(:, 1:3) = RM';
+          J(:, 4:6) = cross(M_pos_base, RM)';
+      end
+  end
+#+end_src
+
+** initializeSample
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :tangle src/initializeSample.m
+  :END:
+#+begin_src matlab
+  function [] = initializeSample(opts_param)
+  %% Default values for opts
+      sample = struct( ...
+          'radius',     100, ... % radius of the cylinder [mm]
+          'height',     300, ... % height of the cylinder [mm]
+          'mass',       50,  ... % mass of the cylinder [kg]
+          'measheight', 150, ... % measurement point z-offset [mm]
+          'offset',     [0, 0, 0],   ... % offset position of the sample [mm]
+          'color',      [0.9 0.1 0.1] ...
+          );
+
+      %% Populate opts with input parameters
+      if exist('opts_param','var')
+          for opt = fieldnames(opts_param)'
+              sample.(opt{1}) = opts_param.(opt{1});
+          end
+      end
+
+      %% Save
+      save('./mat/sample.mat', 'sample');
+  end
+#+end_src