Add cubic configuration initialization

simscape-model.org

@@ -63,27 +63,50 @@ For Simscape, we need:
 - The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$
 
 
-------
+* Procedure
-
 The procedure is the following:
 1. Choose $H$
 2. Choose ${}^{F}\bm{O}_{A}$ and ${}^{M}\bm{O}_{B}$
 3. Choose $\bm{a}_{i}$ and $\bm{b}_{i}$, probably by specifying ${}^{F}\bm{a}_{i}$ and ${}^{M}\bm{b}_{i}$
 4. Choose $k_{i}$ and $c_{i}$
 
+* Matlab Code
+** Matlab Init                                             :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+  <<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+  <<matlab-init>>
+#+end_src
+
+** Simscape Model
+#+begin_src matlab
+  open('stewart_platform.slx')
+#+end_src
+
+** Define the Height of the Platform
 #+begin_src matlab
   %% 1. Height of the platform. Location of {F} and {M}
   H = 90e-3; % [m]
   FO_M = [0; 0; H];
+#+end_src
 
+** Define the location of {A} and {B}
+#+begin_src matlab
   %% 2. Location of {A} and {B}
   FO_A = [0; 0; 100e-3] + FO_M;% [m,m,m]
   MO_B = [0; 0; 100e-3];% [m,m,m]
+#+end_src
 
-  %% 3. Position and Orientation of ai and bi
+** Define the position of $a_{i}$ and $b_{i}$
+#+begin_src matlab
+  %% 3. Position of ai and bi
   Fa = zeros(3, 6); % Fa_i is the i'th vector of Fa
   Mb = zeros(3, 6); % Mb_i is the i'th vector of Mb
+#+end_src
 
+#+begin_src matlab
   Aa = Fa - repmat(FO_A, [1, 6]);
   Bb = Mb - repmat(MO_B, [1, 6]);
 
@@ -105,13 +128,16 @@ The procedure is the following:
     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
+#+end_src
 
+** Define the dynamical properties of each strut
+#+begin_src matlab
   %% 4. Stiffness and Damping of each strut
   Ki = 1e6*ones(6,1);
   Ci = 1e2*ones(6,1);
 #+end_src
 
-------
+** Old Introduction                                                 :ignore:
 
 First, geometrical parameters are defined:
 - ${}^A\bm{a}_i$ - Position of the joints fixed to the fixed base w.r.t $\{A\}$
@@ -144,6 +170,206 @@ Other Parameters are defined for the Simscape simulation:
 - Location of the point for the differential measurements
 - Location of the Jacobian point for velocity/displacement computation
 
+
+** Cubic Configuration
+To define the cubic configuration, we need to define 4 parameters:
+- The size of the cube
+- The location of the cube
+- The position of the plane joint the points $a_{i}$
+- The position of the plane joint the points $b_{i}$
+
+To do so, we specify the following parameters:
+- $H_{C}$ the height of the useful part of the cube
+- ${}^{F}O_{C}$ the position of the center of the cube with respect to $\{F\}$
+- ${}^{F}H_{A}$: the height of the plane joining the points $a_{i}$ with respect to the frame $\{F\}$
+- ${}^{M}H_{B}$: the height of the plane joining the points $b_{i}$ with respect to the frame $\{B\}$
+
+We define the parameters
+#+begin_src matlab
+  Hc = 60e-3; % [m]
+  FOc = 30e-3; % [m]
+  FHa = 15e-3; % [m]
+  MHb = 15e-3; % [m]
+#+end_src
+
+We define the useful points of the cube with respect to the Cube's center.
+${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
+#+begin_src matlab
+  sx = [ 2; -1; -1];
+  sy = [ 0;  1; -1];
+  sz = [ 1;  1;  1];
+
+  R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
+
+  L = Hc*sqrt(3);
+
+  Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*Hc];
+
+  CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
+  CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
+#+end_src
+
+We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).
+#+begin_src matlab
+  CSi = (CCm - CCf)./vecnorm(CCm - CCf);
+#+end_src
+
+We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
+#+begin_src matlab
+  Fa = zeros(3, 6); % Fa_i is the i'th vector of Fa
+  Mb = zeros(3, 6); % Mb_i is the i'th vector of Mb
+#+end_src
+
+#+begin_src matlab
+  Fa = CCf + (FHa./CSi(3,:)).*CSi + [0; 0; FOc];
+  Mb = CCf + ((H-MHb)./CSi(3,:)).*CSi + [0; 0; FOc-H];
+#+end_src
+
+* initializeCubicConfiguration
+  :PROPERTIES:
+  :HEADER-ARGS:matlab+: :exports code
+  :HEADER-ARGS:matlab+: :comments no
+  :HEADER-ARGS:matlab+: :eval no
+  :HEADER-ARGS:matlab+: :tangle src/initializeCubicConfiguration.m
+  :END:
+  <<sec:initializeCubicConfiguration>>
+
+** Function description
+#+begin_src matlab
+  function [stewart] = initializeCubicConfiguration(opts_param)
+#+end_src
+
+** Optional Parameters
+Default values for opts.
+#+begin_src matlab
+  opts = struct(...
+      'H_tot', 90,  ... % Total height of the Hexapod [mm]
+      'L',     110, ... % Size of the Cube [mm]
+      'H',     40,  ... % Height between base joints and platform joints [mm]
+      'H0',    75   ... % Height between the corner of the cube and the plane containing the base joints [mm]
+      );
+#+end_src
+
+Populate opts with input parameters
+#+begin_src matlab
+  if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+          opts.(opt{1}) = opts_param.(opt{1});
+      end
+  end
+#+end_src
+
+** Cube Creation
+#+begin_src matlab :results none
+  points = [0, 0, 0; ...
+            0, 0, 1; ...
+            0, 1, 0; ...
+            0, 1, 1; ...
+            1, 0, 0; ...
+            1, 0, 1; ...
+            1, 1, 0; ...
+            1, 1, 1];
+  points = opts.L*points;
+#+end_src
+
+We create the rotation matrix to rotate the cube
+#+begin_src matlab :results none
+  sx = cross([1, 1, 1], [1 0 0]);
+  sx = sx/norm(sx);
+
+  sy = -cross(sx, [1, 1, 1]);
+  sy = sy/norm(sy);
+
+  sz = [1, 1, 1];
+  sz = sz/norm(sz);
+
+  R = [sx', sy', sz']';
+#+end_src
+
+We use to rotation matrix to rotate the cube
+#+begin_src matlab :results none
+  cube = zeros(size(points));
+  for i = 1:size(points, 1)
+    cube(i, :) = R * points(i, :)';
+  end
+#+end_src
+
+** Vectors of each leg
+#+begin_src matlab :results none
+  leg_indices = [3, 4; ...
+                 2, 4; ...
+                 2, 6; ...
+                 5, 6; ...
+                 5, 7; ...
+                 3, 7];
+#+end_src
+
+Vectors are:
+#+begin_src matlab :results none
+  legs = zeros(6, 3);
+  legs_start = zeros(6, 3);
+
+  for i = 1:6
+    legs(i, :) = cube(leg_indices(i, 2), :) - cube(leg_indices(i, 1), :);
+    legs_start(i, :) = cube(leg_indices(i, 1), :);
+  end
+#+end_src
+
+** Verification of Height of the Stewart Platform
+If the Stewart platform is not contained in the cube, throw an error.
+
+#+begin_src matlab :results none
+  Hmax = cube(4, 3) - cube(2, 3);
+  if opts.H0 < cube(2, 3)
+    error(sprintf('H0 is not high enought. Minimum H0 = %.1f', cube(2, 3)));
+  else if opts.H0 + opts.H > cube(4, 3)
+    error(sprintf('H0+H is too high. Maximum H0+H = %.1f', cube(4, 3)));
+    error('H0+H is too high');
+  end
+#+end_src
+
+** Determinate the location of the joints
+We now determine the location of the joints on the fixed platform w.r.t the fixed frame $\{A\}$.
+$\{A\}$ is fixed to the bottom of the base.
+#+begin_src matlab :results none
+  Aa = zeros(6, 3);
+  for i = 1:6
+    t = (opts.H0-legs_start(i, 3))/(legs(i, 3));
+    Aa(i, :) = legs_start(i, :) + t*legs(i, :);
+  end
+#+end_src
+
+And the location of the joints on the mobile platform with respect to $\{A\}$.
+#+begin_src matlab :results none
+  Ab = zeros(6, 3);
+  for i = 1:6
+    t = (opts.H0+opts.H-legs_start(i, 3))/(legs(i, 3));
+    Ab(i, :) = legs_start(i, :) + t*legs(i, :);
+  end
+#+end_src
+
+And the location of the joints on the mobile platform with respect to $\{B\}$.
+#+begin_src matlab :results none
+  Bb = zeros(6, 3);
+  Bb = Ab - (opts.H0 + opts.H_tot/2 + opts.H/2)*[0, 0, 1];
+#+end_src
+
+#+begin_src matlab :results none
+  h = opts.H0 + opts.H/2 - opts.H_tot/2;
+  Aa = Aa - h*[0, 0, 1];
+  Ab = Ab - h*[0, 0, 1];
+#+end_src
+
+** Returns Stewart Structure
+#+begin_src matlab :results none
+  stewart = struct();
+  stewart.Aa = Aa;
+  stewart.Ab = Ab;
+  stewart.Bb = Bb;
+  stewart.H_tot = opts.H_tot;
+end
+#+end_src
+
 * initializeGeneralConfiguration
 :PROPERTIES:
 :HEADER-ARGS:matlab+: :exports code