Files

Thomas Dehaeze ca64e189b8 Update all the initialization of hexapod and simscape of hexapod

2019-03-22 10:36:32 +01:00

20 KiB

Raw Blame History

Stewart Platform with Simscape

Functions

  <<matlab-init>>
  addpath('src');
  addpath('library');

  open stewart

  hexapod = initializeHexapod();

org_babel_eoe

  initializeSample();

  G = identifyPlant();

Functions

getMaxPositions

  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

getMaxPureDisplacement

  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

getStiffnessMatrix

  function [K] = getStiffnessMatrix(k, J)
  % k - leg stiffness
  % J - Jacobian matrix
      K = k*(J'*J);
  end

identifyPlant

  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

initializeHexapod

Function description and arguments

The initializeHexapod function takes one structure that contains configurations for the hexapod and returns one structure representing the hexapod.

  function [stewart] = initializeHexapod(opts_param)

Default values for opts.

  opts = struct(...
      'height',  90,    ... % Height of the platform [mm]
      'density', 8000,  ... % Density of the material used for the hexapod [kg/m3]
      'k_ax',    1e8,   ... % Stiffness of each actuator [N/m]
      'c_ax',    100,   ... % Damping of each actuator [N/(m/s)]
      'stroke',  50e-6, ... % Maximum stroke of each actuator [m]
      '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

Initialization of the stewart structure

We initialize the Stewart structure

  stewart = struct();

And we defined its total height.

  stewart.H = opts.height; % [mm]

Bottom Plate

The bottom plate structure is initialized.

  BP = struct();

We defined its internal radius (if there is a hole in the bottom plate) and its outer radius.

  BP.Rint = 0;   % Internal Radius [mm]
  BP.Rext = 150; % External Radius [mm]

We define its thickness.

  BP.H = 10; % Thickness of the Bottom Plate [mm]

At which radius legs will be fixed and with that angle offset.

  BP.Rleg  = 100; % Radius where the legs articulations are positionned [mm]
  BP.alpha = 10;  % Angle Offset [deg]

We defined the density of the material of the bottom plate.

  BP.density = opts.density; % Density of the material [kg/m3]

And its color.

  BP.color = [0.7 0.7 0.7]; % Color [RGB]

Then the profile of the bottom plate is computed and will be used by Simscape

  BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm]

The structure is added to the stewart structure

  stewart.BP = BP;

Top Plate

The top plate structure is initialized.

  TP = struct();

We defined the internal and external radius of the top plate.

  TP.Rint = 0;   % [mm]
  TP.Rext = 100; % [mm]

The thickness of the top plate.

  TP.H = 10; % [mm]

At which radius and angle are fixed the legs.

  TP.Rleg   = 100; % Radius where the legs articulations are positionned [mm]
  TP.alpha  = 20; % Angle [deg]
  TP.dalpha = 0; % Angle Offset from 0 position [deg]

The density of its material.

  TP.density = opts.density; % Density of the material [kg/m3]

Its color.

  TP.color = [0.7 0.7 0.7]; % Color [RGB]

Then the shape of the top plate is computed

  TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H];

The structure is added to the stewart structure

  stewart.TP  = TP;

Legs

The leg structure is initialized.

  Leg = struct();

The maximum Stroke of each leg is defined.

  Leg.stroke = opts.stroke; % [m]

The stiffness and damping of each leg are defined

  Leg.k_ax = opts.k_ax; % Stiffness of each leg [N/m]
  Leg.c_ax = opts.c_ax; % Damping of each leg [N/(m/s)]

The radius of the legs are defined

  Leg.Rtop = 10; % Radius of the cylinder of the top part of the leg[mm]
  Leg.Rbot = 12; % Radius of the cylinder of the bottom part of the leg [mm]

The density of its material.

  Leg.density = opts.density; % Density of the material used for the legs [kg/m3]

Its color.

  Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB]

The radius of spheres representing the ball joints are defined.

  Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm]

The structure is added to the stewart structure

  stewart.Leg = Leg;

Ball Joints

SP is the structure representing the support for the ball joints at the extremity of each leg.

The SP structure is initialized.

  SP = struct();

We can define its rotational stiffness and damping. For now, we use perfect joints.

  SP.k = 0; % [N*m/deg]
  SP.c = 0; % [N*m/deg]

Its height is defined

  SP.H = 15; % [mm]

Its radius is based on the radius on the sphere at the end of the legs.

  SP.R = Leg.R; % [mm]

  SP.section = [0    SP.H-SP.R;
                0    0;
                SP.R 0;
                SP.R SP.H];

The density of its material is defined.

  SP.density = opts.density; % [kg/m^3]

Its color is defined.

  SP.color = [0.7 0.7 0.7]; % [RGB]

The structure is added to the Hexapod structure

  stewart.SP  = SP;

More parameters are initialized

  stewart = initializeParameters(stewart);

Save the Stewart Structure

  save('./mat/stewart.mat', 'stewart')

initializeParameters Function

  function [stewart] = initializeParameters(stewart)

Computation of the position of the connection points on the base and moving platform We first initialize pos_base corresponding to $[a_1, a_2, a_3, a_4, a_5, a_6]^T$ and pos_top corresponding to $[b_1, b_2, b_3, b_4, b_5, b_6]^T$.

  stewart.pos_base = zeros(6, 3);
  stewart.pos_top = zeros(6, 3);

We estimate the height between the ball joints of the bottom platform and of the top platform.

  height = stewart.H - stewart.BP.H - stewart.TP.H - 2*stewart.SP.H; % [mm]

      for i = 1:3
          % base points
          angle_m_b = 120*(i-1) - stewart.BP.alpha;
          angle_p_b = 120*(i-1) + stewart.BP.alpha;

          stewart.pos_base(2*i-1,:) = [stewart.BP.Rleg*cos(angle_m_b), stewart.BP.Rleg*sin(angle_m_b), 0.0];
          stewart.pos_base(2*i,:)   = [stewart.BP.Rleg*cos(angle_p_b), stewart.BP.Rleg*sin(angle_p_b), 0.0];

          % top points
          angle_m_t = 120*(i-1) - stewart.TP.alpha + stewart.TP.dalpha;
          angle_p_t = 120*(i-1) + stewart.TP.alpha + stewart.TP.dalpha;

          stewart.pos_top(2*i-1,:) = [stewart.TP.Rleg*cos(angle_m_t), stewart.TP.Rleg*sin(angle_m_t), height];
          stewart.pos_top(2*i,:) = [stewart.TP.Rleg*cos(angle_p_t), stewart.TP.Rleg*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

      stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.h+stewart.Leg.sphere.top+stewart.SP.h.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];

Compute Jacobian Matrix

      %         aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.h - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
      bb = stewart.pos_top_tranform - (stewart.TP.h + 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

initializeParameters Function - BIS

  function [stewart] = initializeParameters(stewart)

We first compute $[a_1, a_2, a_3, a_4, a_5, a_6]^T$ and $[b_1, b_2, b_3, b_4, b_5, b_6]^T$.

  stewart.Aa = zeros(6, 3); % [mm]
  stewart.Ab = zeros(6, 3); % [mm]

  for i = 1:3
      stewart.Aa(2*i-1,:) = [stewart.BP.Rleg*cos( pi/180*(120*(i-1) - stewart.BP.alpha) ), ...
                             stewart.BP.Rleg*sin( pi/180*(120*(i-1) - stewart.BP.alpha) ), ...
                             stewart.BP.H+stewart.SP.H];
      stewart.Aa(2*i,:)   = [stewart.BP.Rleg*cos( pi/180*(120*(i-1) + stewart.BP.alpha) ), ...
                             stewart.BP.Rleg*sin( pi/180*(120*(i-1) + stewart.BP.alpha) ), ...
                             stewart.BP.H+stewart.SP.H];

      stewart.Ab(2*i-1,:) = [stewart.TP.Rleg*cos( pi/180*(120*(i-1) + stewart.TP.dalpha - stewart.TP.alpha) ), ...
                             stewart.TP.Rleg*sin( pi/180*(120*(i-1) + stewart.TP.dalpha - stewart.TP.alpha) ), ...
                             stewart.H - stewart.TP.H - stewart.SP.H];
      stewart.Ab(2*i,:)   = [stewart.TP.Rleg*cos( pi/180*(120*(i-1) + stewart.TP.dalpha + stewart.TP.alpha) ), ...
                             stewart.TP.Rleg*sin( pi/180*(120*(i-1) + stewart.TP.dalpha + stewart.TP.alpha) ), ...
                             stewart.H - stewart.TP.H - stewart.SP.H];
  end

Now, we compute the leg vectors $\hat{s}_i$ and leg position $l_i$: \[ b_i - a_i = l_i \hat{s}_i \]

We initialize $l_i$ and $\hat{s}_i$

  leg_length = zeros(6, 1); % [mm]
  leg_vectors = zeros(6, 3);

We compute $b_i - a_i$, and then:

\begin{align*} l_i &= \left|b_i - a_i\right| \\ \hat{s}_i &= \frac{b_i - a_i}{l_i} \end{align*}

  legs = stewart.Ab - stewart.Aa;

  for i = 1:6
      leg_length(i) = norm(legs(i,:));
      leg_vectors(i,:) = legs(i,:) / leg_length(i);
  end

Then the shape of the bottom leg is estimated

  stewart.Leg.lenght = leg_length(1)/1.5;
  stewart.Leg.shape.bot = ...
      [0                0; ...
       stewart.Leg.Rbot 0; ...
       stewart.Leg.Rbot stewart.Leg.lenght; ...
       stewart.Leg.Rtop stewart.Leg.lenght; ...
       stewart.Leg.Rtop 0.2*stewart.Leg.lenght; ...
       0                0.2*stewart.Leg.lenght];

We compute rotation matrices to have the orientation of the legs. The rotation matrix transforms the $z$ axis to the axis of the leg. The other axis are not important here.

  stewart.Rm = struct('R', eye(3));

  for i = 1:6
    sx = cross(leg_vectors(i,:), [1 0 0]);
    sx = sx/norm(sx);

    sy = -cross(sx, leg_vectors(i,:));
    sy = sy/norm(sy);

    sz = leg_vectors(i,:);
    sz = sz/norm(sz);

    stewart.Rm(i).R = [sx', sy', sz'];
  end

Compute Jacobian Matrix

  J = zeros(6);

  for i = 1:6
    J(i, 1:3) = leg_vectors(i, :);
    J(i, 4:6) = cross(0.001*(stewart.Ab - stewart.H*[0,0,1]), leg_vectors(i, :));
  end

  stewart.J = J;

  stewart.K = stewart.Leg.k_ax*stewart.J'*stewart.J;

    end
  end

initializeSample

  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

20 KiB Raw Blame History

Stewart Platform with Simscape

Functions

getMaxPositions

getMaxPureDisplacement

getStiffnessMatrix

identifyPlant

initializeHexapod

Function description and arguments

Initialization of the stewart structure

Bottom Plate

Top Plate

Legs

Ball Joints

More parameters are initialized

Save the Stewart Structure

initializeParameters Function

initializeParameters Function - BIS

initializeSample

20 KiB

Raw Blame History